diff --git a/doc/清扫车协议-260410.pdf b/doc/清扫车协议-260410.pdf new file mode 100644 index 0000000..04668b6 Binary files /dev/null and b/doc/清扫车协议-260410.pdf differ 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 3a84268..52365a8 100644 --- a/src/communication/mqtt_report/include/mqtt_report/report_struct.h +++ b/src/communication/mqtt_report/include/mqtt_report/report_struct.h @@ -20,6 +20,7 @@ struct GeneralMsg int controllerTemp = 0; // 控制器温度 °C int64_t timestamp = 0; // 时间戳 int drivingStatus = 0; // 行驶状态：0=正常行驶（默认），1=遇障停车，2=绕障中，3=雷达数据超时停车，4=RTK没信号停车，5=到达终点停车 + int driveMode = 0; // 驾驶模式：0=本地模式，1=自驾模式，2=遥控模式，3=远控模式 }; 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 bf4040d..06218fe 100644 --- a/src/communication/mqtt_report/src/mqtt/mqtt_reporter.cpp +++ b/src/communication/mqtt_report/src/mqtt/mqtt_reporter.cpp @@ -47,6 +47,7 @@ void MqttReporter::publish_info() j["controllerTemp"] = snapshot.controllerTemp; j["timestamp"] = snapshot.timestamp; j["drivingStatus"] = snapshot.drivingStatus; + j["driveMode"] = snapshot.driveMode; 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 fab9d37..89ca9b2 100644 --- a/src/communication/mqtt_report/src/node/mqtt_vehicle_node.cpp +++ b/src/communication/mqtt_report/src/node/mqtt_vehicle_node.cpp @@ -16,6 +16,7 @@ #include "sweeper_interfaces/msg/fu.hpp" #include "sweeper_interfaces/msg/rtk.hpp" #include "sweeper_interfaces/msg/vehicle_identity.hpp" +#include "std_msgs/msg/int32.hpp" namespace sweeperMsg = sweeper_interfaces::msg; using sweeper_interfaces::msg::VehicleIdentity; @@ -51,6 +52,10 @@ class VehicleReportNode : public rclcpp::Node fu_sub_ = this->create_subscription( "fu_message", 10, std::bind(&VehicleReportNode::fu_callback, this, std::placeholders::_1)); + // Drive mode + drive_mode_sub_ = this->create_subscription( + "/drive_mode", 10, std::bind(&VehicleReportNode::drive_mode_callback, this, std::placeholders::_1)); + // Timers info_timer_ = this->create_wall_timer(std::chrono::milliseconds(500), [this]() { reporter_.publish_info(); }); @@ -114,6 +119,13 @@ class VehicleReportNode : public rclcpp::Node ctx_.info.drivingStatus = msg->driving_status; } + // ========== Drive mode (驾驶模式) ========== + void drive_mode_callback(const std_msgs::msg::Int32::SharedPtr msg) + { + std::lock_guard lock(ctx_.mtx); + ctx_.info.driveMode = msg->data; + } + // ========== RTK ========== void rtk_callback(const sweeper_interfaces::msg::Rtk::SharedPtr msg) { @@ -155,6 +167,7 @@ class VehicleReportNode : public rclcpp::Node rclcpp::Subscription::SharedPtr identity_sub_; rclcpp::Subscription::SharedPtr rtk_sub_; rclcpp::Subscription::SharedPtr fu_sub_; + rclcpp::Subscription::SharedPtr drive_mode_sub_; rclcpp::TimerBase::SharedPtr info_timer_; rclcpp::TimerBase::SharedPtr gps_timer_; diff --git a/src/control/ctrl_arbiter/src/ctrl_arbiter.cpp b/src/control/ctrl_arbiter/src/ctrl_arbiter.cpp index 21e7c8e..3ecd1a0 100644 --- a/src/control/ctrl_arbiter/src/ctrl_arbiter.cpp +++ b/src/control/ctrl_arbiter/src/ctrl_arbiter.cpp @@ -4,6 +4,7 @@ #include "logger/logger.h" #include "rclcpp/rclcpp.hpp" #include "sweeper_interfaces/msg/mc_ctrl.hpp" +#include "std_msgs/msg/int32.hpp" using namespace std::chrono_literals; namespace sweeperMsg = sweeper_interfaces::msg; @@ -16,6 +17,7 @@ class ArbitrationNode : public rclcpp::Node timeout_ms_ = 200; // 200毫秒超时阈值，可调整 publisher_ = this->create_publisher("/mc_ctrl", 10); + mode_publisher_ = this->create_publisher("/drive_mode", 10); sub_radio_ = this->create_subscription("/radio_mc_ctrl", 10, [this](const sweeperMsg::McCtrl::SharedPtr msg) @@ -53,11 +55,14 @@ class ArbitrationNode : public rclcpp::Node { std::lock_guard lock(mutex_); rclcpp::Time now = this->now(); + std_msgs::msg::Int32 mode_msg; // 检查每个控制源是否在线 if (radio_valid_ && (now - radio_last_time_).nanoseconds() < timeout_ms_ * 1000000) { publisher_->publish(radio_msg_); + mode_msg.data = 2; // 遥控模式 + mode_publisher_->publish(mode_msg); LOG_INFO_THROTTLE(2000, "[ARBITER] Using RADIO control"); return; } @@ -65,6 +70,8 @@ class ArbitrationNode : public rclcpp::Node if (remote_valid_ && (now - remote_last_time_).nanoseconds() < timeout_ms_ * 1000000) { publisher_->publish(remote_msg_); + mode_msg.data = 3; // 远控模式 + mode_publisher_->publish(mode_msg); LOG_INFO_THROTTLE(2000, "[ARBITER] Using REMOTE control"); return; } @@ -72,6 +79,8 @@ class ArbitrationNode : public rclcpp::Node if (auto_valid_ && (now - auto_last_time_).nanoseconds() < timeout_ms_ * 1000000) { publisher_->publish(auto_msg_); + mode_msg.data = 1; // 自驾模式 + mode_publisher_->publish(mode_msg); LOG_INFO_THROTTLE(2000, "[ARBITER] Using AUTO control"); return; } @@ -95,10 +104,13 @@ class ArbitrationNode : public rclcpp::Node safe_msg.enable_water_pump = false; publisher_->publish(safe_msg); + mode_msg.data = 0; // 本地模式（默认） + mode_publisher_->publish(mode_msg); LOG_WARN_THROTTLE(1000, "[ARBITER] All sources timeout, publishing FAILSAFE control"); } rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr mode_publisher_; rclcpp::Subscription::SharedPtr sub_radio_, sub_remote_, sub_auto_; rclcpp::TimerBase::SharedPtr timer_;