diff --git a/src/autonomy/fu/src/fu_node.cpp b/src/autonomy/fu/src/fu_node.cpp index 524e016..535af79 100644 --- a/src/autonomy/fu/src/fu_node.cpp +++ b/src/autonomy/fu/src/fu_node.cpp @@ -265,6 +265,7 @@ class fu_node : public rclcpp::Node // ======== 发布者初始化 ======== pub_mc = this->create_publisher("auto_mc_ctrl", 10); + pub_fu = this->create_publisher("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::SharedPtr pub_mc; + rclcpp::Publisher::SharedPtr pub_fu; rclcpp::Subscription::SharedPtr subscription_grid; rclcpp::Subscription::SharedPtr msg_subscribe_pl; rclcpp::Subscription::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 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", - state_machine_->getStateString().c_str(), message.rpm, message.angle, message.brake); + LOG_WARN("[控制发布] 紧急刹车！状态：%s | 转速：%d | 转向角度：%.1f° | 刹车：%d | 行驶状态：%d", + 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(), - message.rpm, message.angle); + LOG_INFO("[控制发布] 状态：%s | 转速：%d | 转向角度：%.1f° | 行驶状态：%d", + state_machine_->getStateString().c_str(), message.rpm, message.angle, fu_msg.driving_status); } } }; diff --git a/src/base/sweeper_interfaces/msg/Fu.msg b/src/base/sweeper_interfaces/msg/Fu.msg index 4fdd6a1..3585ba1 100644 --- a/src/base/sweeper_interfaces/msg/Fu.msg +++ b/src/base/sweeper_interfaces/msg/Fu.msg @@ -1,4 +1,5 @@ float32 angle int16 speed int16 action -int16 drive_mode \ No newline at end of file +int16 drive_mode +int16 driving_status \ No newline at end of file diff --git a/src/communication/mqtt_report/include/mqtt_report/report_struct.h b/src/communication/mqtt_report/include/mqtt_report/report_struct.h index 163277b..3a84268 100644 --- a/src/communication/mqtt_report/include/mqtt_report/report_struct.h +++ b/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 diff --git a/src/communication/mqtt_report/src/mqtt/mqtt_reporter.cpp b/src/communication/mqtt_report/src/mqtt/mqtt_reporter.cpp index d68b0db..efe947b 100644 --- a/src/communication/mqtt_report/src/mqtt/mqtt_reporter.cpp +++ b/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); } diff --git a/src/communication/mqtt_report/src/node/mqtt_vehicle_node.cpp b/src/communication/mqtt_report/src/node/mqtt_vehicle_node.cpp index 53f455d..6c30bb3 100644 --- a/src/communication/mqtt_report/src/node/mqtt_vehicle_node.cpp +++ b/src/communication/mqtt_report/src/node/mqtt_vehicle_node.cpp @@ -46,6 +46,10 @@ class VehicleReportNode : public rclcpp::Node rtk_sub_ = this->create_subscription( "rtk_message", 10, std::bind(&VehicleReportNode::rtk_callback, this, std::placeholders::_1)); + // Fu (行驶状态) + fu_sub_ = this->create_subscription( + "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 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::SharedPtr subscription_; rclcpp::Subscription::SharedPtr identity_sub_; rclcpp::Subscription::SharedPtr rtk_sub_; + rclcpp::Subscription::SharedPtr fu_sub_; rclcpp::TimerBase::SharedPtr info_timer_; rclcpp::TimerBase::SharedPtr gps_timer_;