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

Compare commits

base: lyq:3a52c24a0ab1b0f128eb03c9e930b9cc2de713e3
Branches Tags
lyq:master
lyq:controller
lyq:250901
...
compare: lyq:82b77a9f2a6652d4122c67246d61a553dcaced8e
Branches Tags
lyq:master
lyq:controller
lyq:250901

2 Commits
3a52c24a0a ... 82b77a9f2a

Author SHA1 Message Date
lyq
82b77a9f2a Merge branch 'master' of http://192.168.4.194:3000/lyq/sweeper_whu 2025-10-25 10:41:07 +08:00
lyq
25f6914b41 更新系统核心功能和配置文件 
主要更新内容：
- 更新主配置文件 config.json
- 优化启动脚本：prepare.sh、run.sh、start_all.sh
- 新增GPS加载文件 gps_load_now.txt 和路由文件
- 功能单元(fu)：更新参数配置和节点逻辑
- 主控(mc)：改进定时任务处理
- MQTT报告：更新客户端配置和通信逻辑
- 路径规划(pl)：优化节点功能
- GPS发布：增强数据发布模块
- 无线电控制：改进控制逻辑
- 远程控制：优化节点处理
- 雷达点云合并：更新启动配置
- RTK定位：改进节点功能
- 订阅模块：优化数据订阅逻辑
- 任务管理器：增强任务管理功能
 2025-10-25 10:39:02 +08:00
19 changed files with 625 additions and 46 deletions
Show Stats Expand all files Collapse all files

18
config.json
View File

@ -1,6 +1,8 @@
{ {
"mqtt": { "mqtt": {
"vid": "V060003", "vid": "V060003",
"upload_url": "https://qsc.ntiov.com:8443/api/sys/route/upload",
"download_url_pre": "http://36.153.162.171:9510/api/ccp-web/file/",
"inter_net_address": "tcp://192.168.4.196", "inter_net_address": "tcp://192.168.4.196",
"inter_net_port": 11883, "inter_net_port": 11883,
"external_net_address": "tcp://36.153.162.171", "external_net_address": "tcp://36.153.162.171",
@ -8,6 +10,20 @@
"mqtt_user": "zxwl", "mqtt_user": "zxwl",
"mqtt_password": "zxwl1234@", "mqtt_password": "zxwl1234@",
"pub_gps_topic": "/zxwl/vehicle/V060003/gps", "pub_gps_topic": "/zxwl/vehicle/V060003/gps",
"remote_topic": "/zxwl/vehicle/V060003/ctrl" "remote_topic": "/zxwl/vehicle/V060003/ctrl",
"mqtt_topic_push_status": "/zxwl/vehicle/V060003/task/status",
"mqtt_topic_sub_task": "/zxwl/vehicle/V060003/task"
},
"detect_line_tolerance": 3.06,
"detect_head_tolerance": 2,
"lidar_detect_locate": {
"front": 5,
"f_front": 18,
"rear": -8,
"r_rear": -16,
"left": -6,
"l_left": -13,
"right": 6,
"r_right": 13
} }
} }

44
gps_load_now.txt Normal file
View File

@ -0,0 +1,44 @@
32.0311442390
120.91525759600
358.131012
0.000000
32.0311534830
120.91525806000
10.571000
0.000000
32.0311624300
120.91526160300
30.403999
0.000000
32.0311698350
120.91526811100
42.812000
0.000000
32.0311764320
120.91527574000
45.012001
0.000000
32.0311827500
120.91528336200
45.269001
0.000000
32.0311892690
120.91529116800
46.345001
0.000000
32.0311954980
120.91529957900
54.297001
0.000000
32.0312002620
120.91530907900
66.992996
0.000000
32.0312030070
120.91531955100
80.920998
0.000000
32.0312029590
120.91530925500
78.594002
0.000000

5
prepare.sh
View File

@ -2,5 +2,6 @@
sudo ./can.sh sudo ./can.sh
sudo chmod 777 /dev/ttyUSB0 sudo chmod 777 /dev/ttyTHS0
sudo chmod 777 /dev/ttyTHS1
sudo ip route add 36.153.162.0/24 via 192.168.5.1 dev eth4

44
routes/gps_load_1761208243532.txt Normal file
View File

@ -0,0 +1,44 @@
32.0311442390
120.91525759600
358.131012
0.000000
32.0311534830
120.91525806000
10.571000
0.000000
32.0311624300
120.91526160300
30.403999
0.000000
32.0311698350
120.91526811100
42.812000
0.000000
32.0311764320
120.91527574000
45.012001
0.000000
32.0311827500
120.91528336200
45.269001
0.000000
32.0311892690
120.91529116800
46.345001
0.000000
32.0311954980
120.91529957900
54.297001
0.000000
32.0312002620
120.91530907900
66.992996
0.000000
32.0312030070
120.91531955100
80.920998
0.000000
32.0312029590
120.91530925500
78.594002
0.000000

