diff --git a/config.json b/config.json index 3a3d229..64b8628 100644 --- a/config.json +++ b/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 } } \ No newline at end of file diff --git a/gps_load_now.txt b/gps_load_now.txt new file mode 100644 index 0000000..d5ec464 --- /dev/null +++ b/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 diff --git a/prepare.sh b/prepare.sh index 50fa184..ab37512 100755 --- a/prepare.sh +++ b/prepare.sh @@ -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 \ No newline at end of file diff --git a/routes/gps_load_1761208243532.txt b/routes/gps_load_1761208243532.txt new file mode 100644 index 0000000..d5ec464 --- /dev/null +++ b/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 diff --git a/run.sh b/run.sh index cf4b0b7..526443f 100755 --- a/run.sh +++ b/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" }& diff --git a/src/fu/config/fu_params.yaml b/src/fu/config/fu_params.yaml index bae0448..95dfd16 100644 --- a/src/fu/config/fu_params.yaml +++ b/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 diff --git a/src/fu/src/fu_node.cpp b/src/fu/src/fu_node.cpp index fde8a4f..b885f77 100644 --- a/src/fu/src/fu_node.cpp +++ b/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 "无避障"; diff --git a/src/mc/src/timer_tasks.cpp b/src/mc/src/timer_tasks.cpp index 38fcfc2..19585bc 100644 --- a/src/mc/src/timer_tasks.cpp +++ b/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"), - "[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; diff --git a/src/mqtt_report/config/config(copy).json b/src/mqtt_report/config/config(copy).json index 531eb75..72bfaee 100644 --- a/src/mqtt_report/config/config(copy).json +++ b/src/mqtt_report/config/config(copy).json @@ -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" } diff --git a/src/mqtt_report/config/config.json b/src/mqtt_report/config/config.json index 72bfaee..531eb75 100644 --- a/src/mqtt_report/config/config.json +++ b/src/mqtt_report/config/config.json @@ -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" } diff --git a/src/mqtt_report/include/mqtt_report/mqtt_client.hpp b/src/mqtt_report/include/mqtt_report/mqtt_client.hpp index cac5667..e8cbc6b 100644 --- a/src/mqtt_report/include/mqtt_report/mqtt_client.hpp +++ b/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_) { diff --git a/src/mqtt_report/src/mqtt_report.cpp b/src/mqtt_report/src/mqtt_report.cpp index 67b5b82..ecbd8a6 100644 --- a/src/mqtt_report/src/mqtt_report.cpp +++ b/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(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((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) diff --git a/src/pl/src/pl_node.cpp b/src/pl/src/pl_node.cpp index 1ea27af..a4a8ad2 100644 --- a/src/pl/src/pl_node.cpp +++ b/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); diff --git a/src/pub_gps/src/pub_gps_node.cpp b/src/pub_gps/src/pub_gps_node.cpp index f2bd443..2e6d707 100644 --- a/src/pub_gps/src/pub_gps_node.cpp +++ b/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(); diff --git a/src/radio_ctrl/src/radio_ctrl.cpp b/src/radio_ctrl/src/radio_ctrl.cpp index 4ac965e..7513372 100644 --- a/src/radio_ctrl/src/radio_ctrl.cpp +++ b/src/radio_ctrl/src/radio_ctrl.cpp @@ -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(msg.gear) << ")"; - // break; - // } - // std::cout << "\n 行走电机转速: " << static_cast(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(msg.gear) << ")"; + break; + } + std::cout << "\n 行走电机转速: " << static_cast(msg.rpm) << " RPM" + << "\n 轮端转向角度: " << msg.angle << "°" + << "\n 清扫状态: " << (msg.sweep ? "正在清扫" : "未清扫") + << std::endl; } else { diff --git a/src/remote/src/remote_node.cpp b/src/remote/src/remote_node.cpp index 2d70fcf..97be598 100644 --- a/src/remote/src/remote_node.cpp +++ b/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://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) { diff --git a/src/rslidar_pointcloud_merger/launch/merge_two_lidars.launch.py b/src/rslidar_pointcloud_merger/launch/merge_two_lidars.launch.py index 88fd1c7..b3f3ff2 100644 --- a/src/rslidar_pointcloud_merger/launch/merge_two_lidars.launch.py +++ b/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。 diff --git a/src/rtk/src/rtk_node.cpp b/src/rtk/src/rtk_node.cpp index 8fcfe06..1410f8d 100644 --- a/src/rtk/src/rtk_node.cpp +++ b/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("rtk_message", 10); diff --git a/src/sub/src/sub_node.cpp b/src/sub/src/sub_node.cpp index 54476ee..8e2f99b 100644 --- a/src/sub/src/sub_node.cpp +++ b/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; diff --git a/src/task_manager/src/task_manager_node.cpp b/src/task_manager/src/task_manager_node.cpp index 5365e12..c776613 100644 --- a/src/task_manager/src/task_manager_node.cpp +++ b/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; diff --git a/start_all.sh b/start_all.sh index 1f99e43..aece40e 100755 --- a/start_all.sh +++ b/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 &