Merge branch 'humble' of http://36.153.162.171:3000/lyq/sweeper_200 into humble

合并

src/autonomy/fu/src/fu_node.cpp

@@ -19,6 +19,7 @@
 #include "sweeper_interfaces/msg/pl.hpp"
 #include "sweeper_interfaces/msg/rtk.hpp"
 #include "sweeper_interfaces/msg/sub.hpp"
+#include "sweeper_interfaces/msg/task.hpp"
 
 using namespace std;
 using namespace std::chrono;
@@ -50,6 +51,9 @@ const int OBSTACLE = 1;
 const int VEHICLE = 2;
 int is_start = 0;
 
+// 当前任务信息
+int current_task_mode = 0;  // 清扫模式: 0=标准模式(刷子), 1=混合模式(刷子+洒水)
+
 // ================== 路点结构体 ==================
 struct Waypoint
 {
@@ -218,6 +222,10 @@ class fu_node : public rclcpp::Node
         // 初始化最后接收时间为启动时间（避免初始状态直接超时）
         last_rtk_time = system_clock::now();
 
+        // 订阅任务消息以获取清扫模式等信息
+        task_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Task>(
+            "task_message/download", 10, std::bind(&fu_node::task_callback, this, std::placeholders::_1));
+
         // ======== 参数声明 ========
         this->declare_parameter("enable_obstacle_stop", true);
         this->declare_parameter("enable_obstacle_avoid", true);
@@ -262,6 +270,8 @@ class fu_node : public rclcpp::Node
             "pl_message", 10, std::bind(&fu_node::msg_callback_pl, this, std::placeholders::_1));
         msg_subscribe_rtk = this->create_subscription<sweeper_interfaces::msg::Rtk>(
             "rtk_message", 10, std::bind(&fu_node::msg_callback_rtk, this, std::placeholders::_1));
+        task_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Task>(
+            "task_message/download", 10, std::bind(&fu_node::task_callback, this, std::placeholders::_1));
 
         // ======== 发布者初始化 ========
         pub_mc = this->create_publisher<sweeper_interfaces::msg::McCtrl>("auto_mc_ctrl", 10);
@@ -337,12 +347,22 @@ class fu_node : public rclcpp::Node
     bool grid_data_valid_ = false;
     rclcpp::Time last_grid_time_;
 
+    // ======== 任务消息回调 ========
+    void task_callback(const sweeper_interfaces::msg::Task::SharedPtr msg)
+    {
+        if (msg->task_id > 0) {
+            current_task_mode = msg->mode;
+            LOG_INFO("任务模式更新为: %d (0=标准模式, 1=混合模式)", current_task_mode);
+        }
+    }
+
     // ======== 发布者和订阅者 ========
     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;
+    rclcpp::Subscription<sweeper_interfaces::msg::Task>::SharedPtr task_subscribe_;
     rclcpp::TimerBase::SharedPtr timer_;
 
     // ======== 参数回调 ========
