lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修复行驶状态bug

Browse Source
This commit is contained in:
lyq 2026-03-26 14:37:52 +08:00
parent 012f9a608a
commit b3b4cbd466
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/autonomy/fu/src/fu_node.cpp
View File

@ -669,6 +669,7 @@ class fu_node : public rclcpp::Node
is_start = msg->is_start; is_start = msg->is_start;
LOG_INFO("[规划层消息回调] 目标点位置： x -- %.2f , y -- %.2f", pl_x_, pl_y_); LOG_INFO("[规划层消息回调] 目标点位置： x -- %.2f , y -- %.2f", pl_x_, pl_y_);
LOG_INFO("[规划层消息回调] pl_speed_： %d ", pl_speed_);
} }
// ======== 绕障路点规划 ======== // ======== 绕障路点规划 ========
@ -763,7 +764,7 @@ class fu_node : public rclcpp::Node
auto now = system_clock::now(); auto now = system_clock::now();
duration<double> time_since_last = now - last_rtk_time; duration<double> time_since_last = now - last_rtk_time;
if (g_rtk.reliability != 0 && time_since_last <= rtk_timeout) if (g_rtk.reliability == 1 && time_since_last <= rtk_timeout)
{ {
// RTK信号良好：使用绝对坐标判断 // RTK信号良好：使用绝对坐标判断
double distance = ntzx_GPS_length(g_rtk.lon, g_rtk.lat, target.lon, target.lat); double distance = ntzx_GPS_length(g_rtk.lon, g_rtk.lat, target.lon, target.lat);
@ -934,7 +935,8 @@ class fu_node : public rclcpp::Node
// 检查RTK信号 // 检查RTK信号
auto rtk_now = system_clock::now(); auto rtk_now = system_clock::now();
duration<double> time_since_last_rtk = rtk_now - last_rtk_time; duration<double> time_since_last_rtk = rtk_now - last_rtk_time;
bool rtk_signal_good = (g_rtk.reliability != 0 && time_since_last_rtk <= rtk_timeout); bool rtk_signal_good = (g_rtk.reliability == 1 && time_since_last_rtk <= rtk_timeout);
// LOG_INFO("rtk_signal_good ? %d",rtk_signal_good);
// 状态3: 雷达数据超时，停车 // 状态3: 雷达数据超时，停车
if (grid_age > 3.0) if (grid_age > 3.0)
@ -1034,7 +1036,7 @@ class fu_node : public rclcpp::Node
// 若之前处于WAITING状态，且现在没有障碍物且RTK信号良好，恢复到NORMAL状态 // 若之前处于WAITING状态，且现在没有障碍物且RTK信号良好，恢复到NORMAL状态
auto now = system_clock::now(); auto now = system_clock::now();
duration<double> time_since_last_rtk = now - last_rtk_time; duration<double> time_since_last_rtk = now - last_rtk_time;
bool rtk_signal_good = (g_rtk.reliability != 0 && time_since_last_rtk <= rtk_timeout); bool rtk_signal_good = (g_rtk.reliability == 1 && time_since_last_rtk <= rtk_timeout);
if (state_machine_->getCurrentState() == StateMachine::WAITING && if (state_machine_->getCurrentState() == StateMachine::WAITING &&
!state_machine_->obstacle_analysis.has_front_critical && !state_machine_->obstacle_analysis.has_front_critical &&