5
run.sh
View File

@ -16,6 +16,11 @@ sleep 0.2s
}& }&
sleep 0.2s sleep 0.2s
{
gnome-terminal --title="report" --tab "XXD_ros" -- bash -c "ros2 run mqtt_report mqtt_report_node"
}&
sleep 0.2s
{ {
gnome-terminal --title="lidar" --tab "XXD_ros" -- bash -c "ros2 launch rslidar_sdk start_double.launch.py" gnome-terminal --title="lidar" --tab "XXD_ros" -- bash -c "ros2 launch rslidar_sdk start_double.launch.py"
}& }&

2
src/fu/config/fu_params.yaml
View File

@ -1,7 +1,7 @@
FU: FU:
ros__parameters: ros__parameters:
# 遇障停车功能 False or True # 遇障停车功能 False or True
enable_obstacle_stop: True enable_obstacle_stop: False
# 绕障功能 False or True # 绕障功能 False or True
enable_obstacle_avoid: False enable_obstacle_avoid: False

6
src/fu/src/fu_node.cpp
View File

@ -648,9 +648,11 @@ private:
x = msg->x; x = msg->x;
y = msg->y; y = msg->y;
speed = (int)msg->speed; // 获取PL消息中的速度 speed = (int)msg->speed; // 获取PL消息中的速度
std::cout << "pl speed " << speed << std::endl;
reliability = msg->reliability; reliability = msg->reliability;
located = msg->enter_range; located = msg->enter_range;
is_start = msg->is_start; is_start = msg->is_start;
std::cout << "pl is_start " << is_start << std::endl;
} }
void msg_callback_DetectLine(const sweeper_interfaces::msg::DetectLine::SharedPtr msg) void msg_callback_DetectLine(const sweeper_interfaces::msg::DetectLine::SharedPtr msg)
@ -677,13 +679,13 @@ private:
message.sweep = (avoid_ctx_.state == AVOID_NONE); // 避障状态为无避障 且 任务开始 才开启清扫 message.sweep = (avoid_ctx_.state == AVOID_NONE); // 避障状态为无避障 且 任务开始 才开启清扫
message.gear = 0; message.gear = 0;
message.brake = (publish_speed == 0) ? 1 : 0; message.brake = (publish_speed == 0) ? 1 : 0;
message.rpm = publish_speed; message.rpm = publish_speed * 25;
message.angle = publish_angle; // 负数表示左转，正数表示右转 message.angle = publish_angle; // 负数表示左转，正数表示右转
message.angle_speed = 800; message.angle_speed = 800;
pub_mc->publish(message); pub_mc->publish(message);
RCLCPP_INFO_STREAM(this->get_logger(), "控制: 速度=" << publish_speed << ", 角度=" << publish_angle << ", 状态=" << [&]() RCLCPP_INFO_STREAM(this->get_logger(), "控制: 速度=" << publish_speed * 30 << ", 角度=" << publish_angle << ", 状态=" << [&]()
{ {
switch(avoid_state_){ switch(avoid_state_){
case AVOID_NONE: return "无避障"; case AVOID_NONE: return "无避障";

12
src/mc/src/timer_tasks.cpp
View File

@ -11,6 +11,10 @@ void mcuTimerTask(CANDriver &canctl)
mcu_cmd.setRPM(msg.rpm); mcu_cmd.setRPM(msg.rpm);
mcu_cmd.setBrake(msg.brake); mcu_cmd.setBrake(msg.brake);
canctl.sendFrame(mcu_cmd.toFrame()); canctl.sendFrame(mcu_cmd.toFrame());
// std::cout << "mcuTimerTask" << std::endl;
std::cout << "msg.gear " << msg.gear << std::endl;
std::cout << "msg.brake " << msg.brake << std::endl;
std::cout << "msg.rpm " << msg.rpm << std::endl;
} }
// 定时器回调：EPS 控制 // 定时器回调：EPS 控制
@ -22,6 +26,7 @@ void epsTimerTask(CANDriver &canctl)
eps_cmd.setAngularSpeed(msg.angle_speed); eps_cmd.setAngularSpeed(msg.angle_speed);
eps_cmd.pack(); eps_cmd.pack();
canctl.sendFrame(eps_cmd.toFrame()); canctl.sendFrame(eps_cmd.toFrame());
// std::cout << "epsTimerTask" << std::endl;
} }
// 定时器回调：VCU 扫地控制 // 定时器回调：VCU 扫地控制
@ -39,6 +44,7 @@ void vcuTimerTask(CANDriver &canctl)
// 发送CAN使能指令 // 发送CAN使能指令
vcu_enable_cmd.setCANEnable(true); vcu_enable_cmd.setCANEnable(true);
canctl.sendFrame(vcu_enable_cmd.toFrame()); canctl.sendFrame(vcu_enable_cmd.toFrame());
// std::cout << "mcuTimerTask" << std::endl;
vcu_initialized = true; vcu_initialized = true;
RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), "[VCU] CAN control enabled"); RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), "[VCU] CAN control enabled");
} }
@ -61,6 +67,7 @@ void vcuTimerTask(CANDriver &canctl)
vcu_motor1_cmd.setByte6(target_value); vcu_motor1_cmd.setByte6(target_value);
vcu_motor1_cmd.setByte7(target_value); vcu_motor1_cmd.setByte7(target_value);
canctl.sendFrame(vcu_motor1_cmd.toFrame()); canctl.sendFrame(vcu_motor1_cmd.toFrame());
// std::cout << "vcuTimerTask1" << std::endl;
// ===== 控制0x212报文 (电机M8和LED输出) ===== // ===== 控制0x212报文 (电机M8和LED输出) =====
vcu_motor2_cmd.setByte0(target_value); vcu_motor2_cmd.setByte0(target_value);
@ -72,9 +79,10 @@ void vcuTimerTask(CANDriver &canctl)
vcu_motor2_cmd.setByte6(target_value); vcu_motor2_cmd.setByte6(target_value);
vcu_motor2_cmd.setByte7(target_value); vcu_motor2_cmd.setByte7(target_value);
canctl.sendFrame(vcu_motor2_cmd.toFrame()); canctl.sendFrame(vcu_motor2_cmd.toFrame());
// std::cout << "vcuTimerTask2" << std::endl;
RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), // RCLCPP_INFO(rclcpp::get_logger("timer_tasks"),
"[VCU] Sweep mode %s", msg.sweep ? "activated" : "deactivated"); // "[VCU] Sweep mode %s", msg.sweep ? "activated" : "deactivated");
// 更新状态记录 // 更新状态记录
last_sweep_state = msg.sweep; last_sweep_state = msg.sweep;

