fu_node 260306

src/autonomy/fu/src/fu_node.cpp

@@ -122,18 +122,6 @@ int ntzx_GPS_posit(double Cur_navAngle, double Cur_lonti, double Cur_lati, doubl
     return 0;
 }
 
-// ================== 状态接口定义 ==================
-class StateMachine;
-
-class AvoidanceState
-{
-   public:
-    virtual ~AvoidanceState() = default;
-    virtual void enter(StateMachine& sm) {}
-    virtual void execute(StateMachine& sm) = 0;
-    virtual void exit(StateMachine& sm) {}
-};
-
 // ================== 状态机核心类 ==================
 class StateMachine
 {
@@ -279,7 +267,7 @@ class fu_node : public rclcpp::Node
         pub_mc = this->create_publisher<sweeper_interfaces::msg::McCtrl>("auto_mc_ctrl", 10);
 
         // ======== 定时器初始化 ========
-        timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&fu_node::timer_callback, this));
+        timer_ = this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&fu_node::timer_callback, this));
 
         // ======== 初始化 ========
         avoiding_obstacle_ = false;
@@ -346,6 +334,7 @@ class fu_node : public rclcpp::Node
     int current_waypoint_idx_;
     std::vector<std::vector<int>> cached_grid_;
     bool grid_data_valid_ = false;
+    rclcpp::Time last_grid_time_;
 
     // ======== 发布者和订阅者 ========
     rclcpp::Publisher<sweeper_interfaces::msg::McCtrl>::SharedPtr pub_mc;
@@ -451,6 +440,8 @@ class fu_node : public rclcpp::Node
     // ======== 栅格回调 ========
     void gridCallback(const std_msgs::msg::Int32MultiArray::SharedPtr msg)
     {
+        last_grid_time_ = node_clock_->now();
+
         if (msg->layout.dim.size() != 2)
         {
             LOG_ERROR("栅格地图消息维度无效！");
@@ -474,6 +465,20 @@ class fu_node : public rclcpp::Node
         analyzeObstacles(grid);
         if (enable_visualize_grid_) visualizeGridInTerminal(grid);
         cacheGridData(grid);
+
+        // 立即执行紧急停车检查（高优先级）
+        updateObstacleAnalysis();
+        if (enable_obstacle_stop_ && is_start)
+        {
+            if (state_machine_->obstacle_analysis.has_front_critical)
+            {
+                state_machine_->publish_speed = 0;
+                state_machine_->publish_angle = 0.0f;
+                state_machine_->setState(StateMachine::WAITING);
+                publishControlCommand(true);  // 立即发布带刹车的停车指令
+                LOG_WARN("[紧急停车] 栅格回调中检测到前方障碍物，立即停车！");
+            }
+        }
     }
 
     // ======== 栅格缓存 ========
@@ -852,7 +857,16 @@ class fu_node : public rclcpp::Node
         if (state_machine_->obstacle_analysis.has_front_critical)
         {
             state_machine_->publish_speed = 0;
+            state_machine_->publish_angle = 0.0f;
+            state_machine_->setState(StateMachine::WAITING);
             LOG_WARN("[绕障安全] 路点跟踪中检测到前方必停区障碍物，立即停车");
+
+            // 立即发布停车指令（带刹车）
+            publishControlCommand(true);
+            avoiding_obstacle_ = false;
+            current_waypoint_idx_ = 0;
+            avoidance_waypoints_.clear();
+            return;
         }
     }
 
@@ -913,6 +927,18 @@ class fu_node : public rclcpp::Node
     {
         if (!enable_obstacle_stop_) return false;
 
+        // 检查栅格数据新鲜度（超过500ms认为数据过期）
+        auto now = node_clock_->now();
+        auto grid_age = (now - last_grid_time_).seconds();
+        if (grid_age > 0.5)
+        {
+            LOG_WARN("[紧急停车] 栅格数据过期(%.3fs)，执行安全停车！", grid_age);
+            state_machine_->publish_speed = 0;
+            state_machine_->publish_angle = 0.0f;
+            state_machine_->setState(StateMachine::WAITING);
+            return true;
+        }
+
         if (state_machine_->obstacle_analysis.has_front_critical)
         {
             state_machine_->publish_speed = 0;
@@ -933,6 +959,13 @@ class fu_node : public rclcpp::Node
         // 开始清扫任务
         if (is_start)
         {
+            // 紧急停车优先级最高
+            if (executeEmergencyStop())
+            {
+                publishControlCommand(true);  // 紧急停车使用刹车
+                return;
+            }
+
             // PL速度为0，停车
             if (pl_speed_ == 0)
             {
@@ -943,13 +976,6 @@ class fu_node : public rclcpp::Node
                 return;
             }
 
-            // 紧急停车优先级最高
-            if (executeEmergencyStop())
-            {
-                publishControlCommand();
-                return;
-            }
-
             // 若之前处于WAITING状态，且现在没有障碍物且RTK信号良好，恢复到NORMAL状态
             auto now = system_clock::now();
             duration<double> time_since_last_rtk = now - last_rtk_time;
@@ -998,11 +1024,11 @@ class fu_node : public rclcpp::Node
     }
 
     // ======== 发布控制指令 ========
-    void publishControlCommand()
+    void publishControlCommand(bool emergency_brake = false)
     {
         auto message = sweeper_interfaces::msg::McCtrl();
         message.sweep = true;
-        message.brake = 0;
+        message.brake = emergency_brake ? 1 : 0;  // 紧急停车时启用刹车
         message.gear = 2;
         message.angle_speed = 800;
 
@@ -1012,9 +1038,17 @@ class fu_node : public rclcpp::Node
 
         pub_mc->publish(message);
 
+        if (emergency_brake)
+        {
+            LOG_WARN("[控制发布] 紧急刹车！状态：%s | 转速：%d | 转向角度：%.1f° | 刹车：%d",
+                     state_machine_->getStateString().c_str(), message.rpm, message.angle, message.brake);
+        }
+        else
+        {
             LOG_INFO("[控制发布] 状态：%s | 转速：%d | 转向角度：%.1f°", state_machine_->getStateString().c_str(),
                      message.rpm, message.angle);
         }
+    }
 };
 
 // ======== 主函数 ========