From a69a8017387c1fe34052201eaafbb05d449d4f3c Mon Sep 17 00:00:00 2001 From: LihanChen2004 <757003373@qq.com> Date: Sat, 9 Dec 2023 15:27:47 +0800 Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20feat:=20=E5=AE=9E=E7=8E=B0=E4=BB=BF?= =?UTF-8?q?=E7=9C=9F=E5=92=8C=E7=8E=B0=E5=AE=9E=E5=90=8C=E4=B8=80=E5=A5=97?= =?UTF-8?q?=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 修改了仿真 mid360 发出的点云数据话题 /livox/lidar/pointcloud 。 分别对仿真环境和真实环境编写不同的 urdf 和 launch 。 修改 README，建议使用 修改后的 livox_ros_driver2，真实雷达将同时发布 CustomMsg 和 PointCloud2 类型消息 --- README.md | 32 ++-- real_mapping.sh | 35 ++++ mapping.sh => sim_mapping.sh | 8 +- nav.sh => sim_nav.sh | 0 src/rm_navigation/params/nav2_params.yaml | 2 +- .../launch/segmentation_params.yaml | 2 +- .../src/livox_points_plugin.cpp | 2 +- .../pb_rm_simulation/launch/rm_real.launch.py | 83 ++++++++++ .../launch/rm_simulation.launch.py | 2 +- .../pb_rm_simulation/rviz/rviz2.rviz | 2 +- .../urdf/real_waking_robot.xacro | 149 ++++++++++++++++++ ...ot.xacro => simulation_waking_robot.xacro} | 0 12 files changed, 294 insertions(+), 23 deletions(-) create mode 100755 real_mapping.sh rename mapping.sh => sim_mapping.sh (94%) rename nav.sh => sim_nav.sh (100%) create mode 100644 src/rm_simulation/pb_rm_simulation/launch/rm_real.launch.py create mode 100644 src/rm_simulation/pb_rm_simulation/urdf/real_waking_robot.xacro rename src/rm_simulation/pb_rm_simulation/urdf/{waking_robot.xacro => simulation_waking_robot.xacro} (100%) diff --git a/README.md b/README.md index 38651e5..86175d8 100644 --- a/README.md +++ b/README.md @@ -22,7 +22,7 @@ Mid360 点云仿真：参考了 [livox_laser_simulation](https://github.com/Livo | **Topic name** | **Type** | **Note** | |:-------------------:|:-------------------------------:|:--------------------------------:| | /livox/lidar | livox_ros_driver2/msg/CustomMsg | Mid360 自定义消息类型 | -| /livox/lidar_PointCloud2 | sensor_msgs/msg/PointCloud2 | ROS2 点云消息类型 | +| /livox/lidar/pointcloud | sensor_msgs/msg/PointCloud2 | ROS2 点云消息类型 | | /livox/imu | sensor_msgs/msg/Imu | Gazebo 插件仿真 IMU | | /cmd_vel | geometry_msgs/msg/Twist | 麦克纳姆轮小车运动控制接口 | | /odom | nav_msgs/msg/Odometry | FAST_LIO 输出的里程计 | @@ -64,22 +64,21 @@ Mid360 点云仿真：参考了 [livox_laser_simulation](https://github.com/Livo cmake .. && make -j sudo make install ``` -3. 安装 [Livox ROS Driver 2](https://github.com/Livox-SDK/Livox-SDK) +3. 安装 [Livox ROS Driver 2](https://gitee.com/SMBU-POLARBEAR/livox_ros_driver2_humble) ``` - git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2 + git clone https://gitee.com/SMBU-POLARBEAR/livox_ros_driver2_humble.git ``` ``` - cd ws_livox/src/livox_ros_driver2 - source /opt/ros/humble/setup.sh - ./build.sh humble + cd livox_ros_driver2_humble + colcon build --symlink-install ``` - 设置每次启动终端时，自动 source livox_ros_driver2 的工作空间，才能使用 livox 雷达的自定义消息类型。**注意替换 `WHERE_YOU_GIT_CLONE`** + 设置每次启动终端时，自动 source livox_ros_driver2 的工作空间，才能使用 livox 雷达的自定义消息类型。**注意替换 `xxx`** ``` - echo '# livox_ros_driver2' >> ~/.bashrc - echo 'source source /WHERE_YOU_GIT_CLONE/ws_livox/install/setup.bash' >> ~/.bashrc + echo '# livox_ros_driver2_humble (Polarbear version)' >> ~/.bashrc + echo 'source /xxx/livox_ros_driver2_humble/install/setup.bash' >> ~/.bashrc ``` ## 四. 编译运行 @@ -88,14 +87,19 @@ Mid360 点云仿真：参考了 [livox_laser_simulation](https://github.com/Livo git clone https://gitee.com/SMBU-POLARBEAR/pb_rmsimulation ``` -#### 边建图边导航 +#### 仿真环境：边建图边导航 ``` -./mapping.sh +./sim_mapping.sh ``` -#### 已知全局地图导航 +#### 仿真环境：已知全局地图导航 ``` -./nav.sh +./sim_nav.sh +``` + +#### 真实环境：边建图边导航(文档完善中) +``` +./real_mapping.sh ``` #### 小工具 - 键盘控制 @@ -137,7 +141,7 @@ Avia, HAP, Horizon, Mid40, Mid70, Mid360, Tele ## ATTENTION 当前项目存在性能问题 1. 仿真性能瓶颈，livox_laser_simulation 无法多核并行计算，导致一核有难，十核围观。在 RMUC 地图中 mapping 模式 的 /livox/lidar 的频率仅 3.6~4.1Hz，在 nav 模式中频率更低。 -2. 2023.12.8 版本往后修复了之前错误的 tf 树，现在的里程计完全由 FAST_LIO 提供，在 RMUC 地图中 mapping 模式 的 /Odometry 的频率仅约 3.8Hz ，远远不及使用 Gazebo libgazebo_ros_planar_move 直接提供的 里程计（50~60Hz）），这导致 navigation 控制器发出的小车运动速度很小。 +2. 2023.12.8 版本往后修复了之前错误的 tf 树，现在的里程计完全由 FAST_LIO 提供，在 RMUC 地图中 mapping 模式 的 /Odometry 的频率仅约 3.8Hz ，远远不及使用 Gazebo libgazebo_ros_planar_move 直接提供的 里程计（50~60Hz）），这导致小车运动控制延迟加剧。 3. 在 nav 仿真模式下，运行 icp_localization 时存在概率性中断，抛出以下报错： ```bash diff --git a/real_mapping.sh b/real_mapping.sh new file mode 100755 index 0000000..7c3284a --- /dev/null +++ b/real_mapping.sh @@ -0,0 +1,35 @@ +# colcon build --symlink-install + +cmds=( + " + ros2 launch livox_ros_driver2 msg_MID360_launch.py + " + "ros2 launch pb_rm_simulation rm_real.launch.py \ + use_sim_time:=false + " + "ros2 launch imu_complementary_filter complementary_filter.launch.py \ + use_sim_time:=false + " + "ros2 launch fast_lio mapping_mid360.launch.py \ + use_sim_time:=false rviz:=true + " + "ros2 launch linefit_ground_segmentation_ros segmentation.launch.py \ + use_sim_time:=false + " + "ros2 launch pointcloud_to_laserscan pointcloud_to_laserscan_launch.py \ + use_sim_time:=false + " + "ros2 launch rm_navigation online_async_launch.py \ + use_sim_time:=false + " + "ros2 launch rm_navigation bringup_no_amcl_launch.py \ + use_sim_time:=false + " +) + +for cmd in "${cmds[@]}" +do + echo Current CMD : "$cmd" + gnome-terminal -- bash -c "cd $(pwd);source install/setup.bash;$cmd;exec bash;" + sleep 0.2 +done \ No newline at end of file diff --git a/mapping.sh b/sim_mapping.sh similarity index 94% rename from mapping.sh rename to sim_mapping.sh index 6d1be23..bfd1b19 100755 --- a/mapping.sh +++ b/sim_mapping.sh @@ -1,7 +1,7 @@ -colcon build --symlink-install +# colcon build --symlink-install cmds=( - "ros2 launch pb_rm_simulation rm_simulation.launch.py world:=RMUC \ + "ros2 launch pb_rm_simulation rm_simulation.launch.py world:=RMUL \ use_sim_time:=true " "ros2 launch imu_complementary_filter complementary_filter.launch.py \ @@ -9,10 +9,10 @@ cmds=( " "ros2 launch fast_lio mapping_mid360.launch.py \ use_sim_time:=true rviz:=false - " + " "ros2 launch linefit_ground_segmentation_ros segmentation.launch.py \ use_sim_time:=true - " + " "ros2 launch pointcloud_to_laserscan pointcloud_to_laserscan_launch.py \ use_sim_time:=true " diff --git a/nav.sh b/sim_nav.sh similarity index 100% rename from nav.sh rename to sim_nav.sh diff --git a/src/rm_navigation/params/nav2_params.yaml b/src/rm_navigation/params/nav2_params.yaml index f7c96c0..f398879 100644 --- a/src/rm_navigation/params/nav2_params.yaml +++ b/src/rm_navigation/params/nav2_params.yaml @@ -186,7 +186,7 @@ controller_server: vx_samples: 20 vy_samples: 20 vtheta_samples: 20 - sim_time: 0.36 + sim_time: 0.38 linear_granularity: 0.05 angular_granularity: 0.025 transform_tolerance: 0.2 diff --git a/src/rm_perception/linefit_ground_segementation_ros2/linefit_ground_segmentation_ros/launch/segmentation_params.yaml b/src/rm_perception/linefit_ground_segementation_ros2/linefit_ground_segmentation_ros/launch/segmentation_params.yaml index 3ccaa27..ce45574 100644 --- a/src/rm_perception/linefit_ground_segementation_ros2/linefit_ground_segmentation_ros/launch/segmentation_params.yaml +++ b/src/rm_perception/linefit_ground_segementation_ros2/linefit_ground_segmentation_ros/launch/segmentation_params.yaml @@ -23,6 +23,6 @@ ground_segmentation: latch: false # latch output topics or not visualize: false # visualize segmentation result - USE ONLY FOR DEBUGGING - input_topic: "/livox/lidar_PointCloud2" + input_topic: "/livox/lidar/pointcloud" obstacle_output_topic: "segmentation/obstacle" ground_output_topic: "segmentation/ground" \ No newline at end of file diff --git a/src/rm_simulation/livox_laser_simulation_RO2/src/livox_points_plugin.cpp b/src/rm_simulation/livox_laser_simulation_RO2/src/livox_points_plugin.cpp index d6babbf..a92697f 100644 --- a/src/rm_simulation/livox_laser_simulation_RO2/src/livox_points_plugin.cpp +++ b/src/rm_simulation/livox_laser_simulation_RO2/src/livox_points_plugin.cpp @@ -72,7 +72,7 @@ namespace gazebo node = transport::NodePtr(new transport::Node()); node->Init(raySensor->WorldName()); // PointCloud2 publisher - cloud2_pub = node_->create_publisher(curr_scan_topic + "_PointCloud2", 10); + cloud2_pub = node_->create_publisher(curr_scan_topic + "/pointcloud", 10); // CustomMsg publisher custom_pub = node_->create_publisher(curr_scan_topic, 10); diff --git a/src/rm_simulation/pb_rm_simulation/launch/rm_real.launch.py b/src/rm_simulation/pb_rm_simulation/launch/rm_real.launch.py new file mode 100644 index 0000000..de1fcc4 --- /dev/null +++ b/src/rm_simulation/pb_rm_simulation/launch/rm_real.launch.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory, get_package_share_path + +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration, Command +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, GroupAction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue +from launch.conditions import LaunchConfigurationEquals + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('pb_rm_simulation') + + # Specify xacro path + urdf_dir = get_package_share_path('pb_rm_simulation') / 'urdf' / 'real_waking_robot.xacro' + + # Create the launch configuration variables + use_sim_time = LaunchConfiguration('use_sim_time') + world = LaunchConfiguration('world') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='True', + description='Use simulation (Gazebo) clock if true' + ) + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config_file', + default_value=os.path.join(bringup_dir, 'rviz', 'rviz2.rviz'), + description='Full path to the RVIZ config file to use' + ) + + # Specify the actions + start_joint_state_publisher_cmd = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + parameters=[{ + 'use_sim_time': use_sim_time, + 'robot_description': ParameterValue( + Command(['xacro ', str(urdf_dir)]), value_type=str + ), + }], + output='screen' + ) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + parameters=[{ + 'use_sim_time': use_sim_time, + 'robot_description': ParameterValue( + Command(['xacro ', str(urdf_dir)]), value_type=str + ), + }], + output='screen' + ) + + start_rviz_cmd = Node( + package='rviz2', + namespace='', + executable='rviz2', + arguments=['-d' + os.path.join(bringup_dir, 'rviz', 'rviz2.rviz')] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(start_joint_state_publisher_cmd) + ld.add_action(start_robot_state_publisher_cmd) + + # Uncomment this line if you want to start RViz + # ld.add_action(start_rviz_cmd) + + return ld diff --git a/src/rm_simulation/pb_rm_simulation/launch/rm_simulation.launch.py b/src/rm_simulation/pb_rm_simulation/launch/rm_simulation.launch.py index 5a36016..21829c8 100644 --- a/src/rm_simulation/pb_rm_simulation/launch/rm_simulation.launch.py +++ b/src/rm_simulation/pb_rm_simulation/launch/rm_simulation.launch.py @@ -42,7 +42,7 @@ def generate_launch_description(): pkg_gazebo_ros = get_package_share_directory('gazebo_ros') # Specify xacro path - urdf_dir = get_package_share_path('pb_rm_simulation') / 'urdf' / 'waking_robot.xacro' + urdf_dir = get_package_share_path('pb_rm_simulation') / 'urdf' / 'simulation_waking_robot.xacro' # Create the launch configuration variables use_sim_time = LaunchConfiguration('use_sim_time') diff --git a/src/rm_simulation/pb_rm_simulation/rviz/rviz2.rviz b/src/rm_simulation/pb_rm_simulation/rviz/rviz2.rviz index 89eca4c..e1f8fac 100644 --- a/src/rm_simulation/pb_rm_simulation/rviz/rviz2.rviz +++ b/src/rm_simulation/pb_rm_simulation/rviz/rviz2.rviz @@ -111,7 +111,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /mid360_PointCloud2 + Value: /livox/lidar/pointcloud Use Fixed Frame: true Use rainbow: true Value: true diff --git a/src/rm_simulation/pb_rm_simulation/urdf/real_waking_robot.xacro b/src/rm_simulation/pb_rm_simulation/urdf/real_waking_robot.xacro new file mode 100644 index 0000000..daac791 --- /dev/null +++ b/src/rm_simulation/pb_rm_simulation/urdf/real_waking_robot.xacro @@ -0,0 +1,149 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.5 + 0.5 + + + + + + + + + + + + + + + + + 0.5 + 0.5 + + + + + + + + + + + + + + + + + 0.5 + 0.5 + + + + + + + + + + + + + + + + + 0.5 + 0.5 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/rm_simulation/pb_rm_simulation/urdf/waking_robot.xacro b/src/rm_simulation/pb_rm_simulation/urdf/simulation_waking_robot.xacro similarity index 100% rename from src/rm_simulation/pb_rm_simulation/urdf/waking_robot.xacro rename to src/rm_simulation/pb_rm_simulation/urdf/simulation_waking_robot.xacro -- Gitee