@@ -1085,6 +1105,7 @@ class fu_node : public rclcpp::Node
     {
         auto message = sweeper_interfaces::msg::McCtrl();
         message.sweep = true;
+        message.sweep_mode = current_task_mode;
         message.brake = emergency_brake ? 1 : 0;  // 紧急停车时启用刹车
         message.gear = 2;
         message.angle_speed = 800;

src/autonomy/pl/src/pl_node.cpp

@@ -22,6 +22,15 @@ using namespace std::chrono;  // 新增：时间戳命名空间
 pthread_t pl_thread_t;
 int is_start = 0;
 
+// 保存完整的任务信息
+int current_task_id = 0;
+std::string current_task_name = "";
+int current_task_mode = 0;
+int current_task_duration = 0;
+int current_task_count = 0;
+std::string current_route_name = "";
+std::string current_file_name = "";
+
 static int sock = -1;
 
 // 新增：全局变量记录最后接收RTK数据的时间戳
@@ -79,6 +88,19 @@ class pl_node : public rclcpp::Node
 
     void task_callback(const sweeper_interfaces::msg::Task::SharedPtr msg)
     {
+        // 保存任务详细信息
+        current_task_id = msg->task_id;
+        current_task_name = msg->task_name;
+        current_task_mode = msg->mode;
+        current_task_duration = msg->duration;
+        current_task_count = msg->count;
+        current_route_name = msg->route_name;
+        current_file_name = msg->file_name;
+
+        LOG_INFO("任务信息更新：ID=%d, 名称=%s, 模式=%d, 时长=%d",
+                 current_task_id, current_task_name.c_str(),
+                 current_task_mode, current_task_duration);
+
         if (is_start == 0 && msg->task_status == 1)
         {
             pthread_create(&pl_thread_t, NULL, pl_thread, NULL);

src/base/mc/src/control_cache.cpp

@@ -34,6 +34,7 @@ sweeper_interfaces::msg::McCtrl get_safe_control()
         msg.angle_speed = 120;
 
         msg.sweep = false;
+        msg.sweep_mode = 0;  // 默认标准模式
     }
     return msg;
 }

src/base/mc/src/timer_tasks.cpp

@@ -32,7 +32,6 @@ void epsTimerTask(CANDriver& canctl)
 }
 
 // 定时器回调：VCU 扫地控制
-// 修改timer_tasks.cpp中的vcuTimerTask函数
 void vcuTimerTask(CANDriver& canctl)
 {
     auto msg = get_safe_control();
@@ -45,29 +44,39 @@ void vcuTimerTask(CANDriver& canctl)
     vcu_enable_cmd.setCANEnable(true);
     canctl.sendFrame(vcu_enable_cmd.toFrame());
 
+    // mode: 0=标准模式(刷子), 1=混合模式(刷子+洒水)
+    bool enable_brush = msg.sweep;  // 两种模式都启用刷子
+    bool enable_water = msg.sweep && (msg.sweep_mode == 1);  // 仅混合模式启用洒水
+
     // ===== 控制0x211报文 (电机M1-M7) =====
     vcu_motor1_cmd.setByte0(msg.sweep ? 1 : 0);  // 1表示正向运行，0表示停止
-    vcu_motor1_cmd.setByte1(target_value);
+
-    vcu_motor1_cmd.setByte2(target_value);
+    // 标准模式和混合模式都启用的部件（刷子等）
-    vcu_motor1_cmd.setByte3(target_value);
+    vcu_motor1_cmd.setByte1(enable_brush ? target_value : 0);
-    vcu_motor1_cmd.setByte4(target_value2);
+    vcu_motor1_cmd.setByte2(enable_brush ? target_value : 0);
-    vcu_motor1_cmd.setByte5(target_value2);
+    vcu_motor1_cmd.setByte3(enable_brush ? target_value : 0);
-    vcu_motor1_cmd.setByte6(target_value);
+    vcu_motor1_cmd.setByte4(enable_brush ? target_value2 : 100);
-    vcu_motor1_cmd.setByte7(target_value);
+    vcu_motor1_cmd.setByte5(enable_brush ? target_value2 : 100);
+    vcu_motor1_cmd.setByte6(enable_brush ? target_value : 0);
+    vcu_motor1_cmd.setByte7(enable_brush ? target_value : 0);
     canctl.sendFrame(vcu_motor1_cmd.toFrame());
 
     // ===== 控制0x212报文 (电机M8和LED输出) =====
-    vcu_motor2_cmd.setByte0(target_value);
+    // 注意：这里假设某个字节控制洒水，需要根据实际硬件确认
-    vcu_motor2_cmd.setByte1(target_value);
+    // 模式1（混合模式）时启用洒水相关部件
-    vcu_motor2_cmd.setByte2(target_value);
+    vcu_motor2_cmd.setByte0(enable_brush ? target_value : 0);
-    vcu_motor2_cmd.setByte3(target_value);
+    vcu_motor2_cmd.setByte1(enable_brush ? target_value : 0);
-    vcu_motor2_cmd.setByte4(target_value);
+    vcu_motor2_cmd.setByte2(enable_brush ? target_value : 0);
-    vcu_motor2_cmd.setByte5(target_value);
+    vcu_motor2_cmd.setByte3(enable_brush ? target_value : 0);
-    vcu_motor2_cmd.setByte6(target_value);
+    vcu_motor2_cmd.setByte4(enable_water ? target_value : 0);  // 假设Byte4控制洒水
-    vcu_motor2_cmd.setByte7(target_value);
+    vcu_motor2_cmd.setByte5(enable_water ? target_value : 0);  // 假设Byte5控制洒水
+    vcu_motor2_cmd.setByte6(enable_brush ? target_value : 0);
+    vcu_motor2_cmd.setByte7(enable_brush ? target_value : 0);
     canctl.sendFrame(vcu_motor2_cmd.toFrame());
 
-    LOG_INFO_THROTTLE(1000, "[VCU] Sweep? %s", msg.sweep ? "Yes" : "No");
+    LOG_INFO_THROTTLE(1000, "[VCU] Sweep? %s, Mode: %d (0=标准,1=混合), Water: %s",
+                      msg.sweep ? "Yes" : "No", msg.sweep_mode,
+                      enable_water ? "On" : "Off");
 }
 
 // 定时器回调：BMS 查询任务