4
src/mqtt_report/config/config(copy).json
View File

@ -1,8 +1,8 @@
{ {
"vid": "V060003", "vid": "V060003",
"mqtt": { "mqtt": {
"ip": "36.153.162.171", "ip": "192.168.4.196",
"port": 19683, "port": 11883,
"info_topic": "/zxwl/vehicle/{vid}/info", "info_topic": "/zxwl/vehicle/{vid}/info",
"fault_topic": "/zxwl/vehicle/{vid}/fault" "fault_topic": "/zxwl/vehicle/{vid}/fault"
} }

4
src/mqtt_report/config/config.json
View File

@ -1,8 +1,8 @@
{ {
"vid": "V060003", "vid": "V060003",
"mqtt": { "mqtt": {
"ip": "192.168.4.196", "ip": "36.153.162.171",
"port": 11883, "port": 19683,
"info_topic": "/zxwl/vehicle/{vid}/info", "info_topic": "/zxwl/vehicle/{vid}/info",
"fault_topic": "/zxwl/vehicle/{vid}/fault" "fault_topic": "/zxwl/vehicle/{vid}/fault"
} }

5
src/mqtt_report/include/mqtt_report/mqtt_client.hpp
View File

@ -43,7 +43,8 @@ public:
MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer; MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer;
conn_opts.keepAliveInterval = 20; conn_opts.keepAliveInterval = 20;
conn_opts.cleansession = 1; conn_opts.cleansession = 1;
if (!username_.empty()) if (!username_.empty())
{ {
conn_opts.username = username_.c_str(); conn_opts.username = username_.c_str();
@ -77,7 +78,7 @@ public:
} }
} }
void publish(const std::string &topic, const std::string &payload, int qos = 1, bool retained = false) void publish(const std::string &topic, const std::string &payload, int qos = 0, bool retained = false)
{ {
if (!client_) if (!client_)
{ {

12
src/mqtt_report/src/mqtt_report.cpp
View File

@ -24,7 +24,7 @@ void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg)
{ {
info_report.power = msg->data[4] * 0.5f; info_report.power = msg->data[4] * 0.5f;
info_report.chargeStatus = (msg->data[3] == 0x61); info_report.chargeStatus = (msg->data[3] == 0x61);
// std::cout << "[0x1821E5F1] power : " << info_report.power << std::endl; std::cout << "[0x1821E5F1] power : " << info_report.power << std::endl;
// std::cout << "[0x1821E5F1] chargeStatus : " << info_report.chargeStatus << std::endl; // std::cout << "[0x1821E5F1] chargeStatus : " << info_report.chargeStatus << std::endl;
break; break;
} }
@ -63,7 +63,7 @@ void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg)
float gear_ratio = 32.0; // 电机速比 float gear_ratio = 32.0; // 电机速比
float wheel_radius = 0.3; // 轮胎直径(m) float wheel_radius = 0.3; // 轮胎直径(m)
info_report.speed = (actual_speed_rpm * 60 * 3.1416 * wheel_radius) / (1000 * gear_ratio); info_report.speed = (actual_speed_rpm * 60 * 3.1416 * wheel_radius) / (1000 * gear_ratio);
// std::cout << "[0x0CF11E00] speed : " << info_report.speed << std::endl; std::cout << "[0x0CF11E00] speed : " << info_report.speed << std::endl;
// 故障码（data[6]：低字节，data[7]：高字节） // 故障码（data[6]：低字节，data[7]：高字节）
uint16_t mcu_fault_code = (static_cast<uint16_t>(msg->data[7]) << 8) | msg->data[6]; uint16_t mcu_fault_code = (static_cast<uint16_t>(msg->data[7]) << 8) | msg->data[6];
@ -110,9 +110,9 @@ void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg)
// 档位优先级：命令状态优先（空挡时以反馈状态为准，可根据需求调整） // 档位优先级：命令状态优先（空挡时以反馈状态为准，可根据需求调整）
info_report.gear = feedback_gear; info_report.gear = feedback_gear;
// std::cout << "[0x0CF11F00] controllerTemp : " << info_report.controllerTemp << std::endl; std::cout << "[0x0CF11F00] controllerTemp : " << info_report.controllerTemp << std::endl;
// std::cout << "[0x0CF11F00] motorTemp : " << info_report.motorTemp << std::endl; std::cout << "[0x0CF11F00] motorTemp : " << info_report.motorTemp << std::endl;
// std::cout << "[0x0CF11F00] gear : " << info_report.gear << std::endl; std::cout << "[0x0CF11F00] gear : " << info_report.gear << std::endl;
break; break;
} }
@ -123,7 +123,7 @@ void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg)
// 第3、4字节拼接为原始角度数据 // 第3、4字节拼接为原始角度数据
uint16_t raw_value = static_cast<uint16_t>((msg->data[3] << 8) | msg->data[4]); uint16_t raw_value = static_cast<uint16_t>((msg->data[3] << 8) | msg->data[4]);
info_report.steeringAngle = (raw_value - 1024) / 7.0f; info_report.steeringAngle = (raw_value - 1024) / 7.0f;
// std::cout << "[0x401] angle : " << info_report.angle << std::endl; std::cout << "[0x401] steeringAngle : " << info_report.steeringAngle << std::endl;
// 清空所有 EPS 错误码 // 清空所有 EPS 错误码
for (int code = 1201; code <= 1218; ++code) for (int code = 1201; code <= 1218; ++code)

