pfu pl task_manager debug
This commit is contained in:
parent
cf512e711e
commit
681fa81409
@ -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;
|
||||
|
||||
@ -1,6 +1,7 @@
|
||||
#ifndef __PL_H__
|
||||
#define __PL_H__
|
||||
|
||||
#include <atomic>
|
||||
#include <string>
|
||||
|
||||
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<TaskStatus> task_status;
|
||||
extern std::atomic<int> thread_exit_flag; // 线程退出标志
|
||||
|
||||
#endif
|
||||
@ -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<TaskStatus> task_status{TaskStatus::PENDING};
|
||||
std::atomic<int> thread_exit_flag{0}; // 线程退出标志，0=继续运行，1=需要退出
|
||||
|
||||
// 返回单位：m
|
||||
double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2)
|
||||
|
||||
@ -4,6 +4,7 @@
|
||||
#include <sys/stat.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono> // 新增：用于时间戳处理
|
||||
#include <cstring>
|
||||
#include <fstream>
|
||||
@ -21,7 +22,7 @@
|
||||
using namespace std;
|
||||
using namespace std::chrono; // 新增：时间戳命名空间
|
||||
pthread_t pl_thread_t;
|
||||
int is_start = 0;
|
||||
std::atomic<int> 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)
|
||||
|
||||
@ -1,6 +1,7 @@
|
||||
#ifndef __TASK_MANAGER_H__
|
||||
#define __TASK_MANAGER_H__
|
||||
|
||||
#include <atomic>
|
||||
#include <boost/any.hpp>
|
||||
#include <condition_variable>
|
||||
#include <functional>
|
||||
@ -104,7 +105,7 @@ class TaskManager
|
||||
std::mutex task_mutex_;
|
||||
|
||||
string destination_file_path_;
|
||||
int task_status_;
|
||||
std::atomic<int> task_status_;
|
||||
|
||||
// 回调函数用于发送MQTT响应
|
||||
std::function<void(const string& topic, int seqNo, int code, const string& msg)> send_response_callback_;
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user