src/base/sweeper_interfaces/msg/McCtrl.msg

@@ -11,3 +11,4 @@ uint16 angle_speed	#转向角速度 120-1500rpm
 
 #vcu部分
 bool sweep          #清扫 true:清扫 false:不清扫
+int32 sweep_mode    #清扫模式: 0=标准模式(刷子), 1=混合模式(刷子+洒水)

src/base/sweeper_interfaces/msg/Task.msg

@@ -1 +1,8 @@
 int32 task_status
+int32 task_id
+string task_name
+int32 mode           # 清扫模式: 0=标准模式(刷子), 1=混合模式(刷子+洒水)
+int32 duration       # 作业时长，单位：分钟
+int32 count          # 清扫次数
+string route_name    # 路径名称
+string file_name     # 文件名称

src/communication/task_manager/include/task_manager/task_manager.hpp

@@ -34,8 +34,25 @@ class TaskManager
     // 获取当前任务ID
     int getCurrentTaskId();
 
-    // 更新当前任务
+    // 更新当前任务（完整信息）
-    void updateCurrentTask(int taskId, TaskStatus status);
+    void updateCurrentTask(int taskId, TaskStatus status, const std::string& name = "",
+                          int mode = 0, int duration = 0, int count = 0,
+                          const ROUTE_INFO& routeInfo = ROUTE_INFO());
+
+    // 获取当前任务名称
+    std::string getCurrentTaskName();
+
+    // 获取当前任务模式
+    int getCurrentTaskMode();
+
+    // 获取当前任务时长
+    int getCurrentTaskDuration();
+
+    // 获取当前任务次数
+    int getCurrentTaskCount();
+
+    // 获取当前路径信息
+    ROUTE_INFO getCurrentRouteInfo();
 
     // 获取任务队列大小
     int getQueueSize();

src/communication/task_manager/include/task_manager/task_types.hpp

@@ -53,8 +53,9 @@ struct TASK_VALUE
 {
     long id;
     string name;
-    int mode;
+    int mode;          // 清扫模式: 0=标准模式(刷子), 1=混合模式(刷子+洒水)
-    int count;
+    int duration;      // 作业时长，单位：分钟
+    int count;         // 清扫次数（保留兼容）
     ROUTE_INFO routeInfo;
 };
 struct JSON_DATA
