diff --git a/config.json b/config.json index b199dea..3a3d229 100644 --- a/config.json +++ b/config.json @@ -1,13 +1,13 @@ { "mqtt": { - "vid": "V060002", + "vid": "V060003", "inter_net_address": "tcp://192.168.4.196", "inter_net_port": 11883, "external_net_address": "tcp://36.153.162.171", "external_net_port": 19683, "mqtt_user": "zxwl", "mqtt_password": "zxwl1234@", - "pub_gps_topic": "/zxwl/vehicle/V060002/gps", - "remote_topic": "/zxwl/vehicle/V060002/ctrl" + "pub_gps_topic": "/zxwl/vehicle/V060003/gps", + "remote_topic": "/zxwl/vehicle/V060003/ctrl" } } \ No newline at end of file diff --git a/ros-foxy-pcl-conversions_2.2.1-1focal.20230606.034520_arm64.deb b/ros-foxy-pcl-conversions_2.2.1-1focal.20230606.034520_arm64.deb new file mode 100644 index 0000000..e395b20 Binary files /dev/null and b/ros-foxy-pcl-conversions_2.2.1-1focal.20230606.034520_arm64.deb differ diff --git a/run.sh b/run.sh index 972e2d9..cf4b0b7 100755 --- a/run.sh +++ b/run.sh @@ -8,24 +8,42 @@ sleep 0.2s { gnome-terminal --title="mc" --tab "XXD_ros" -- bash -c "ros2 run mc mc_node" -} & +}& sleep 0.2s { gnome-terminal --title="ctrl_arbiter" --tab "XXD_ros" -- bash -c "ros2 run ctrl_arbiter ctrl_arbiter_node" -} & +}& 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="rtk" --tab "XXD_ros" -- bash -c "ros2 run rtk rtk_node" -# }& -# sleep 0.2s - -# { -# gnome-terminal --title="pub_gps" --tab "XXD_ros" -- bash -c "ros2 run pub_gps pub_gps_node" -# } +{ + gnome-terminal --title="lidar" --tab "XXD_ros" -- bash -c "ros2 launch rslidar_sdk start_double.launch.py" +}& +sleep 0.1s +{ + gnome-terminal --title="rtk" --tab "XXD_ros" -- bash -c "ros2 run rtk rtk_node" +}& +sleep 0.1s +{ + gnome-terminal --title="route" --tab "XXD_ros" -- bash -c "ros2 run route route_node" +}& +sleep 0.1s +{ + gnome-terminal --title="sub" --tab "XXD_ros" -- bash -c "ros2 run sub sub_node" +}& +sleep 0.1s +{ + gnome-terminal --title="task_manager" --tab "XXD_ros" -- bash -c "ros2 run task_manager task_manager_node" +}& +sleep 0.1s +{ + gnome-terminal --title="pl" --tab "XXD_ros" -- bash -c "ros2 run pl pl_node" +}& +sleep 0.1s +{ + gnome-terminal --title="fu" --tab "XXD_ros" -- bash -c "ros2 launch fu fu.launch.py" +} +sleep 0.1s +{ + gnome-terminal --title="pub_gps" --tab "XXD_ros" -- bash -c "ros2 run pub_gps pub_gps_node" +} \ No newline at end of file diff --git a/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfig.cmake b/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfig.cmake index 04a58e5..f875d31 100644 --- a/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfig.cmake +++ b/src/airy/rslidar_sdk-main/src/rs_driver/cmake/rs_driverConfig.cmake @@ -19,8 +19,8 @@ if(${ENABLE_TRANSFORM}) add_definitions("-DENABLE_TRANSFORM") endif(${ENABLE_TRANSFORM}) -set(rs_driver_INCLUDE_DIRS "/home/alvin/project/zxwl/sweeper/200_whu/sweeper_whu/src/airy/rslidar_sdk-main/src/rs_driver/src;/usr/local/rslidar_sdk/include") -set(RS_DRIVER_INCLUDE_DIRS "/home/alvin/project/zxwl/sweeper/200_whu/sweeper_whu/src/airy/rslidar_sdk-main/src/rs_driver/src;/usr/local/rslidar_sdk/include") +set(rs_driver_INCLUDE_DIRS "/home/aiec/sweeper_whu/src/airy/rslidar_sdk-main/src/rs_driver/src;/usr/local/rslidar_sdk/include") +set(RS_DRIVER_INCLUDE_DIRS "/home/aiec/sweeper_whu/src/airy/rslidar_sdk-main/src/rs_driver/src;/usr/local/rslidar_sdk/include") set(rs_driver_LIBRARIES "pthread;pcap") set(RS_DRIVER_LIBRARIES "pthread;pcap") diff --git a/src/fu/CMakeLists.txt b/src/fu/CMakeLists.txt index 8edeb60..5cad2ad 100644 --- a/src/fu/CMakeLists.txt +++ b/src/fu/CMakeLists.txt @@ -8,7 +8,7 @@ endif() # Default to C++14 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/src/fu/config/fu_params.yaml b/src/fu/config/fu_params.yaml index 758cb04..bae0448 100644 --- a/src/fu/config/fu_params.yaml +++ b/src/fu/config/fu_params.yaml @@ -4,7 +4,7 @@ FU: enable_obstacle_stop: True # 绕障功能 False or True - enable_obstacle_avoid: True + enable_obstacle_avoid: False # 可视化栅格 False or True enable_visualize_grid: False diff --git a/src/fu/src/fu_node.cpp b/src/fu/src/fu_node.cpp index 200fb19..fde8a4f 100644 --- a/src/fu/src/fu_node.cpp +++ b/src/fu/src/fu_node.cpp @@ -674,12 +674,7 @@ private: // 发布控制指令 sweeper_interfaces::msg::McCtrl message; - bool sweep = (avoid_ctx_.state == AVOID_NONE); // 避障状态为无避障 且 任务开始 才开启清扫 - message.edge_brush_lift = sweep; - message.sweep_ctrl = sweep; - message.spray = sweep; - message.mud_flap = sweep; - message.dust_shake = sweep; + message.sweep = (avoid_ctx_.state == AVOID_NONE); // 避障状态为无避障 且 任务开始 才开启清扫 message.gear = 0; message.brake = (publish_speed == 0) ? 1 : 0; message.rpm = publish_speed; diff --git a/src/sub/include/sub/sub_node.hpp b/src/sub/include/sub/sub_node.hpp index cafb4d5..fbf8002 100644 --- a/src/sub/include/sub/sub_node.hpp +++ b/src/sub/include/sub/sub_node.hpp @@ -8,40 +8,11 @@ struct car_ctrl // 1. 使能模式 int mode; // 0:手动驾驶, 3:远程驾驶 - // 2. 挡位 - int gear; // 0:P档, 1:D档, 2:R档 - - // 3. 油门，制动，转向 - int throttle; // 0~65535，油门，松开为0，踩死为65535 - int brake; // 0~65535，刹车，松开为0，踩死为65535 - int steering; // 0~65535，转向，左打死为0，右打死为65535 - - // 4. 转向灯 - int turnLight; // 0:关, 1:左转, 2:右转 - - // 5. 大灯 - int headLight; // 0:关, 1:开 - - // 6. 主刷推杆 - int mainBrush; // 0:关, 1:开 - - // 7. 扫地控制 - int sweepCtrl; // 0:关, 1:开 - - // 8. 边刷推杆 - int edgeBrush; // 0:关, 1:开 - - // 9. 挡皮控制 - int flap; // 0:关, 1:开 - - // 10. 振尘控制 - int dustShaking; // 0:关, 1:开 - - // 12. 水泵 - int waterPump; // 0:关, 1:开 - - // 13. 倒灰 - int dump; // 0:全部收回, 1:斗升, 2:倒斗 + int gear; // 挡位 0：N档，1：D档，2：R档 + int throttle; // 油门 0~65535 + int steering; // 转向 0~65535 + int brake; // 刹车 0~65535 + int sweepCtrl; // 清扫 1:清扫 0:不清扫 }; extern car_ctrl car_ctrl_mes; diff --git a/src/sub/src/sub_node.cpp b/src/sub/src/sub_node.cpp index 5586cd9..54476ee 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_CLIENT_800"; // 客户端ID，提供默认值 +std::string CLIENTID_SUB = "sweeper_whu_CLIENT_1"; // 客户端ID，提供默认值 std::chrono::steady_clock::time_point last_message_time = std::chrono::steady_clock::now(); constexpr auto QOS = 1; @@ -158,10 +158,6 @@ int msgarrvd(void *context, char *topicName, int topicLen, MQTTClient_message *m car_ctrl_mes.mode = value; else if (command == "gear") car_ctrl_mes.gear = value; - else if (command == "turnLight") - car_ctrl_mes.turnLight = value; - else if (command == "headLight") - car_ctrl_mes.headLight = value; else if (command == "drive") { std::string driveValues = data["value"].asString(); @@ -178,28 +174,13 @@ int msgarrvd(void *context, char *topicName, int topicLen, MQTTClient_message *m int brake = std::stoi(values[1]); int steering = std::stoi(values[2]); steering = 32768 - steering; - car_ctrl_mes.throttle = throttle / 65535.0 * 50; + car_ctrl_mes.throttle = throttle; car_ctrl_mes.brake = brake; - car_ctrl_mes.steering = steering / 32768.0 * 40; + car_ctrl_mes.steering = steering; } } - - // ===================== 使用 mainBrush 代 sweepCtrl，之后小程序适配了改回来 ===================== else if (command == "sweepCtrl") - // else if (command == "mainBrush") car_ctrl_mes.sweepCtrl = value; - // ===================== 使用 mainBrush 代 sweepCtrl，之后小程序适配了改回来 ===================== - - else if (command == "edgeBrush") - car_ctrl_mes.edgeBrush = value; - else if (command == "flap") - car_ctrl_mes.flap = value; - else if (command == "dustShaking") - car_ctrl_mes.dustShaking = value; - else if (command == "waterPump") - car_ctrl_mes.waterPump = value; - else if (command == "dump") - car_ctrl_mes.dump = value; else if (command == "gather") get_route = value; @@ -352,16 +333,42 @@ private: if (car_ctrl_mes.mode == 3) // 台架端远程驾驶模式 { msg.brake = 0; - msg.gear = car_ctrl_mes.gear - 1; - msg.rpm = car_ctrl_mes.throttle; - // ehb部分 - float brake_pressure = car_ctrl_mes.brake / 65535.0 * 8; - msg.ehb_anable = car_ctrl_mes.brake > 100 ? 1 : 0; - msg.ehb_brake_pressure = brake_pressure; + 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; - // eps部分 - msg.angle = car_ctrl_mes.steering; + 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(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((car_ctrl_mes.steering - 32200) * unitLength2); + } + else + { + steering = 0; // 朝向前方 + } + msg.angle = steering; msg.angle_speed = 120; } else @@ -370,53 +377,25 @@ private: msg.gear = 0; msg.rpm = 0; - // ehb部分 - msg.ehb_anable = 1; - msg.ehb_brake_pressure = 8; - // eps部分 msg.angle = 0; msg.angle_speed = 120; } - // vcu部分 - // 非0即true 0:关, 1:开 - msg.edge_brush_lift = (car_ctrl_mes.sweepCtrl != 0); // 和sweepCtrl<一键清扫>协同作业 - msg.sweep_ctrl = (car_ctrl_mes.sweepCtrl != 0); - msg.spray = (car_ctrl_mes.waterPump != 0); - msg.mud_flap = (car_ctrl_mes.flap != 0); - msg.dust_shake = (car_ctrl_mes.dustShaking != 0); - - msg.left_light = (car_ctrl_mes.turnLight == 1); // 1:左转时左灯亮 - msg.right_light = (car_ctrl_mes.turnLight == 2); // 2:右转时右灯亮 - msg.brake_light = msg.ehb_anable; // 0:关, 1:开 - msg.headlight = car_ctrl_mes.headLight; // 0:关, 1:开 - - // 倒灰 - // 0:全部收回, 1:斗升, 2:倒斗 - msg.dump = car_ctrl_mes.dump; + msg.sweep = car_ctrl_mes.sweepCtrl; pub_mc->publish(msg); - RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:" - << "\n brake: " << static_cast(msg.brake) // uint8 - << "\n gear: " << static_cast(msg.gear) // uint8 - << "\n rpm: " << static_cast(msg.rpm) // uint8 - << "\n ehb_enable: " << static_cast(msg.ehb_anable) // bool - << "\n ehb_brake_pressure: " << static_cast(msg.ehb_brake_pressure) // float32 - << "\n angle: " << msg.angle // float32 - << "\n angle_speed: " << msg.angle_speed // uint16 - << "\n edge_brush_lift: " << static_cast(msg.edge_brush_lift) // bool - << "\n sweep_ctrl: " << static_cast(msg.sweep_ctrl) // bool - << "\n spray: " << static_cast(msg.spray) // bool - << "\n mud_flap: " << static_cast(msg.mud_flap) // bool - << "\n dust_shake: " << static_cast(msg.dust_shake) // bool - << "\n left_light: " << static_cast(msg.left_light) // bool - << "\n right_light: " << static_cast(msg.right_light) // bool - << "\n brake_light: " << static_cast(msg.brake_light) // bool - << "\n headlight: " << static_cast(msg.headlight) // bool - << "\n dump: " << static_cast(msg.dump) // uint8 - ); + // 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 ? "开启清扫" : "关闭清扫")); } } @@ -465,7 +444,7 @@ int main(int argc, char **argv) { init_main(); memset(&car_ctrl_mes, 0, sizeof(car_ctrl_mes)); - car_ctrl_mes.steering = 32767.0; + car_ctrl_mes.steering = 32767; pthread_create(&mqtt_sub_thread_t, NULL, mqtt_sub, NULL); rclcpp::init(argc, argv); diff --git a/src/sweeper_interfaces/CMakeLists.txt b/src/sweeper_interfaces/CMakeLists.txt index 1dc4f17..79c87e7 100644 --- a/src/sweeper_interfaces/CMakeLists.txt +++ b/src/sweeper_interfaces/CMakeLists.txt @@ -13,7 +13,13 @@ find_package(std_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/McCtrl.msg" "msg/CanFrame.msg" + "msg/Sub.msg" "msg/Rtk.msg" + "msg/Pl.msg" + "msg/DetectLine.msg" + "msg/Route.msg" + "msg/Fu.msg" + "msg/Task.msg" DEPENDENCIES std_msgs ) diff --git a/start_all.sh b/start_all.sh old mode 100644 new mode 100755 index e32b2f5..1f99e43 --- a/start_all.sh +++ b/start_all.sh @@ -2,10 +2,10 @@ set -e # 加载 ROS2 系统环境 -source /opt/ros/humble/setup.bash +source /opt/ros/foxy/setup.bash # 加载你的工作空间环境 -source /home/lyq/project/minibus/install/setup.bash +source ~/sweeper_whu/install/setup.bash # 启动节点 ros2 run radio_ctrl radio_ctrl_node & @@ -14,5 +14,11 @@ ros2 run ctrl_arbiter ctrl_arbiter_node & ros2 run mqtt_report mqtt_report_node & 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 task_manager task_manager_node & +ros2 run pl pl_node & +ros2 launch fu fu.launch.py & wait