lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

完善任务处理系统

Browse Source
This commit is contained in:
Alvin-lyq 2026-03-30 12:40:31 +08:00
parent f474f3901c
commit 45327d34ca
10 changed files with 193 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/autonomy/fu/src/fu_node.cpp
View File

@ -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;

22
src/autonomy/pl/src/pl_node.cpp
View File

@ -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);

1
src/base/mc/src/control_cache.cpp
View File

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

43
src/base/mc/src/timer_tasks.cpp
View File

@ -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 查询任务

1
src/base/sweeper_interfaces/msg/McCtrl.msg
View File

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

7
src/base/sweeper_interfaces/msg/Task.msg
View File

@ -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 # 文件名称

21
src/communication/task_manager/include/task_manager/task_manager.hpp
View File

@ -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();

12
src/communication/task_manager/include/task_manager/task_types.hpp
View File

@ -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<std::chrono::steady_clock> lastUpdateTime;
CurrentTask() : id(0), status(PENDING) {}
CurrentTask() : id(0), status(PENDING), mode(0), duration(0), count(0) {}
};
#endif

53
src/communication/task_manager/src/task_manager.cpp
View File

@ -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;

45
src/communication/task_manager/src/task_manager_main.cpp
View File

@ -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<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,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;