新增行驶状态(drivingStatus)

src/autonomy/fu/src/fu_node.cpp

@@ -265,6 +265,7 @@ class fu_node : public rclcpp::Node
 
         // ======== 发布者初始化 ========
         pub_mc = this->create_publisher<sweeper_interfaces::msg::McCtrl>("auto_mc_ctrl", 10);
+        pub_fu = this->create_publisher<sweeper_interfaces::msg::Fu>("fu_message", 10);
 
         // ======== 定时器初始化 ========
         timer_ = this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&fu_node::timer_callback, this));
@@ -338,6 +339,7 @@ class fu_node : public rclcpp::Node
 
     // ======== 发布者和订阅者 ========
     rclcpp::Publisher<sweeper_interfaces::msg::McCtrl>::SharedPtr pub_mc;
+    rclcpp::Publisher<sweeper_interfaces::msg::Fu>::SharedPtr pub_fu;
     rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr subscription_grid;
     rclcpp::Subscription<sweeper_interfaces::msg::Pl>::SharedPtr msg_subscribe_pl;
     rclcpp::Subscription<sweeper_interfaces::msg::Rtk>::SharedPtr msg_subscribe_rtk;
@@ -922,6 +924,53 @@ class fu_node : public rclcpp::Node
                  state_machine_->obstacle_analysis.left_edge, state_machine_->obstacle_analysis.right_edge);
     }
 
+    // ======== 计算行驶状态 ========
+    int16_t calculateDrivingStatus() const
+    {
+        // 检查栅格数据新鲜度（超过3秒认为数据过期）
+        auto now = node_clock_->now();
+        auto grid_age = (now - last_grid_time_).seconds();
+
+        // 检查RTK信号
+        auto rtk_now = system_clock::now();
+        duration<double> time_since_last_rtk = rtk_now - last_rtk_time;
+        bool rtk_signal_good = (g_rtk.reliability != 0 && time_since_last_rtk <= rtk_timeout);
+
+        // 状态3: 雷达数据超时，停车
+        if (grid_age > 3.0)
+        {
+            return 3;
+        }
+
+        // 状态4: RTK没信号，停车
+        if (!rtk_signal_good)
+        {
+            return 4;
+        }
+
+        // 状态5: 到达终点，停车 (PL速度为0)
+        if (pl_speed_ == 0 && is_start)
+        {
+            return 5;
+        }
+
+        // 状态2: 绕障中 (LATERAL_SHIFT 或 PARALLEL_MOVE)
+        StateMachine::State current_state = state_machine_->getCurrentState();
+        if (current_state == StateMachine::LATERAL_SHIFT || current_state == StateMachine::PARALLEL_MOVE)
+        {
+            return 2;
+        }
+
+        // 状态1: 遇障停车，停车 (WAITING状态且有障碍物)
+        if (current_state == StateMachine::WAITING && state_machine_->obstacle_analysis.has_front_critical)
+        {
+            return 1;
+        }
+
+        // 状态0: 默认值，正常行驶
+        return 0;
+    }
+
     // ======== 紧急停车执行 ========
     bool executeEmergencyStop()
     {
@@ -1044,15 +1093,25 @@ class fu_node : public rclcpp::Node
 
         pub_mc->publish(message);
 
+        // 发布 Fu 消息，包含行驶状态
+        auto fu_msg = sweeper_interfaces::msg::Fu();
+        fu_msg.angle = state_machine_->publish_angle;
+        fu_msg.speed = state_machine_->publish_speed;
+        fu_msg.action = 0;
+        fu_msg.drive_mode = 0;
+        fu_msg.driving_status = calculateDrivingStatus();
+        pub_fu->publish(fu_msg);
+
         if (emergency_brake)
         {
-            LOG_WARN("[控制发布] 紧急刹车！状态：%s | 转速：%d | 转向角度：%.1f° | 刹车：%d",
+            LOG_WARN("[控制发布] 紧急刹车！状态：%s | 转速：%d | 转向角度：%.1f° | 刹车：%d | 行驶状态：%d",
-                     state_machine_->getStateString().c_str(), message.rpm, message.angle, message.brake);
+                     state_machine_->getStateString().c_str(), message.rpm, message.angle, message.brake,
+                     fu_msg.driving_status);
         }
         else
         {
-            LOG_INFO("[控制发布] 状态：%s | 转速：%d | 转向角度：%.1f°", state_machine_->getStateString().c_str(),
+            LOG_INFO("[控制发布] 状态：%s | 转速：%d | 转向角度：%.1f° | 行驶状态：%d",
-                     message.rpm, message.angle);
+                     state_machine_->getStateString().c_str(), message.rpm, message.angle, fu_msg.driving_status);
         }
     }
 };

