From 681fa81409c7292a44deacde149681f91bb5d482 Mon Sep 17 00:00:00 2001 From: Alvin-lyq Date: Thu, 7 May 2026 15:02:30 +0800 Subject: [PATCH] pfu pl task_manager debug --- src/autonomy/fu/src/fu_node.cpp | 29 ++++++++++++------- src/autonomy/pl/include/pl/pl.hpp | 5 ++-- src/autonomy/pl/src/pl.cpp | 4 +-- src/autonomy/pl/src/pl_node.cpp | 5 ++-- .../include/task_manager/task_manager.hpp | 3 +- .../task_manager/src/task_manager_main.cpp | 6 ++-- 6 files changed, 30 insertions(+), 22 deletions(-) diff --git a/src/autonomy/fu/src/fu_node.cpp b/src/autonomy/fu/src/fu_node.cpp index 0b05a3e..3aa461e 100644 --- a/src/autonomy/fu/src/fu_node.cpp +++ b/src/autonomy/fu/src/fu_node.cpp @@ -541,7 +541,7 @@ class fu_node : public rclcpp::Node state_machine_->publish_speed = 0; state_machine_->publish_angle = 0.0f; state_machine_->setState(StateMachine::WAITING); - publishControlCommand(true); // 立即发布带刹车的停车指令 + publishControlCommand(true, true); // 立即发布带刹车的停车指令，关闭清扫 LOG_WARN("[紧急停车] 栅格回调中检测到前方障碍物，立即停车！"); } } @@ -1136,7 +1136,7 @@ class fu_node : public rclcpp::Node state_machine_->setState(StateMachine::WAITING); LOG_WARN("[超声波停车] 前方检测到障碍物！左:%ucm 右:%ucm 阈值:%dcm，立即停车！", front_left, front_right, uss_stop_distance_); - publishControlCommand(true); + publishControlCommand(true, true); // 紧急停车并关闭清扫 return; } } @@ -1145,7 +1145,7 @@ class fu_node : public rclcpp::Node // 紧急停车优先级最高 if (executeEmergencyStop()) { - publishControlCommand(true); // 紧急停车使用刹车 + publishControlCommand(true, true); // 紧急停车使用刹车并关闭清扫 return; } @@ -1155,7 +1155,7 @@ class fu_node : public rclcpp::Node state_machine_->publish_speed = 0; state_machine_->publish_angle = 0; LOG_INFO("PL速度为0，已停车"); - publishControlCommand(); + publishControlCommand(false, true); // 停车时关闭清扫部件 return; } @@ -1203,21 +1203,28 @@ class fu_node : public rclcpp::Node // 发布控制指令 publishControlCommand(); } + else + { + // 任务未运行时，发送停止命令，确保车辆和清扫部件都停止 + state_machine_->publish_speed = 0; + state_machine_->publish_angle = 0; + publishControlCommand(false, true); // emergency_brake=false, disable_sweep=true + } } // ======== 发布控制指令 ======== - void publishControlCommand(bool emergency_brake = false) + void publishControlCommand(bool emergency_brake = false, bool disable_sweep = false) { auto message = sweeper_interfaces::msg::McCtrl(); - message.enable_main_brush = true; - message.enable_vacuum = true; + message.enable_main_brush = !disable_sweep; + message.enable_vacuum = !disable_sweep; message.enable_dust_shake = false; - message.enable_main_brush_pole = true; - message.enable_flap_pole = true; - message.enable_side_brush = true; + message.enable_main_brush_pole = !disable_sweep; + message.enable_flap_pole = !disable_sweep; + message.enable_side_brush = !disable_sweep; message.sweep_mode = current_task_mode; // 清扫模式: 0=标准模式(刷子), 1=混合模式(刷子+洒水) - message.enable_water_pump = (message.sweep_mode == 1); + message.enable_water_pump = (!disable_sweep && message.sweep_mode == 1); message.brake = emergency_brake ? 1 : 0; // 紧急停车时启用刹车 message.gear = 2; diff --git a/src/autonomy/pl/include/pl/pl.hpp b/src/autonomy/pl/include/pl/pl.hpp index e0e4c41..246e35b 100644 --- a/src/autonomy/pl/include/pl/pl.hpp +++ b/src/autonomy/pl/include/pl/pl.hpp @@ -1,6 +1,7 @@ #ifndef __PL_H__ #define __PL_H__ +#include #include enum Direction @@ -70,7 +71,7 @@ extern int last_mode; extern std::string filename; extern int d_file; extern Direction drive_mode; -extern TaskStatus task_status; -extern int thread_exit_flag; // 线程退出标志 +extern std::atomic task_status; +extern std::atomic thread_exit_flag; // 线程退出标志 #endif \ No newline at end of file diff --git a/src/autonomy/pl/src/pl.cpp b/src/autonomy/pl/src/pl.cpp index 48fa209..e2d34a7 100644 --- a/src/autonomy/pl/src/pl.cpp +++ b/src/autonomy/pl/src/pl.cpp @@ -39,8 +39,8 @@ int mode = 0; int last_mode = 0; Direction drive_mode = Direction::STRAIGHT_D; -TaskStatus task_status = TaskStatus::PENDING; -int thread_exit_flag = 0; // 线程退出标志，0=继续运行，1=需要退出 +std::atomic task_status{TaskStatus::PENDING}; +std::atomic thread_exit_flag{0}; // 线程退出标志，0=继续运行，1=需要退出 // 返回单位：m double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2) diff --git a/src/autonomy/pl/src/pl_node.cpp b/src/autonomy/pl/src/pl_node.cpp index 3c58775..654dd80 100644 --- a/src/autonomy/pl/src/pl_node.cpp +++ b/src/autonomy/pl/src/pl_node.cpp @@ -4,6 +4,7 @@ #include #include +#include #include // 新增：用于时间戳处理 #include #include @@ -21,7 +22,7 @@ using namespace std; using namespace std::chrono; // 新增：时间戳命名空间 pthread_t pl_thread_t; -int is_start = 0; +std::atomic is_start{0}; // 保存完整的任务信息 int current_task_id = 0; @@ -181,7 +182,7 @@ class pl_node : public rclcpp::Node { close(sock); sock = -1; - return timer_callback_pl(); // 重试 + return; // 下次定时器回调会重试连接 } } else if (sock >= 0) 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 dbc1c66..a425216 100644 --- a/src/communication/task_manager/include/task_manager/task_manager.hpp +++ b/src/communication/task_manager/include/task_manager/task_manager.hpp @@ -1,6 +1,7 @@ #ifndef __TASK_MANAGER_H__ #define __TASK_MANAGER_H__ +#include #include #include #include @@ -104,7 +105,7 @@ class TaskManager std::mutex task_mutex_; string destination_file_path_; - int task_status_; + std::atomic task_status_; // 回调函数用于发送MQTT响应 std::function send_response_callback_; diff --git a/src/communication/task_manager/src/task_manager_main.cpp b/src/communication/task_manager/src/task_manager_main.cpp index 7f86c04..1a607f5 100644 --- a/src/communication/task_manager/src/task_manager_main.cpp +++ b/src/communication/task_manager/src/task_manager_main.cpp @@ -373,8 +373,6 @@ int handleMqttMessage(char* topicName, int topicLen, MQTTClient_message* message LOG_INFO("Stop task: %d", stop_id); task_manager.addTask(mqtt_json); - - sendTaskResponse(seqNo, 200, "Task stop command received"); } catch (const std::exception& e) { @@ -609,9 +607,9 @@ class TaskManagerNode : public rclcpp::Node } else { - // 最后一条路径完成，更新任务管理器内部状态为 COMPLETED + // 最后一条路径完成，发送 Status=0 触发 PL 停止线程 LOG_INFO("所有 %d 条路径执行完成，任务结束", task_manager.getRouteCount()); - task_manager.setTaskStatus(TaskStatus::COMPLETED); + task_manager.setTaskStatus(0); // 设为0触发PL停止 task_manager.updateCurrentTaskStatus(TaskStatus::COMPLETED); } }