diff --git a/README.md b/README.md index 38651e571f134b21a7c2602e687d5e6e041f1131..86175d8816451060cc47c3d0db9c24c6ea3fde51 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 0000000000000000000000000000000000000000..7c3284a7dea9e4dd6e7d7993681fe37e8f541880 --- /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 6d1be23d088dc2161ffd21800568edfa44dfda51..bfd1b198a9f41d777ca627d4f72ac23be839c64f 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 f7c96c0af183a51f3a152b984adb2a74aca33121..f39887929cc38ded59263fed9deb1af4d589298f 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 3ccaa27d2fca345229aa7be5c9a254ce0739a255..ce455747378e5cf61066dbafeba59282c841524e 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 d6babbf24f1d3c7a2f7e2399d62c7e4141aef231..a92697fa18069a7ecd46ce3e8fea4c05a992fce2 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 0000000000000000000000000000000000000000..de1fcc48483a26bcc230db78ff78f8b94af172a9 --- /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 5a360165f6c539e9f6a5b5eee4292dba12b77f0e..21829c89ae576d6c79ff86a9f85fae27f96fd133 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 89eca4cea7d99fcd7d929a830d75480cbb2e23b7..e1f8fac1f16c8f194cbe81c6f416378e0d8171f5 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 0000000000000000000000000000000000000000..daac791bd68830ff5172e3d18545ff844bb66c87 --- /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