遇障停车bug

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_)
             {