From b3b4cbd4661e02ae4822d3e084a08769150aca35 Mon Sep 17 00:00:00 2001 From: lyq Date: Thu, 26 Mar 2026 14:37:52 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E8=A1=8C=E9=A9=B6=E7=8A=B6?= =?UTF-8?q?=E6=80=81bug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/autonomy/fu/src/fu_node.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) 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 &&