2
src/pl/src/pl_node.cpp
View File

@ -94,7 +94,7 @@ private:
serv_addr.sin_family = AF_INET; serv_addr.sin_family = AF_INET;
serv_addr.sin_port = htons(9999); serv_addr.sin_port = htons(9999);
if (inet_pton(AF_INET, "192.168.3.17", &serv_addr.sin_addr) <= 0) if (inet_pton(AF_INET, "192.168.5.1", &serv_addr.sin_addr) <= 0)
{ {
RCLCPP_ERROR(this->get_logger(), "Invalid address/ Address not supported"); RCLCPP_ERROR(this->get_logger(), "Invalid address/ Address not supported");
close(sock); close(sock);

2
src/pub_gps/src/pub_gps_node.cpp
View File

@ -167,7 +167,7 @@ bool load_config(const std::string &path)
// int port = m["inter_net_port"].asInt(); // 11883 // int port = m["inter_net_port"].asInt(); // 11883
g_conf.address = ip + ":" + std::to_string(port); g_conf.address = ip + ":" + std::to_string(port);
g_conf.client_id = "MINIBUS_CLIENT_PUB_GPS"; // 如需可改成 VID 等 g_conf.client_id = "sweeper_CLIENT_1532_GPS"; // 如需可改成 VID 等
g_conf.user = m["mqtt_user"].asString(); g_conf.user = m["mqtt_user"].asString();
g_conf.password = m["mqtt_password"].asString(); g_conf.password = m["mqtt_password"].asString();
g_conf.pub_gps_topic = m["pub_gps_topic"].asString(); g_conf.pub_gps_topic = m["pub_gps_topic"].asString();

42
src/radio_ctrl/src/radio_ctrl.cpp
View File

@ -141,27 +141,27 @@ private:
// 发布控制消息 // 发布控制消息
pub_->publish(msg); pub_->publish(msg);
// std::cout << "\n 刹车: " << (msg.brake ? "已刹车" : "未刹车") std::cout << "\n 刹车: " << (msg.brake ? "已刹车" : "未刹车")
// << "\n 挡位: "; << "\n 挡位: ";
// switch (msg.gear) switch (msg.gear)
// { {
// case 0: case 0:
// std::cout << "空挡"; std::cout << "空挡";
// break; break;
// case 2: case 2:
// std::cout << "前进挡"; std::cout << "前进挡";
// break; break;
// case 1: case 1:
// std::cout << "后退挡"; std::cout << "后退挡";
// break; break;
// default: default:
// std::cout << "未知挡位(" << static_cast<int>(msg.gear) << ")"; std::cout << "未知挡位(" << static_cast<int>(msg.gear) << ")";
// break; break;
// } }
// std::cout << "\n 行走电机转速: " << static_cast<int>(msg.rpm) << " RPM" std::cout << "\n 行走电机转速: " << static_cast<int>(msg.rpm) << " RPM"
// << "\n 轮端转向角度: " << msg.angle << "°" << "\n 轮端转向角度: " << msg.angle << "°"
// << "\n 清扫状态: " << (msg.sweep ? "正在清扫" : "未清扫") << "\n 清扫状态: " << (msg.sweep ? "正在清扫" : "未清扫")
// << std::endl; << std::endl;
} }
else else
{ {

2
src/rslidar_pointcloud_merger/launch/merge_two_lidars.launch.py
View File

@ -71,7 +71,7 @@ def generate_launch_description():
"filter_min_y": -5.0, # Y轴最小坐标（米） "filter_min_y": -5.0, # Y轴最小坐标（米）
"filter_max_y": 5.0, # Y轴最大坐标（米） "filter_max_y": 5.0, # Y轴最大坐标（米）
"filter_min_z": 0.0, # Z轴最小坐标（米） "filter_min_z": 0.0, # Z轴最小坐标（米）
"filter_max_z": 1.65, # Z轴最大坐标（米） "filter_max_z": 0.50, # Z轴最大坐标（米）1.65
"voxel_size": 0.1, # 体素网格大小（米） 0.1~0.3m "voxel_size": 0.1, # 体素网格大小（米） 0.1~0.3m
"stat_mean_k": 30, # 计算点的平均距离时考虑的邻居数 "stat_mean_k": 30, # 计算点的平均距离时考虑的邻居数
"stat_std_thresh": 1.5, # 标准差倍数阈值 噪声较多 ：0.5~1.0。 噪声较少 ：1.0~2.0。 "stat_std_thresh": 1.5, # 标准差倍数阈值 噪声较多 ：0.5~1.0。 噪声较少 ：1.0~2.0。

2
src/rtk/src/rtk_node.cpp
View File

@ -21,7 +21,7 @@ public:
// 初始化串口读取 // 初始化串口读取
boost_serial = new Boost_serial(); boost_serial = new Boost_serial();
boost_serial->init("/dev/ttyTHS1"); boost_serial->init("/dev/ttyTHS0");
// 创建发布者 // 创建发布者
command_publisher_ = this->create_publisher<sweeper_interfaces::msg::Rtk>("rtk_message", 10); command_publisher_ = this->create_publisher<sweeper_interfaces::msg::Rtk>("rtk_message", 10);

458
src/sub/src/sub_node.cpp Normal file
View File

@ -0,0 +1,458 @@
#include "MQTTClient.h"
#include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/sub.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
#include "json.h"
#include "sub_node.hpp"
#include <curl/curl.h>
#include "md5.h"
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <chrono>
#include <ctime>
#include <string.h>
#include <time.h>
#include <sstream>
#include <iomanip>
#include <pthread.h>
#include <cstdlib>
using namespace std;
// 全局变量声明
std::string command;
std::string sub_topic;
int get_route = 0;
int horn = 0;
int isDisconn = 0;
MQTTClient client;
MQTTClient_deliveryToken token_d_m;
MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer;
MQTTClient_message pubmsg_d_m = MQTTClient_message_initializer;
// mqtt相关
std::string mqtt_vid;
std::string mqtt_inter_net_address;
int mqtt_inter_net_port;
std::string mqtt_external_net_address;
int mqtt_external_net_port;
std::string mqtt_user;
std::string mqtt_password;
std::string mqtt_topic_remote;
// 新增：声明未定义的常量
std::string ADDRESS;
std::string CLIENTID_SUB = "sweeper200_sub"; // 客户端ID，提供默认值
std::chrono::steady_clock::time_point last_message_time = std::chrono::steady_clock::now();
constexpr auto QOS = 1;
constexpr auto TIMEOUT = 10000L;
volatile MQTTClient_deliveryToken deliveredtoken;
char sub_buff[500];
car_ctrl car_ctrl_mes;
void sendResponse(MQTTClient client, const std::string &topic, long long seqNo, int code, std::string msg)
{
// 使用JSON库构建响应数据
Json::Value responseData;
responseData["type"] = "response";
responseData["seqNo"] = static_cast<Json::Int64>(seqNo);
Json::Value data;
data["code"] = code;
data["msg"] = msg;
responseData["data"] = data;
// 使用JSON writer生成字符串
Json::FastWriter writer;
std::string responseJson = writer.write(responseData);
// 发布MQTT消息
pubmsg_d_m.payload = (void *)responseJson.c_str();
pubmsg_d_m.payloadlen = responseJson.length();
pubmsg_d_m.qos = QOS;
pubmsg_d_m.retained = 0;
MQTTClient_publishMessage(client, topic.c_str(), &pubmsg_d_m, &token_d_m);
MQTTClient_waitForCompletion(client, token_d_m, TIMEOUT);
printf("Message with delivery token %d delivered\n", token_d_m);
}
// 修正：将getSubTopic()函数移到使用它的mqtt_sub()函数之前
const char *getSubTopic()
{
return sub_topic.c_str();
}
void delivered(void *context, MQTTClient_deliveryToken dt)
{
(void)context;
printf("Message with token value %d delivery confirmed\n", dt);
deliveredtoken = dt;
}
int msgarrvd(void *context, char *topicName, int topicLen, MQTTClient_message *message)
{
(void)context;
(void)topicLen;
// printf("Message arrived\n");
last_message_time = std::chrono::steady_clock::now();
// printf("topic: %s\n", topicName);
// printf("message: %.*s\n", message->payloadlen, (char *)message->payload);
memset(sub_buff, 0, sizeof(sub_buff));
memcpy(&sub_buff, (char *)message->payload, message->payloadlen);
Json::Reader reader;
Json::Value root;
if (!reader.parse(sub_buff, root))
{
printf("recv json fail\n");
MQTTClient_freeMessage(&message);
MQTTClient_free(topicName);
return 1;
}
isDisconn = 0;
// std::cout << root << std::endl;
// Check type
if (!root.isMember("type") || root["type"].asString() != "request")
{
std::cout << "Message type is not 'request', ignoring.\n";
MQTTClient_freeMessage(&message);
MQTTClient_free(topicName);
return 1;
}
// Extract seqNo
if (!root.isMember("seqNo") || !root["seqNo"].isInt64())
{
std::cout << "Invalid or missing seqNo.\n";
MQTTClient_freeMessage(&message);
MQTTClient_free(topicName);
return 1;
}
long long seqNo = root["seqNo"].asInt64();
// Check data
if (!root.isMember("data") || !root["data"].isObject())
{
std::cout << "Invalid message: missing 'data' field.\n";
MQTTClient_freeMessage(&message);
MQTTClient_free(topicName);
return 1;
}
Json::Value data = root["data"];
std::string command = data["command"].asString();
int value = data["value"].asInt();
// Handle command
if (command == "mode")
car_ctrl_mes.mode = value;
else if (command == "gear")
car_ctrl_mes.gear = value;
else if (command == "drive")
{
std::string driveValues = data["value"].asString();
std::stringstream ss(driveValues);
std::vector<std::string> values;
std::string token;
while (std::getline(ss, token, ','))
{
values.push_back(token);
}
if (values.size() >= 3)
{
int throttle = std::stoi(values[0]);
int brake = std::stoi(values[1]);
int steering = std::stoi(values[2]);
steering = 32768 - steering;
car_ctrl_mes.throttle = throttle;
car_ctrl_mes.brake = brake;
car_ctrl_mes.steering = steering;
}
}
else if (command == "sweepCtrl")
car_ctrl_mes.sweepCtrl = value;
else if (command == "gather")
get_route = value;
// Generate response
// sendResponse(client, mqtt_topic_remote, seqNo, 200, "Success");
// sendResponse作为线程函数，发送响应
// 创建线程发送响应
std::thread responseThread(sendResponse, client, mqtt_topic_remote, seqNo, 200, "Success");
responseThread.detach();
MQTTClient_freeMessage(&message);
MQTTClient_free(topicName);
return 1;
}
void connlost(void *context, char *cause)
{
(void)context;
isDisconn = 1;
printf("\nConnection lost\n");
printf("cause: %s\n", cause);
}
void *mqtt_sub(void *arg)
{
(void)arg;
// 使用已定义的getSubTopic()函数
const char *SUB_TOPIC = getSubTopic();
int rc;
const char *username = mqtt_user.c_str();
const char *password = mqtt_password.c_str();
if ((rc = MQTTClient_create(&client, ADDRESS.c_str(), CLIENTID_SUB.c_str(), MQTTCLIENT_PERSISTENCE_NONE, NULL)) != MQTTCLIENT_SUCCESS)
{
printf("Failed to create client, return code %d\n", rc);
MQTTClient_destroy(&client);
return NULL;
}
if ((rc = MQTTClient_setCallbacks(client, NULL, connlost, msgarrvd, delivered)) != MQTTCLIENT_SUCCESS)
{
printf("Failed to set callbacks, return code %d\n", rc);
MQTTClient_destroy(&client);
return NULL;
}
conn_opts.keepAliveInterval = 20;
conn_opts.cleansession = 1;
conn_opts.username = username;
conn_opts.password = password;
if ((rc = MQTTClient_connect(client, &conn_opts)) != MQTTCLIENT_SUCCESS)
{
printf("Failed to connect, return code %d\n", rc);
MQTTClient_destroy(&client);
return NULL;
}
printf("Subscribing to topic %s\nfor client %s using QoS%d\n\n", SUB_TOPIC, CLIENTID_SUB.c_str(), QOS);
if ((rc = MQTTClient_subscribe(client, SUB_TOPIC, QOS)) != MQTTCLIENT_SUCCESS)
{
printf("Failed to subscribe, return code %d\n", rc);
MQTTClient_destroy(&client);
return NULL;
}
while (1)
{
usleep(100);
// 检查连接状态，如果连接丢失，则尝试重新连接
if (MQTTClient_isConnected(client) == 0)
{
printf("MQTT connection lost, trying to reconnect...\n");
if ((rc = MQTTClient_connect(client, &conn_opts)) != MQTTCLIENT_SUCCESS)
{
isDisconn = 1;
printf("Failed to reconnect, return code %d\n", rc);
// 处理连接失败的情况，例如等待一段时间后再次尝试
}
else
{
isDisconn = 0;
printf("Reconnected to MQTT server.\n");
// 重新订阅主题
if ((rc = MQTTClient_subscribe(client, SUB_TOPIC, QOS)) != MQTTCLIENT_SUCCESS)
{
printf("Failed to resubscribe, return code %d\n", rc);
}
}
}
else
{
isDisconn = 0;
auto current_time = std::chrono::steady_clock::now();
if (std::chrono::duration_cast<std::chrono::milliseconds>(current_time - last_message_time).count() > 500)
{
isDisconn = 1;
// printf("Heartbeat timeout: No message received in 500ms.\n");
// 执行心跳超时的处理逻辑
}
}
}
if ((rc = MQTTClient_unsubscribe(client, SUB_TOPIC)) != MQTTCLIENT_SUCCESS)
{
printf("Failed to unsubscribe, return code %d\n", rc);
MQTTClient_destroy(&client);
return NULL;
}
if ((rc = MQTTClient_disconnect(client, 10000)) != MQTTCLIENT_SUCCESS)
{
printf("Failed to disconnect, return code %d\n", rc);
MQTTClient_destroy(&client);
return NULL;
}
MQTTClient_destroy(&client);
return NULL;
}
class sub_node : public rclcpp::Node
{
public:
// 构造函数,有一个参数为节点名称
sub_node(std::string name) : Node(name)
{
RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str());
timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&sub_node::timer_callback, this));
// 发布控制指令消息的发布器
pub_mc = this->create_publisher<sweeper_interfaces::msg::McCtrl>("remote_mc_ctrl", 10);
pub_gather = this->create_publisher<sweeper_interfaces::msg::Sub>("gather", 10);
}
private:
void timer_callback()
{
sweeper_interfaces::msg::Sub message;
message.get_route = get_route;
pub_gather->publish(message);
sweeper_interfaces::msg::McCtrl msg;
// if (isDisconn == 0)
{
// mcu 部分
if (car_ctrl_mes.mode == 3) // 台架端远程驾驶模式
{
msg.brake = 0;
if (car_ctrl_mes.gear == 0) // N档
msg.gear = 0;
else if (car_ctrl_mes.gear == 1) // D档
msg.gear = 2;
else if (car_ctrl_mes.gear == 2) // R档
msg.gear = 1;
msg.rpm = car_ctrl_mes.throttle / 65535 * 1000;
// 转向
// 0~32200 -> -40~0 33200~65535 -> 0~40
int steering = 0;
if (car_ctrl_mes.steering < 32200)
{
const int originalWidth1 = 32200;
const int targetWidth1 = 40;
double unitLength1 = targetWidth1 / originalWidth1;
steering = -40 + static_cast<int>(car_ctrl_mes.steering * unitLength1);
}
else if (car_ctrl_mes.steering > 33200)
{
const int originalWidth2 = 32355; // 注意这里是从 33200 到 65535，共计 32355 个数
const int targetWidth2 = 40;
double unitLength2 = targetWidth2 / originalWidth2;
steering = static_cast<int>((car_ctrl_mes.steering - 32200) * unitLength2);
}
else
{
steering = 0; // 朝向前方
}
msg.angle = steering;
msg.angle_speed = 120;
}
else
{
msg.brake = 1;
msg.gear = 0;
msg.rpm = 0;
// eps部分
msg.angle = 0;
msg.angle_speed = 120;
}
msg.sweep = car_ctrl_mes.sweepCtrl;
pub_mc->publish(msg);
// RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:" << "\n 档位: " << [&]()
// {
// switch(msg.gear) {
// case 0: return "空挡";
// case 1: return "后退";
// case 2: return "前进";
// default: return "未知档位"; // 添加默认分支
// } }() << "\n 油门转速: " << msg.rpm << " rpm"
// << "\n 转向角度: " << msg.angle << "°"
// << "\n 刹车: " << (msg.brake == 0 ? "开(释放)" : "关(刹住)") << "\n 清扫: " << (msg.sweep ? "开启清扫" : "关闭清扫"));
}
}
// 声明定时器指针
rclcpp::TimerBase::SharedPtr timer_;
// 声明话题发布者指针
rclcpp::Publisher<sweeper_interfaces::msg::Sub>::SharedPtr pub_gather;
rclcpp::Publisher<sweeper_interfaces::msg::McCtrl>::SharedPtr pub_mc;
};
void init_main()
{
Json::Reader reader;
Json::Value root;
std::ifstream in("config.json", std::ios::binary);
if (!in.is_open())
{
std::cout << "read config file error" << std::endl;
return;
}
if (reader.parse(in, root))
{
mqtt_vid = root["mqtt"]["vid"].asString();
mqtt_inter_net_address = root["mqtt"]["inter_net_address"].asString();
mqtt_inter_net_port = root["mqtt"]["inter_net_port"].asInt();
mqtt_external_net_address = root["mqtt"]["external_net_address"].asString();
mqtt_external_net_port = root["mqtt"]["external_net_port"].asInt();
mqtt_user = root["mqtt"]["mqtt_user"].asString();
mqtt_password = root["mqtt"]["mqtt_password"].asString();
mqtt_topic_remote = root["mqtt"]["remote_topic"].asString();
// 添加对sub_topic的初始化
sub_topic = mqtt_topic_remote;
CLIENTID_SUB = CLIENTID_SUB + "_" + mqtt_vid;
// ADDRESS = mqtt_inter_net_address + ":" + std::to_string(mqtt_inter_net_port);
ADDRESS = mqtt_external_net_address + ":" + std::to_string(mqtt_external_net_port);
cout << "ADDRESS: " << ADDRESS << endl;
cout << "CLIENTID_SUB: " << CLIENTID_SUB << endl;
cout << "mqtt_vid: " << mqtt_vid << endl;
}
in.close(); // 关闭文件流
}
pthread_t mqtt_sub_thread_t;
int main(int argc, char **argv)
{
init_main();
memset(&car_ctrl_mes, 0, sizeof(car_ctrl_mes));
car_ctrl_mes.steering = 32767;
pthread_create(&mqtt_sub_thread_t, NULL, mqtt_sub, NULL);
rclcpp::init(argc, argv);
/*创建对应节点的共享指针对象*/
auto node = std::make_shared<sub_node>("sub_node");
/* 运行节点，并检测退出信号*/
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

2
src/task_manager/src/task_manager_node.cpp
View File

@ -35,7 +35,7 @@ std::string sub_topic;
// 修复变量声明和初始化 // 修复变量声明和初始化
std::string ADDRESS; std::string ADDRESS;
std::string CLIENTID_SUB = "sweeper_CLIENT_200"; // 客户端ID std::string CLIENTID_SUB = "sweeper200_task"; // 客户端ID
std::string destinationFilePath1 = "./gps_load_now.txt"; std::string destinationFilePath1 = "./gps_load_now.txt";
std::chrono::steady_clock::time_point last_message_time = std::chrono::steady_clock::now(); std::chrono::steady_clock::time_point last_message_time = std::chrono::steady_clock::now();
constexpr auto QOS = 0; constexpr auto QOS = 0;