diff --git a/src/autonomy/fu/src/fu_node.cpp b/src/autonomy/fu/src/fu_node.cpp index 535af79..2baf635 100644 --- a/src/autonomy/fu/src/fu_node.cpp +++ b/src/autonomy/fu/src/fu_node.cpp @@ -669,6 +669,7 @@ class fu_node : public rclcpp::Node is_start = msg->is_start; 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(); duration 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信号良好:使用绝对坐标判断 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信号 auto rtk_now = system_clock::now(); duration 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: 雷达数据超时,停车 if (grid_age > 3.0) @@ -1034,7 +1036,7 @@ class fu_node : public rclcpp::Node // 若之前处于WAITING状态,且现在没有障碍物且RTK信号良好,恢复到NORMAL状态 auto now = system_clock::now(); duration 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 && !state_machine_->obstacle_analysis.has_front_critical &&