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

更新系统核心功能和配置文件

Browse Source 
主要更新内容：
- 更新主配置文件 config.json
- 优化启动脚本：prepare.sh、run.sh、start_all.sh
- 新增GPS加载文件 gps_load_now.txt 和路由文件
- 功能单元(fu)：更新参数配置和节点逻辑
- 主控(mc)：改进定时任务处理
- MQTT报告：更新客户端配置和通信逻辑
- 路径规划(pl)：优化节点功能
- GPS发布：增强数据发布模块
- 无线电控制：改进控制逻辑
- 远程控制：优化节点处理
- 雷达点云合并：更新启动配置
- RTK定位：改进节点功能
- 订阅模块：优化数据订阅逻辑
- 任务管理器：增强任务管理功能
This commit is contained in:
lyq 2025-10-25 10:39:02 +08:00
parent 27d93c009e
commit 25f6914b41
21 changed files with 176 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
config.json
View File

@ -1,6 +1,8 @@
{
"mqtt": {
"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_port": 11883,
"external_net_address": "tcp://36.153.162.171",
@ -8,6 +10,20 @@
"mqtt_user": "zxwl",
"mqtt_password": "zxwl1234@",
"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 chmod 777 /dev/ttyUSB0
sudo chmod 777 /dev/ttyTHS1
sudo chmod 777 /dev/ttyTHS0
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
{
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"
}&

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

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

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

@ -648,9 +648,11 @@ private:
x = msg->x;
y = msg->y;
speed = (int)msg->speed; // 获取PL消息中的速度
std::cout << "pl speed " << speed << std::endl;
reliability = msg->reliability;
located = msg->enter_range;
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)
@ -677,13 +679,13 @@ private:
message.sweep = (avoid_ctx_.state == AVOID_NONE); // 避障状态为无避障 且 任务开始 才开启清扫
message.gear = 0;
message.brake = (publish_speed == 0) ? 1 : 0;
message.rpm = publish_speed;
message.rpm = publish_speed * 25;
message.angle = publish_angle; // 负数表示左转，正数表示右转
message.angle_speed = 800;
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_){
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.setBrake(msg.brake);
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 控制
@ -22,6 +26,7 @@ void epsTimerTask(CANDriver &canctl)
eps_cmd.setAngularSpeed(msg.angle_speed);
eps_cmd.pack();
canctl.sendFrame(eps_cmd.toFrame());
// std::cout << "epsTimerTask" << std::endl;
}
// 定时器回调：VCU 扫地控制
@ -39,6 +44,7 @@ void vcuTimerTask(CANDriver &canctl)
// 发送CAN使能指令
vcu_enable_cmd.setCANEnable(true);
canctl.sendFrame(vcu_enable_cmd.toFrame());
// std::cout << "mcuTimerTask" << std::endl;
vcu_initialized = true;
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.setByte7(target_value);
canctl.sendFrame(vcu_motor1_cmd.toFrame());
// std::cout << "vcuTimerTask1" << std::endl;
// ===== 控制0x212报文 (电机M8和LED输出) =====
vcu_motor2_cmd.setByte0(target_value);
@ -72,9 +79,10 @@ void vcuTimerTask(CANDriver &canctl)
vcu_motor2_cmd.setByte6(target_value);
vcu_motor2_cmd.setByte7(target_value);
canctl.sendFrame(vcu_motor2_cmd.toFrame());
// std::cout << "vcuTimerTask2" << std::endl;
RCLCPP_INFO(rclcpp::get_logger("timer_tasks"),
"[VCU] Sweep mode %s", msg.sweep ? "activated" : "deactivated");
// RCLCPP_INFO(rclcpp::get_logger("timer_tasks"),
// "[VCU] Sweep mode %s", msg.sweep ? "activated" : "deactivated");
// 更新状态记录
last_sweep_state = msg.sweep;

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

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

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