@@ -84,9 +85,14 @@ struct CurrentTask
 {
     int id;
     TaskStatus status;
+    string name;           // 任务名称
+    int mode;              // 清扫模式: 0=标准模式(刷子), 1=混合模式(刷子+洒水)
+    int duration;          // 作业时长，单位：分钟
+    int count;             // 清扫次数
+    ROUTE_INFO routeInfo;  // 路径信息
     std::chrono::time_point<std::chrono::steady_clock> lastUpdateTime;
 
-    CurrentTask() : id(0), status(PENDING) {}
+    CurrentTask() : id(0), status(PENDING), mode(0), duration(0), count(0) {}
 };
 
 #endif

src/communication/task_manager/src/task_manager.cpp

@@ -75,13 +75,51 @@ int TaskManager::getCurrentTaskId()
     return current_task_.id;
 }
 
-void TaskManager::updateCurrentTask(int taskId, TaskStatus status)
+void TaskManager::updateCurrentTask(int taskId, TaskStatus status, const std::string& name,
+                                     int mode, int duration, int count,
+                                     const ROUTE_INFO& routeInfo)
 {
     std::lock_guard<std::mutex> lock(task_mutex_);
     current_task_.id = taskId;
     current_task_.status = status;
+    if (!name.empty()) current_task_.name = name;
+    if (mode >= 0) current_task_.mode = mode;
+    if (duration >= 0) current_task_.duration = duration;
+    if (count >= 0) current_task_.count = count;
+    if (!routeInfo.fileName.empty()) current_task_.routeInfo = routeInfo;
     current_task_.lastUpdateTime = std::chrono::steady_clock::now();
-    LOG_INFO("Task updated: ID=%d, Status=%d", taskId, status);
+    LOG_INFO("Task updated: ID=%d, Status=%d, Name=%s, Mode=%d, Duration=%d",
+             taskId, status, name.c_str(), mode, duration);
+}
+
+std::string TaskManager::getCurrentTaskName()
+{
+    std::lock_guard<std::mutex> lock(task_mutex_);
+    return current_task_.name;
+}
+
+int TaskManager::getCurrentTaskMode()
+{
+    std::lock_guard<std::mutex> lock(task_mutex_);
+    return current_task_.mode;
+}
+
+int TaskManager::getCurrentTaskDuration()
+{
+    std::lock_guard<std::mutex> lock(task_mutex_);
+    return current_task_.duration;
+}
+
+int TaskManager::getCurrentTaskCount()
+{
+    std::lock_guard<std::mutex> lock(task_mutex_);
+    return current_task_.count;
+}
+
+ROUTE_INFO TaskManager::getCurrentRouteInfo()
+{
+    std::lock_guard<std::mutex> lock(task_mutex_);
+    return current_task_.routeInfo;
 }
 
 string TaskManager::calculate_md5(const string& filename)
