From db1f82ef3432af2a5349651ae6411893ccbb239a Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Fri, 14 Jun 2024 20:07:08 -0400 Subject: [PATCH] rtabmap good 3d map --- src/rove_description/config/basic.rviz | 66 ++--- src/rove_rtabmap/package.xml | 33 --- src/rove_rtabmap/resource/rove_rtabmap | 0 src/rove_rtabmap/setup.cfg | 4 - src/rove_rtabmap/setup.py | 29 --- src/rove_slam/config/ekf.yaml | 9 + .../launch/rtabmap.launch.py | 0 src/rove_slam/launch/slam3d.launch.py | 235 +++++++++--------- 8 files changed, 155 insertions(+), 221 deletions(-) delete mode 100644 src/rove_rtabmap/package.xml delete mode 100644 src/rove_rtabmap/resource/rove_rtabmap delete mode 100644 src/rove_rtabmap/setup.cfg delete mode 100644 src/rove_rtabmap/setup.py rename src/{rove_rtabmap => rove_slam}/launch/rtabmap.launch.py (100%) diff --git a/src/rove_description/config/basic.rviz b/src/rove_description/config/basic.rviz index df8d7c0..fa2d445 100644 --- a/src/rove_description/config/basic.rviz +++ b/src/rove_description/config/basic.rviz @@ -154,9 +154,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0 + Max Intensity: 100 Min Color: 0; 0; 0 - Min Intensity: 0 + Min Intensity: 1 Name: LaserScan Position Transformer: XYZ Selectable: true @@ -180,16 +180,6 @@ Visualization Manager: All Enabled: false base_link: Value: true - flipper_fl: - Value: true - flipper_fr: - Value: true - flipper_rl: - Value: true - flipper_rr: - Value: true - map: - Value: true odom: Value: true sensor_link: @@ -216,33 +206,23 @@ Visualization Manager: Show Axes: true Show Names: false Tree: - map: - odom: - base_link: - flipper_fl: - {} - flipper_fr: + odom: + base_link: + sensor_link: + {} + track_l: + track_fl: {} - flipper_rl: + track_rl: {} - flipper_rr: + track_r: + track_fr: {} - sensor_link: + track_rr: {} - track_l: - track_fl: - {} - track_rl: - {} - track_r: - track_fr: - {} - track_rr: - {} - velodyne_link: + velodyne_link: + velodyne_laser: {} - velodyne_laser: - {} Update Interval: 0 Value: true - Alpha: 0.699999988079071 @@ -331,7 +311,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /odometry/filtered + Value: /odometry/local Value: true - Acceleration properties: Acc. vector alpha: 1 @@ -376,7 +356,7 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0 + Max Intensity: 255 Min Color: 0; 0; 0 Min Intensity: 0 Name: 3d map @@ -391,7 +371,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /cloud_map + Value: /assembled_cloud Use Fixed Frame: true Use rainbow: true Value: true @@ -441,25 +421,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 10.68095874786377 + Distance: 9.305854797363281 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -1.4718998670578003 - Y: 2.8054065704345703 - Z: 2.2840819358825684 + X: -0.6736313104629517 + Y: -1.0152995586395264 + Z: 2.0505826473236084 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6003987789154053 + Pitch: 0.600398600101471 Target Frame: Value: Orbit (rviz) - Yaw: 3.7735798358917236 + Yaw: 1.893579363822937 Saved: ~ Window Geometry: Displays: diff --git a/src/rove_rtabmap/package.xml b/src/rove_rtabmap/package.xml deleted file mode 100644 index 671fbef..0000000 --- a/src/rove_rtabmap/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - rove_rtabmap - 0.0.0 - Rtabmap package for rove - rove - MIT - - - rtabmap_ros - - rtabmap_odom - rtabmap_slam - rtabmap_util - rtabmap_rviz_plugins - rtabmap_slam - rtabmap_util - rtabmap_viz - imu_filter_madgwick - tf2_ros - realsense2_camera - velodyne - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/src/rove_rtabmap/resource/rove_rtabmap b/src/rove_rtabmap/resource/rove_rtabmap deleted file mode 100644 index e69de29..0000000 diff --git a/src/rove_rtabmap/setup.cfg b/src/rove_rtabmap/setup.cfg deleted file mode 100644 index c9ff06a..0000000 --- a/src/rove_rtabmap/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/rove_rtabmap -[install] -install_scripts=$base/lib/rove_rtabmap diff --git a/src/rove_rtabmap/setup.py b/src/rove_rtabmap/setup.py deleted file mode 100644 index 37c0e51..0000000 --- a/src/rove_rtabmap/setup.py +++ /dev/null @@ -1,29 +0,0 @@ -from setuptools import find_packages, setup -import os -from glob import glob - -package_name = 'rove_rtabmap' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ('share/' + package_name + '/launch', glob('launch/*.launch.py')), - ('share/' + package_name + '/config', glob('config/*')), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='rove', - maintainer_email='capra@ens.etsmtl.ca', - description='Rtabmap package for rove', - license='MIT', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - ], - }, -) diff --git a/src/rove_slam/config/ekf.yaml b/src/rove_slam/config/ekf.yaml index 93e59ca..56c3cc8 100644 --- a/src/rove_slam/config/ekf.yaml +++ b/src/rove_slam/config/ekf.yaml @@ -24,6 +24,15 @@ ekf_filter_node_local: false, false, false] # ax ay az imu0_queue_size: 20 + # Add ICP odometry as a new sensor input + odom1: /icp_odom + odom1_config: [true, true, false, + false, false, true, + true, true, false, + false, false, true, + false, false, false] + + ekf_filter_node_global: ros__parameters: frequency: 30.0 diff --git a/src/rove_rtabmap/launch/rtabmap.launch.py b/src/rove_slam/launch/rtabmap.launch.py similarity index 100% rename from src/rove_rtabmap/launch/rtabmap.launch.py rename to src/rove_slam/launch/rtabmap.launch.py diff --git a/src/rove_slam/launch/slam3d.launch.py b/src/rove_slam/launch/slam3d.launch.py index 00710e6..b175edd 100644 --- a/src/rove_slam/launch/slam3d.launch.py +++ b/src/rove_slam/launch/slam3d.launch.py @@ -2,123 +2,134 @@ from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +import os def generate_launch_description(): - camera_model = 'zed2i' - - lidar_frame_id = LaunchConfiguration('lidar_frame_id') + # Launch configuration use_sim_time = LaunchConfiguration('use_sim_time') - deskewing = False - - return LaunchDescription([ - # Launch arguments - DeclareLaunchArgument( - 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true'), - - DeclareLaunchArgument( - 'deskewing', default_value='true', - description='Enable lidar deskewing'), - - # Nodes to launch - Node( - package='rtabmap_odom', executable='icp_odometry', output='screen', - parameters=[{ - 'odom_frame_id': 'odom', - 'wait_for_transform': 0.2, - 'expected_update_rate': 50.0, - 'deskewing': deskewing, - 'use_sim_time': use_sim_time, - }], - remappings=[ - ('scan_cloud', '/velodyne_points') - ], - arguments=[ - 'Icp/PointToPlane', 'true', - 'Icp/Iterations', '10', - 'Icp/VoxelSize', '0.1', - 'Icp/Epsilon', '0.001', - 'Icp/PointToPlaneK', '20', - 'Icp/PointToPlaneRadius', '0', - 'Icp/MaxTranslation', '2', - 'Icp/MaxCorrespondenceDistance', '1', - 'Icp/Strategy', '1', - 'Icp/OutlierRatio', '0.7', - 'Icp/CorrespondenceRatio', '0.01', - 'Odom/ScanKeyFrameThr', '0.4', - 'OdomF2M/ScanSubtractRadius', '0.1', - 'OdomF2M/ScanMaxSize', '15000', - 'OdomF2M/BundleAdjustment', 'false', - ]), - - Node( - package='rtabmap_util', executable='point_cloud_assembler', output='screen', - parameters=[{ - 'max_clouds': 10, - 'use_sim_time': use_sim_time, - }], - remappings=[ - ('cloud', '/velodyne_points') - ]), - - Node( - package='rtabmap_slam', executable='rtabmap', output='screen', - parameters=[{ - 'subscribe_depth': False, - 'subscribe_rgb': False, - 'subscribe_scan_cloud': True, - 'approx_sync': False, - 'wait_for_transform': 0.2, - 'use_sim_time': use_sim_time, - 'wait_imu_to_int': True, - 'imu_topic': 'imu', - }], - remappings=[ - ('scan_cloud', 'assembled_cloud') - ], - arguments=[ - '-d', # This will delete the previous database (~/.ros/rtabmap.db) - 'RGBD/ProximityMaxGraphDepth', '0', - 'RGBD/ProximityPathMaxNeighbors', '1', - 'RGBD/AngularUpdate', '0.05', - 'RGBD/LinearUpdate', '0.05', - 'RGBD/CreateOccupancyGrid', 'true', - 'Mem/NotLinkedNodesKept', 'false', - 'Mem/STMSize', '30', - 'Mem/LaserScanNormalK', '20', - 'Reg/Strategy', '1', - 'Icp/VoxelSize', '0.5', - 'Icp/PointToPlaneK', '20', - 'Icp/PointToPlaneRadius', '0', - 'Icp/PointToPlane', 'true', - 'Icp/Iterations', '10', - 'Icp/Epsilon', '0.001', - 'Icp/MaxTranslation', '3', - 'Icp/MaxCorrespondenceDistance', '1', - 'Icp/Strategy', '1', - 'Icp/OutlierRatio', '0.7', - 'Icp/CorrespondenceRatio', '0.2', - 'Grid/MaxGroundHeight', '0.05', - 'Grid/MaxObstacleHeight', '1.5', - 'Reg/Force3DoF', 'true', - 'Optimizer/Slam2D', 'true', + # Nodes + icp_odometry_node = Node( + package='rtabmap_odom', + executable='icp_odometry', + name='icp_odometry', + output='screen', + parameters=[{ + 'odom_frame_id': '/odom', + 'wait_for_transform': 0.2, + 'publish_tf': False, + 'expected_update_rate': 50.0, + 'deskewing': deskewing, + 'use_sim_time': use_sim_time, + }], + remappings=[ + ('scan_cloud', '/velodyne_points'), + ('odom', '/icp_odom') + ], + arguments=[ + 'Icp/PointToPlane', 'true', + 'Icp/Iterations', '10', + 'Icp/VoxelSize', '0.1', + 'Icp/Epsilon', '0.001', + 'Icp/PointToPlaneK', '20', + 'Icp/PointToPlaneRadius', '0', + 'Icp/MaxTranslation', '2', + 'Icp/MaxCorrespondenceDistance', '1', + 'Icp/Strategy', '1', + 'Icp/OutlierRatio', '0.7', + 'Icp/CorrespondenceRatio', '0.01', + 'Odom/ScanKeyFrameThr', '0.4', + 'OdomF2M/ScanSubtractRadius', '0.1', + 'OdomF2M/ScanMaxSize', '15000', + 'OdomF2M/BundleAdjustment', 'false', + ] + ) + + point_cloud_assembler_node = Node( + package='rtabmap_util', + executable='point_cloud_assembler', + name='point_cloud_assembler', + output='screen', + parameters=[{ + 'max_clouds': 10, + 'use_sim_time': use_sim_time, + }], + remappings=[ + ('cloud', '/velodyne_points'), + ('odom', '/odometry/local') + ] + ) + + rtabmap_node = Node( + package='rtabmap_slam', + executable='rtabmap', + name='rtabmap', + output='screen', + parameters=[{ + 'subscribe_depth': False, + 'subscribe_rgb': False, + 'subscribe_scan_cloud': True, + 'approx_sync': True, + 'wait_for_transform': 0.2, + 'use_sim_time': use_sim_time, + }], + remappings=[ + ('scan_cloud', '/assembled_cloud'), + ('odom', '/odometry/local') - ]), - - Node( - package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[{ - 'odom_frame_id': 'odom', - 'subscribe_odom_info': True, - 'subscribe_scan_cloud': True, - 'approx_sync': False, - 'use_sim_time': use_sim_time, - }], - remappings=[ - ('scan_cloud', 'odom_filtered_input_scan') - ]), + ], + arguments=[ + '-d', # This will delete the previous database (~/.ros/rtabmap.db) + 'RGBD/ProximityMaxGraphDepth', '0', + 'RGBD/ProximityPathMaxNeighbors', '1', + 'RGBD/AngularUpdate', '0.05', + 'RGBD/LinearUpdate', '0.05', + 'RGBD/CreateOccupancyGrid', 'true', + 'Mem/NotLinkedNodesKept', 'false', + 'Mem/STMSize', '30', + 'Mem/LaserScanNormalK', '20', + 'Reg/Strategy', '1', + 'Icp/VoxelSize', '0.5', + 'Icp/PointToPlaneK', '20', + 'Icp/PointToPlaneRadius', '0', + 'Icp/PointToPlane', 'true', + 'Icp/Iterations', '10', + 'Icp/Epsilon', '0.001', + 'Icp/MaxTranslation', '3', + 'Icp/MaxCorrespondenceDistance', '1', + 'Icp/Strategy', '1', + 'Icp/OutlierRatio', '0.7', + 'Icp/CorrespondenceRatio', '0.2', + 'Grid/MaxGroundHeight', '0.05', + 'Grid/MaxObstacleHeight', '1.5', + 'Reg/Force3DoF', 'true', + 'Optimizer/Slam2D', 'true', + ] + ) + + rtabmap_viz_node = Node( + package='rtabmap_viz', + executable='rtabmap_viz', + name='rtabmap_viz', + output='screen', + parameters=[{ + 'odom_frame_id': 'odom', + 'subscribe_odom_info': True, + 'subscribe_scan_cloud': True, + 'approx_sync': True, + 'use_sim_time': use_sim_time, + }], + remappings=[ + ('scan_cloud', '/assembled_cloud'), + ('odom', '/odometry/local') + ] + ) + + return LaunchDescription([ + icp_odometry_node, + point_cloud_assembler_node, + rtabmap_node, + #rtabmap_viz_node ]) - \ No newline at end of file