From 45327d34ca078330d4214aa79200d324cdded4cd Mon Sep 17 00:00:00 2001 From: Alvin-lyq Date: Mon, 30 Mar 2026 12:40:31 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=8C=E5=96=84=E4=BB=BB=E5=8A=A1=E5=A4=84?= =?UTF-8?q?=E7=90=86=E7=B3=BB=E7=BB=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/autonomy/fu/src/fu_node.cpp | 21 ++++++++ src/autonomy/pl/src/pl_node.cpp | 22 ++++++++ src/base/mc/src/control_cache.cpp | 1 + src/base/mc/src/timer_tasks.cpp | 43 +++++++++------ src/base/sweeper_interfaces/msg/McCtrl.msg | 1 + src/base/sweeper_interfaces/msg/Task.msg | 9 +++- .../include/task_manager/task_manager.hpp | 21 +++++++- .../include/task_manager/task_types.hpp | 12 +++-- .../task_manager/src/task_manager.cpp | 53 +++++++++++++++++-- .../task_manager/src/task_manager_main.cpp | 45 +++++++++++++--- 10 files changed, 193 insertions(+), 35 deletions(-) diff --git a/src/autonomy/fu/src/fu_node.cpp b/src/autonomy/fu/src/fu_node.cpp index 2baf635..3a3e137 100644 --- a/src/autonomy/fu/src/fu_node.cpp +++ b/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( + "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( "rtk_message", 10, std::bind(&fu_node::msg_callback_rtk, this, std::placeholders::_1)); + task_subscribe_ = this->create_subscription( + "task_message/download", 10, std::bind(&fu_node::task_callback, this, std::placeholders::_1)); // ======== 发布者初始化 ======== pub_mc = this->create_publisher("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::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; + rclcpp::Subscription::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; diff --git a/src/autonomy/pl/src/pl_node.cpp b/src/autonomy/pl/src/pl_node.cpp index faa3b4a..2be7df4 100644 --- a/src/autonomy/pl/src/pl_node.cpp +++ b/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); diff --git a/src/base/mc/src/control_cache.cpp b/src/base/mc/src/control_cache.cpp index c5fec19..1d24549 100644 --- a/src/base/mc/src/control_cache.cpp +++ b/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; } diff --git a/src/base/mc/src/timer_tasks.cpp b/src/base/mc/src/timer_tasks.cpp index 8357a4c..e77ed69 100644 --- a/src/base/mc/src/timer_tasks.cpp +++ b/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.setByte4(target_value2); - vcu_motor1_cmd.setByte5(target_value2); - vcu_motor1_cmd.setByte6(target_value); - vcu_motor1_cmd.setByte7(target_value); + + // 标准模式和混合模式都启用的部件（刷子等） + vcu_motor1_cmd.setByte1(enable_brush ? target_value : 0); + vcu_motor1_cmd.setByte2(enable_brush ? target_value : 0); + vcu_motor1_cmd.setByte3(enable_brush ? target_value : 0); + vcu_motor1_cmd.setByte4(enable_brush ? target_value2 : 100); + 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); - vcu_motor2_cmd.setByte2(target_value); - vcu_motor2_cmd.setByte3(target_value); - vcu_motor2_cmd.setByte4(target_value); - vcu_motor2_cmd.setByte5(target_value); - vcu_motor2_cmd.setByte6(target_value); - vcu_motor2_cmd.setByte7(target_value); + // 注意：这里假设某个字节控制洒水，需要根据实际硬件确认 + // 模式1（混合模式）时启用洒水相关部件 + vcu_motor2_cmd.setByte0(enable_brush ? target_value : 0); + vcu_motor2_cmd.setByte1(enable_brush ? target_value : 0); + vcu_motor2_cmd.setByte2(enable_brush ? target_value : 0); + vcu_motor2_cmd.setByte3(enable_brush ? target_value : 0); + vcu_motor2_cmd.setByte4(enable_water ? target_value : 0); // 假设Byte4控制洒水 + 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 查询任务 diff --git a/src/base/sweeper_interfaces/msg/McCtrl.msg b/src/base/sweeper_interfaces/msg/McCtrl.msg index df4d98d..6bedf30 100644 --- a/src/base/sweeper_interfaces/msg/McCtrl.msg +++ b/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=混合模式(刷子+洒水) diff --git a/src/base/sweeper_interfaces/msg/Task.msg b/src/base/sweeper_interfaces/msg/Task.msg index 4dc0cd4..0d7493b 100644 --- a/src/base/sweeper_interfaces/msg/Task.msg +++ b/src/base/sweeper_interfaces/msg/Task.msg @@ -1 +1,8 @@ -int32 task_status \ No newline at end of file +int32 task_status +int32 task_id +string task_name +int32 mode # 清扫模式: 0=标准模式(刷子), 1=混合模式(刷子+洒水) +int32 duration # 作业时长，单位：分钟 +int32 count # 清扫次数 +string route_name # 路径名称 +string file_name # 文件名称 \ No newline at end of file diff --git a/src/communication/task_manager/include/task_manager/task_manager.hpp b/src/communication/task_manager/include/task_manager/task_manager.hpp index 113e1ee..1e44787 100644 --- a/src/communication/task_manager/include/task_manager/task_manager.hpp +++ b/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(); diff --git a/src/communication/task_manager/include/task_manager/task_types.hpp b/src/communication/task_manager/include/task_manager/task_types.hpp index c8cc51a..73f590b 100644 --- a/src/communication/task_manager/include/task_manager/task_types.hpp +++ b/src/communication/task_manager/include/task_manager/task_types.hpp @@ -53,8 +53,9 @@ struct TASK_VALUE { long id; string name; - int mode; - int count; + int mode; // 清扫模式: 0=标准模式(刷子), 1=混合模式(刷子+洒水) + 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 lastUpdateTime; - CurrentTask() : id(0), status(PENDING) {} + CurrentTask() : id(0), status(PENDING), mode(0), duration(0), count(0) {} }; #endif \ No newline at end of file diff --git a/src/communication/task_manager/src/task_manager.cpp b/src/communication/task_manager/src/task_manager.cpp index 4830816..cf3db20 100644 --- a/src/communication/task_manager/src/task_manager.cpp +++ b/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 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 lock(task_mutex_); + return current_task_.name; +} + +int TaskManager::getCurrentTaskMode() +{ + std::lock_guard lock(task_mutex_); + return current_task_.mode; +} + +int TaskManager::getCurrentTaskDuration() +{ + std::lock_guard lock(task_mutex_); + return current_task_.duration; +} + +int TaskManager::getCurrentTaskCount() +{ + std::lock_guard lock(task_mutex_); + return current_task_.count; +} + +ROUTE_INFO TaskManager::getCurrentRouteInfo() +{ + std::lock_guard 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; diff --git a/src/communication/task_manager/src/task_manager_main.cpp b/src/communication/task_manager/src/task_manager_main.cpp index 4921000..feca6ee 100644 --- a/src/communication/task_manager/src/task_manager_main.cpp +++ b/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()) - task_value.id = root["id"].asInt(); + if (root.isMember("id") && (root["id"].isInt() || root["id"].isDouble())) + task_value.id = root["id"].asInt64(); else LOG_ERROR("Missing or invalid 'id' field"); - if (root.isMember("name") && root["name"].isString()) - task_value.name = root["name"].asString(); + 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(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,11 +494,25 @@ 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); - LOG_INFO("Task status published: %d", msg.task_status); + 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;