@@ -158,11 +196,18 @@ void TaskManager::copyFileAndOverwrite(const string& sourceFilePath, const strin
 bool TaskManager::processStartTask(const JSON_DATA& json_data, long seqNo)
 {
     LOG_INFO("Processing start task ID: %d", json_data.value.id);
+    LOG_INFO("Name: %s", json_data.value.name.c_str());
+    LOG_INFO("Mode: %d", json_data.value.mode);
+    LOG_INFO("Duration: %d minutes", json_data.value.duration);
+    LOG_INFO("Count: %d", json_data.value.count);
     LOG_INFO("URL: %s", json_data.value.routeInfo.url.c_str());
     LOG_INFO("MD5: %s", json_data.value.routeInfo.md5.c_str());
 
-    // 更新当前任务ID
+    // 更新当前任务（保存完整信息）
-    updateCurrentTask(json_data.value.id, TaskStatus::RUNNING);
+    updateCurrentTask(json_data.value.id, TaskStatus::RUNNING,
+                     json_data.value.name, json_data.value.mode,
+                     json_data.value.duration, json_data.value.count,
+                     json_data.value.routeInfo);
 
     string downFileName = json_data.value.routeInfo.fileName;
     string filepath = "routes/" + downFileName;

src/communication/task_manager/src/task_manager_main.cpp

@@ -120,25 +120,36 @@ TASK_VALUE get_task_value(const Json::Value& root)
 
     if (root.isObject())
     {
-        if (root.isMember("id") && root["id"].isInt())
+        if (root.isMember("id") && (root["id"].isInt() || root["id"].isDouble()))
-            task_value.id = root["id"].asInt();
+            task_value.id = root["id"].asInt64();
         else
             LOG_ERROR("Missing or invalid 'id' field");
 
-        if (root.isMember("name") && root["name"].isString())
+        if (root.isMember("name"))
+        {
+            if (root["name"].isString())
                 task_value.name = root["name"].asString();
+            else if (root["name"].isInt() || root["name"].isDouble())
+                task_value.name = std::to_string(root["name"].asInt64());
+        }
         else
-            LOG_ERROR("Missing or invalid 'name' field");
+            LOG_ERROR("Missing 'name' field");
 
         if (root.isMember("mode") && root["mode"].isInt())
             task_value.mode = root["mode"].asInt();
         else
             LOG_ERROR("Missing or invalid 'mode' field");
 
+        // 协议中是 duration (作业时长)，同时兼容 count (清扫次数)
+        if (root.isMember("duration") && root["duration"].isInt())
+            task_value.duration = root["duration"].asInt();
+        else
+            LOG_WARN("Missing 'duration' field, using default 0");
+
         if (root.isMember("count") && root["count"].isInt())
             task_value.count = root["count"].asInt();
         else
-            LOG_ERROR("Missing or invalid 'count' field");
+            LOG_WARN("Missing 'count' field, using default 0");
 
         if (root.isMember("routeInfo") && root["routeInfo"].isObject())
             task_value.routeInfo = get_route_info(root["routeInfo"]);
@@ -414,6 +425,10 @@ void reportTaskStatusToMqtt()
             Json::Value statusMsg;
             statusMsg["id"] = current_id;
             statusMsg["status"] = static_cast<int>(current_status);
+            statusMsg["name"] = task_manager.getCurrentTaskName();
+            statusMsg["mode"] = task_manager.getCurrentTaskMode();
+            statusMsg["duration"] = task_manager.getCurrentTaskDuration();
+            statusMsg["count"] = task_manager.getCurrentTaskCount();
 
             Json::FastWriter writer;
             string statusJson = writer.write(statusMsg);
@@ -479,12 +494,26 @@ class TaskManagerNode : public rclcpp::Node
     {
         sweeper_interfaces::msg::Task msg;
         msg.task_status = task_manager.getTaskStatus();
+        msg.task_id = task_manager.getCurrentTaskId();
+        msg.task_name = task_manager.getCurrentTaskName();
+        msg.mode = task_manager.getCurrentTaskMode();
+        msg.duration = task_manager.getCurrentTaskDuration();
+        msg.count = task_manager.getCurrentTaskCount();
+        auto route_info = task_manager.getCurrentRouteInfo();
+        msg.route_name = route_info.routeName;
+        msg.file_name = route_info.fileName;
 
-        if (msg.task_status != last_status)
+        // 当任务状态变化或有有效任务时发布消息
+        if (msg.task_status != last_status || msg.task_id > 0)
         {
             msg_publisher_->publish(msg);
+            if (msg.task_id > 0) {
+                LOG_INFO("Task published: ID=%d, Status=%d, Name=%s, Mode=%d, Duration=%d",
+                         msg.task_id, msg.task_status, msg.task_name.c_str(), msg.mode, msg.duration);
+            } else {
                 LOG_INFO("Task status published: %d", msg.task_status);
             }
+        }
 
         last_status = msg.task_status;
     }