From 3cdea52783d0ef1dd49ee259c735fe43f14a0162 Mon Sep 17 00:00:00 2001
From: lyq <zxwl@44.com>
Date: Mon, 2 Feb 2026 17:32:55 +0800
Subject: [PATCH] =?UTF-8?q?=E9=81=87=E9=9A=9C=E5=81=9C=E8=BD=A6bug?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/autonomy/fu/src/fu_node.cpp | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/src/autonomy/fu/src/fu_node.cpp b/src/autonomy/fu/src/fu_node.cpp
index f5978cb..fba0bc0 100644
--- a/src/autonomy/fu/src/fu_node.cpp
+++ b/src/autonomy/fu/src/fu_node.cpp
@@ -950,6 +950,19 @@ class fu_node : public rclcpp::Node
                 return;
             }
 
+            // 若之前处于WAITING状态，且现在没有障碍物且RTK信号良好，恢复到NORMAL状态
+            auto now = system_clock::now();
+            duration<double> time_since_last_rtk = now - last_rtk_time;
+            bool rtk_signal_good = (g_rtk.reliability != 0 && time_since_last_rtk <= rtk_timeout);
+
+            if (state_machine_->getCurrentState() == StateMachine::WAITING &&
+                !state_machine_->obstacle_analysis.has_front_critical &&
+                rtk_signal_good)
+            {
+                state_machine_->setState(StateMachine::NORMAL);
+                LOG_INFO("[状态恢复] 障碍物已消失且RTK信号良好，恢复正常行驶状态");
+            }
+
             // 绕障逻辑
             if (enable_obstacle_avoid_)
             {