@ -1,8 +1,8 @@
{
"vid": "V060003",
"mqtt": {
"ip": "192.168.4.196",
"port": 11883,
"ip": "36.153.162.171",
"port": 19683,
"info_topic": "/zxwl/vehicle/{vid}/info",
"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;
conn_opts.keepAliveInterval = 20;
conn_opts.cleansession = 1;
if (!username_.empty())
{
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_)
{

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.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;
break;
}
@ -63,7 +63,7 @@ void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg)
float gear_ratio = 32.0; // 电机速比
float wheel_radius = 0.3; // 轮胎直径(m)
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]：高字节）
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;
// std::cout << "[0x0CF11F00] controllerTemp : " << info_report.controllerTemp << std::endl;
// std::cout << "[0x0CF11F00] motorTemp : " << info_report.motorTemp << std::endl;
// std::cout << "[0x0CF11F00] gear : " << info_report.gear << std::endl;
std::cout << "[0x0CF11F00] controllerTemp : " << info_report.controllerTemp << std::endl;
std::cout << "[0x0CF11F00] motorTemp : " << info_report.motorTemp << std::endl;
std::cout << "[0x0CF11F00] gear : " << info_report.gear << std::endl;
break;
}
@ -123,7 +123,7 @@ void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg)
// 第3、4字节拼接为原始角度数据
uint16_t raw_value = static_cast<uint16_t>((msg->data[3] << 8) | msg->data[4]);
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 错误码
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_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");
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
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.password = m["mqtt_password"].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);
// std::cout << "\n 刹车: " << (msg.brake ? "已刹车" : "未刹车")
// << "\n 挡位: ";
// switch (msg.gear)
// {
// case 0:
// std::cout << "空挡";
// break;
// case 2:
// std::cout << "前进挡";
// break;
// case 1:
// std::cout << "后退挡";
// break;
// default:
// std::cout << "未知挡位(" << static_cast<int>(msg.gear) << ")";
// break;
// }
// std::cout << "\n 行走电机转速: " << static_cast<int>(msg.rpm) << " RPM"
// << "\n 轮端转向角度: " << msg.angle << "°"
// << "\n 清扫状态: " << (msg.sweep ? "正在清扫" : "未清扫")
// << std::endl;
std::cout << "\n 刹车: " << (msg.brake ? "已刹车" : "未刹车")
<< "\n 挡位: ";
switch (msg.gear)
{
case 0:
std::cout << "空挡";
break;
case 2:
std::cout << "前进挡";
break;
case 1:
std::cout << "后退挡";
break;
default:
std::cout << "未知挡位(" << static_cast<int>(msg.gear) << ")";
break;
}
std::cout << "\n 行走电机转速: " << static_cast<int>(msg.rpm) << " RPM"
<< "\n 轮端转向角度: " << msg.angle << "°"
<< "\n 清扫状态: " << (msg.sweep ? "正在清扫" : "未清扫")
<< std::endl;
}
else
{

8
src/remote/src/remote_node.cpp
View File

@ -24,8 +24,8 @@ Json::Value data;
std::string command;
std::string remote_topic;
int time_cunt = 0;
// constexpr auto ADDRESS = "tcp://36.153.162.171:19583"; // 更改此处地址
constexpr auto ADDRESS = "tcp://192.168.4.196:11883"; // 更改此处地址
constexpr auto ADDRESS = "tcp://36.153.162.171:19683"; // 更改此处地址
// constexpr auto ADDRESS = "tcp://192.168.4.196:11883"; // 更改此处地址
constexpr auto CLIENTID_REMOTE = "SWEEPER_CLIENT_REMOTE"; // 更改此处客户端ID
const char *getRemoteTopic()
@ -120,8 +120,8 @@ void *mqtt_remote(void *arg)
const char *REMOTE_TOPIC = getRemoteTopic();
int rc;
char *username = NULL;
char *password = NULL;
char *username = "zxwl";
char *password = "zxwl1234@";
if ((rc = MQTTClient_create(&client, ADDRESS, CLIENTID_REMOTE, MQTTCLIENT_PERSISTENCE_NONE, NULL)) != MQTTCLIENT_SUCCESS)
{

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_max_y": 5.0, # Y轴最大坐标（米）
"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
"stat_mean_k": 30, # 计算点的平均距离时考虑的邻居数
"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->init("/dev/ttyTHS1");
boost_serial->init("/dev/ttyTHS0");
// 创建发布者
command_publisher_ = this->create_publisher<sweeper_interfaces::msg::Rtk>("rtk_message", 10);

6
src/sub/src/sub_node.cpp
View File

@ -43,7 +43,7 @@ std::string mqtt_topic_remote;
// 新增：声明未定义的常量
std::string ADDRESS;
std::string CLIENTID_SUB = "sweeper_whu_CLIENT_1"; // 客户端ID，提供默认值
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;
@ -425,10 +425,10 @@ void init_main()
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"]["mqtt_topic_remote"].asString();
mqtt_topic_remote = root["mqtt"]["remote_topic"].asString();
// 添加对sub_topic的初始化
sub_topic = mqtt_topic_remote;
CLIENTID_SUB = CLIENTID_SUB + "_" + mqtt_vid + "_sub";
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;

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

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

4
start_all.sh
View File

@ -5,7 +5,7 @@ set -e
source /opt/ros/foxy/setup.bash
# 加载你的工作空间环境
source ~/sweeper_whu/install/setup.bash
source ~/Downloads/sweeper_whu/install/setup.bash
# 启动节点
ros2 run radio_ctrl radio_ctrl_node &
@ -16,7 +16,7 @@ ros2 run rtk rtk_node &
ros2 run pub_gps pub_gps_node &
ros2 launch rslidar_sdk start_double.launch.py &
ros2 run route route_node &
ros2 run sub sub_node &
ros2 run remote remote_node &
ros2 run task_manager task_manager_node &
ros2 run pl pl_node &
ros2 launch fu fu.launch.py &