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

主要更新内容：
- 更新主配置文件 config.json
- 优化启动脚本：prepare.sh、run.sh、start_all.sh
- 新增GPS加载文件 gps_load_now.txt 和路由文件
- 功能单元(fu)：更新参数配置和节点逻辑
- 主控(mc)：改进定时任务处理
- MQTT报告：更新客户端配置和通信逻辑
- 路径规划(pl)：优化节点功能
- GPS发布：增强数据发布模块
- 无线电控制：改进控制逻辑
- 远程控制：优化节点处理
- 雷达点云合并：更新启动配置
- RTK定位：改进节点功能
- 订阅模块：优化数据订阅逻辑
- 任务管理器：增强任务管理功能

config.json

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

gps_load_now.txt

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

prepare.sh

@@ -2,5 +2,6 @@
 
 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

routes/gps_load_1761208243532.txt

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

run.sh

@@ -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" 
 }&

src/fu/config/fu_params.yaml

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

src/fu/src/fu_node.cpp

@@ -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 "无避障";

src/mc/src/timer_tasks.cpp

@@ -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"),
+    // 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;

src/mqtt_report/config/config(copy).json

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

src/mqtt_report/config/config.json

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

src/mqtt_report/include/mqtt_report/mqtt_client.hpp

@@ -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_)
         {

src/mqtt_report/src/mqtt_report.cpp

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

src/pl/src/pl_node.cpp

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

src/pub_gps/src/pub_gps_node.cpp

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

src/radio_ctrl/src/radio_ctrl.cpp

@@ -141,27 +141,27 @@ private:
             // 发布控制消息
             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
         {

src/remote/src/remote_node.cpp

@@ -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://36.153.162.171:19683"; // 更改此处地址
-constexpr auto ADDRESS = "tcp://192.168.4.196:11883";     // 更改此处地址
+// 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 *username = "zxwl";
-    char *password = NULL;
+    char *password = "zxwl1234@";
 
     if ((rc = MQTTClient_create(&client, ADDRESS, CLIENTID_REMOTE, MQTTCLIENT_PERSISTENCE_NONE, NULL)) != MQTTCLIENT_SUCCESS)
     {

src/rslidar_pointcloud_merger/launch/merge_two_lidars.launch.py

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

src/rtk/src/rtk_node.cpp

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

src/sub/src/sub_node.cpp

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

src/task_manager/src/task_manager_node.cpp

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

start_all.sh

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