src/base/sweeper_interfaces/msg/Fu.msg

@@ -1,4 +1,5 @@
 float32 angle
 int16 speed
 int16 action
-int16 drive_mode
+int16 drive_mode
+int16 driving_status

src/communication/mqtt_report/include/mqtt_report/report_struct.h

@@ -19,6 +19,7 @@ struct GeneralMsg
     int motorTemp = 0;           // 电机温度 °C
     int controllerTemp = 0;      // 控制器温度 °C
     int64_t timestamp = 0;       // 时间戳
+    int drivingStatus = 0;       // 行驶状态：0=正常行驶（默认），1=遇障停车，2=绕障中，3=雷达数据超时停车，4=RTK没信号停车，5=到达终点停车
 };
 
 struct BMSFault

src/communication/mqtt_report/src/mqtt/mqtt_reporter.cpp

@@ -46,6 +46,7 @@ void MqttReporter::publish_info()
     j["motorTemp"] = snapshot.motorTemp;
     j["controllerTemp"] = snapshot.controllerTemp;
     j["timestamp"] = snapshot.timestamp;
+    j["drivingStatus"] = snapshot.drivingStatus;
 
     mqtt_.publish(make_topic(config_.info_topic_template, vid), j.dump(), 1);
 }

src/communication/mqtt_report/src/node/mqtt_vehicle_node.cpp

@@ -46,6 +46,10 @@ class VehicleReportNode : public rclcpp::Node
         rtk_sub_ = this->create_subscription<sweeper_interfaces::msg::Rtk>(
             "rtk_message", 10, std::bind(&VehicleReportNode::rtk_callback, this, std::placeholders::_1));
 
+        // Fu (行驶状态)
+        fu_sub_ = this->create_subscription<sweeper_interfaces::msg::Fu>(
+            "fu_message", 10, std::bind(&VehicleReportNode::fu_callback, this, std::placeholders::_1));
+
         // Timers
         info_timer_ = this->create_wall_timer(std::chrono::milliseconds(500), [this]() { reporter_.publish_info(); });
 
@@ -102,6 +106,13 @@ class VehicleReportNode : public rclcpp::Node
         health_.on_can_rx(now);
     }
 
+    // ========== Fu (行驶状态) ==========
+    void fu_callback(const sweeper_interfaces::msg::Fu::SharedPtr msg)
+    {
+        std::lock_guard<std::mutex> lock(ctx_.mtx);
+        ctx_.info.drivingStatus = msg->driving_status;
+    }
+
     // ========== RTK ==========
     void rtk_callback(const sweeper_interfaces::msg::Rtk::SharedPtr msg)
     {
@@ -142,6 +153,7 @@ class VehicleReportNode : public rclcpp::Node
     rclcpp::Subscription<sweeperMsg::CanFrame>::SharedPtr subscription_;
     rclcpp::Subscription<VehicleIdentity>::SharedPtr identity_sub_;
     rclcpp::Subscription<sweeper_interfaces::msg::Rtk>::SharedPtr rtk_sub_;
+    rclcpp::Subscription<sweeper_interfaces::msg::Fu>::SharedPtr fu_sub_;
 
     rclcpp::TimerBase::SharedPtr info_timer_;
     rclcpp::TimerBase::SharedPtr gps_timer_;