From a73a79253cd7708330ed6f6967d07a94c0ac254c Mon Sep 17 00:00:00 2001 From: matlabbe <matlabbe@gmail.com> Date: Sun, 24 Nov 2024 22:08:01 -0800 Subject: [PATCH] Created two base general examples for 3D lidar usage, then create hardware specific examples on top of them. --- rtabmap_examples/launch/k4a.launch.py | 1 - rtabmap_examples/launch/lidar3d.launch.py | 220 +++++++++++++++++ .../launch/lidar3d_assemble.launch.py | 226 ++++++++++++++++++ rtabmap_examples/launch/vlp16.launch.py | 127 ---------- rtabmap_examples/launch/vlp16_imu.launch.py | 183 -------------- .../launch/vlp16_imu_assemble.launch.py | 201 ---------------- rtabmap_examples/launch/vlp16_zed.launch.py | 121 ++++++++++ .../launch/vlp16_zed_vio_assemble.launch.py | 185 -------------- 8 files changed, 567 insertions(+), 697 deletions(-) create mode 100644 rtabmap_examples/launch/lidar3d.launch.py create mode 100644 rtabmap_examples/launch/lidar3d_assemble.launch.py delete mode 100644 rtabmap_examples/launch/vlp16.launch.py delete mode 100644 rtabmap_examples/launch/vlp16_imu.launch.py delete mode 100644 rtabmap_examples/launch/vlp16_imu_assemble.launch.py create mode 100644 rtabmap_examples/launch/vlp16_zed.launch.py delete mode 100644 rtabmap_examples/launch/vlp16_zed_vio_assemble.launch.py diff --git a/rtabmap_examples/launch/k4a.launch.py b/rtabmap_examples/launch/k4a.launch.py index bf06124c2..d09693a60 100644 --- a/rtabmap_examples/launch/k4a.launch.py +++ b/rtabmap_examples/launch/k4a.launch.py @@ -31,7 +31,6 @@ def generate_launch_description(): Node( package='rtabmap_odom', executable='rgbd_odometry', output='screen', parameters=[{ 'frame_id':'camera_base', - 'subscribe_odom_info':True, 'approx_sync':True, 'approx_sync_max_interval':0.01, 'wait_imu_to_init':True, diff --git a/rtabmap_examples/launch/lidar3d.launch.py b/rtabmap_examples/launch/lidar3d.launch.py new file mode 100644 index 000000000..daec929fd --- /dev/null +++ b/rtabmap_examples/launch/lidar3d.launch.py @@ -0,0 +1,220 @@ +# Description: +# In this example, we keep only minimal data to do LiDAR SLAM. +# +# Example: +# Launch your lidar sensor: +# $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py +# $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py +# +# If an IMU is used, make sure TF between lidar/base frame and imu is +# already calibrated. In this example, we assume the imu topic has +# already the orientation estimated, it not, you can use +# imu_filter_madgwick_node (with use_mag:=false publish_tf:=false) +# and set imu_topic to output topic of the filter. +# +# If a camera is used, make sure TF between lidar/base frame and camera is +# already calibrated. To provide image data to this example, you should use +# rtabmap_sync's rgbd_sync or stereo_sync node. +# +# Launch the example by adjusting the lidar topic and base frame: +# $ ros2 launch rtabmap_examples lidar3d.launch.py lidar_topic:=/velodyne_points frame_id:=velodyne + +from launch import LaunchDescription, LaunchContext +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def launch_setup(context: LaunchContext, *args, **kwargs): + + frame_id = LaunchConfiguration('frame_id') + + imu_topic = LaunchConfiguration('imu_topic') + imu_used = imu_topic.perform(context) != '' + + rgbd_image_topic = LaunchConfiguration('rgbd_image_topic') + rgbd_image_used = rgbd_image_topic.perform(context) != '' + + voxel_size = LaunchConfiguration('voxel_size') + voxel_size_value = float(voxel_size.perform(context)) + + use_sim_time = LaunchConfiguration('use_sim_time') + + lidar_topic = LaunchConfiguration('lidar_topic') + lidar_topic_value = lidar_topic.perform(context) + lidar_topic_deskewed = lidar_topic_value + "/deskewed" + + localization = LaunchConfiguration('localization').perform(context) + localization = localization == 'true' or localization == 'True' + + fixed_frame_from_imu = False + fixed_frame_id = LaunchConfiguration('fixed_frame_id').perform(context) + if not fixed_frame_id and imu_used: + fixed_frame_from_imu = True + fixed_frame_id = frame_id.perform(context) + "_stabilized" + + if not fixed_frame_id: + lidar_topic_deskewed = lidar_topic + + # Rule of thumb: + max_correspondence_distance = voxel_size_value * 10.0 + + shared_parameters = { + 'use_sim_time': use_sim_time, + 'frame_id': frame_id, + 'qos': LaunchConfiguration('qos'), + 'approx_sync': rgbd_image_used, + 'wait_for_transform': 0.2, + # RTAB-Map's internal parameters are strings: + 'Icp/PointToPlane': 'true', + 'Icp/Iterations': '10', + 'Icp/VoxelSize': str(voxel_size_value), + 'Icp/Epsilon': '0.001', + 'Icp/PointToPlaneK': '20', + 'Icp/PointToPlaneRadius': '0', + 'Icp/MaxTranslation': '3', + 'Icp/MaxCorrespondenceDistance': str(max_correspondence_distance), + 'Icp/Strategy': '1', + 'Icp/OutlierRatio': '0.7', + } + + icp_odometry_parameters = { + 'expected_update_rate': 15.0, + 'deskewing': not fixed_frame_id, # If fixed_frame_id is set, we do deskewing externally below + 'odom_frame_id': 'icp_odom', + 'guess_frame_id': fixed_frame_id, + # RTAB-Map's internal parameters are strings: + 'Odom/ScanKeyFrameThr': '0.4', + 'OdomF2M/ScanSubtractRadius': str(voxel_size_value), + 'OdomF2M/ScanMaxSize': '15000', + 'OdomF2M/BundleAdjustment': 'false', + 'Icp/CorrespondenceRatio': '0.01' + } + if imu_used: + icp_odometry_parameters['wait_imu_to_init'] = True + + rtabmap_parameters = { + 'subscribe_depth': False, + 'subscribe_rgb': False, + 'subscribe_odom_info': True, + 'subscribe_scan_cloud': True, + # RTAB-Map's internal parameters are strings: + 'RGBD/ProximityMaxGraphDepth': '0', + 'RGBD/ProximityPathMaxNeighbors': '1', + 'RGBD/AngularUpdate': '0.05', + 'RGBD/LinearUpdate': '0.05', + 'RGBD/CreateOccupancyGrid': 'false', + 'Mem/NotLinkedNodesKept': 'false', + 'Mem/STMSize': '30', + 'Reg/Strategy': '1', + 'Icp/CorrespondenceRatio': '0.2' + } + + arguments = [] + if localization: + rtabmap_parameters['Mem/IncrementalMemory'] = 'False' + rtabmap_parameters['Mem/InitWMWithAllNodes'] = 'True' + else: + arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db) + + remappings = [('odom', 'icp_odom')] + if imu_used: + remappings.append(('imu', LaunchConfiguration('imu_topic'))) + else: + remappings.append(('imu', 'imu_not_used')) + if rgbd_image_used: + remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic'))) + + nodes = [ + Node( + package='rtabmap_odom', executable='icp_odometry', output='screen', + parameters=[shared_parameters, icp_odometry_parameters], + remappings=remappings + [('scan_cloud', lidar_topic_deskewed)]), + + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[shared_parameters, rtabmap_parameters, {'subscribe_rgbd': rgbd_image_used}], + remappings=remappings + [('scan_cloud', lidar_topic_deskewed)], + arguments=arguments), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=[shared_parameters, rtabmap_parameters], + remappings=remappings + [('scan_cloud', 'odom_filtered_input_scan')]) + ] + + if fixed_frame_from_imu: + # Create a stabilized base frame based on imu for lidar deskewing + nodes.append( + Node( + package='rtabmap_util', executable='imu_to_tf', output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'fixed_frame_id': fixed_frame_id, + 'base_frame_id': frame_id, + 'wait_for_transform_duration': 0.001}], + remappings=[('imu/data', imu_topic)])) + + if fixed_frame_id: + # Lidar deskewing + nodes.append( + Node( + package='rtabmap_util', executable='lidar_deskewing', output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'fixed_frame_id': fixed_frame_id, + 'wait_for_transform': 0.2}], + remappings=[ + ('input_cloud', lidar_topic) + ]) + ) + + return nodes + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use simulated clock.'), + + DeclareLaunchArgument( + 'deskewing', default_value='true', + description='Enable lidar deskewing.'), + + DeclareLaunchArgument( + 'frame_id', default_value='velodyne', + description='Base frame of the robot.'), + + DeclareLaunchArgument( + 'fixed_frame_id', default_value='', + description='Fixed frame used for lidar deskewing. If not set, we will generate one from IMU.'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Localization mode.'), + + DeclareLaunchArgument( + 'lidar_topic', default_value='/velodyne_points', + description='Name of the lidar PointCloud2 topic.'), + + DeclareLaunchArgument( + 'imu_topic', default_value='', + description='IMU topic (ignored if empty).'), + + DeclareLaunchArgument( + 'rgbd_image_topic', default_value='', + description='RGBD image topic (ignored if empty). Would be the output of a rtabmap_sync\'s rgbd_sync, stereo_sync or rgb_sync node.'), + + DeclareLaunchArgument( + 'voxel_size', default_value='0.1', + description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'), + + DeclareLaunchArgument( + 'qos', default_value='1', + description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'), + + OpaqueFunction(function=launch_setup), + ]) + + diff --git a/rtabmap_examples/launch/lidar3d_assemble.launch.py b/rtabmap_examples/launch/lidar3d_assemble.launch.py new file mode 100644 index 000000000..118cbcfd6 --- /dev/null +++ b/rtabmap_examples/launch/lidar3d_assemble.launch.py @@ -0,0 +1,226 @@ +# Description: +# In this example, we will record ALL lidar scans. An IMU or low latency odometry is required for this example. +# +# Example: +# Launch your lidar sensor: +# $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py +# $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py +# +# Launch your IMU sensor, make sure TF between lidar/base frame and imu is already calibrated. +# In this example, we assume the imu topic has +# already the orientation estimated, it not, you can launch +# imu_filter_madgwick_node (with use_mag:=false publish_tf:=false) +# and set imu_topic to output topic of the filter. +# +# If a camera is used, make sure TF between lidar/base frame and camera is +# already calibrated. To provide image data to this example, you should use +# rtabmap_sync's rgbd_sync or stereo_sync node. +# +# Launch the example by adjusting the lidar topic, imu topic and base frame: +# $ ros2 launch rtabmap_examples lidar3d.launch.py lidar_topic:=/velodyne_points imu_topic:=/imu/data frame_id:=velodyne + +from launch import LaunchDescription, LaunchContext +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def launch_setup(context: LaunchContext, *args, **kwargs): + + frame_id = LaunchConfiguration('frame_id') + + fixed_frame_from_imu = False + fixed_frame_id = LaunchConfiguration('fixed_frame_id').perform(context) + if not fixed_frame_id: + fixed_frame_from_imu = True + fixed_frame_id = frame_id.perform(context) + "_stabilized" + + imu_topic = LaunchConfiguration('imu_topic') + + rgbd_image_topic = LaunchConfiguration('rgbd_image_topic') + rgbd_image_used = rgbd_image_topic.perform(context) != '' + + lidar_topic = LaunchConfiguration('lidar_topic') + lidar_topic_value = lidar_topic.perform(context) + lidar_topic_deskewed = lidar_topic_value + "/deskewed" + + voxel_size = LaunchConfiguration('voxel_size') + voxel_size_value = float(voxel_size.perform(context)) + + use_sim_time = LaunchConfiguration('use_sim_time') + + localization = LaunchConfiguration('localization').perform(context) + localization = localization == 'true' or localization == 'True' + + # Rule of thumb: + max_correspondence_distance = voxel_size_value * 10.0 + + shared_parameters = { + 'use_sim_time': use_sim_time, + 'frame_id': frame_id, + 'qos': LaunchConfiguration('qos'), + 'approx_sync': rgbd_image_used, + 'wait_for_transform': 0.2, + # RTAB-Map's internal parameters are strings: + 'Icp/PointToPlane': 'true', + 'Icp/Iterations': '10', + 'Icp/VoxelSize': str(voxel_size_value), + 'Icp/Epsilon': '0.001', + 'Icp/PointToPlaneK': '20', + 'Icp/PointToPlaneRadius': '0', + 'Icp/MaxTranslation': '3', + 'Icp/MaxCorrespondenceDistance': str(max_correspondence_distance), + 'Icp/Strategy': '1', + 'Icp/OutlierRatio': '0.7', + } + + icp_odometry_parameters = { + 'expected_update_rate': 15.0, + 'wait_imu_to_init': True, + 'odom_frame_id': 'icp_odom', + 'guess_frame_id': fixed_frame_id, + # RTAB-Map's internal parameters are strings: + 'Odom/ScanKeyFrameThr': '0.4', + 'OdomF2M/ScanSubtractRadius': str(voxel_size_value), + 'OdomF2M/ScanMaxSize': '15000', + 'OdomF2M/BundleAdjustment': 'false', + 'Icp/CorrespondenceRatio': '0.01' + } + + rtabmap_parameters = { + 'subscribe_depth': False, + 'subscribe_rgb': False, + 'subscribe_odom_info': True, + 'subscribe_scan_cloud': True, + 'odom_sensor_sync': True, # This will adjust camera position based on difference between lidar and camera stamps. + # RTAB-Map's internal parameters are strings: + 'Rtabmap/DetectionRate': '0', # indirectly set to 1 Hz by the assembling time below (1s) + 'RGBD/ProximityMaxGraphDepth': '0', + 'RGBD/ProximityPathMaxNeighbors': '1', + 'RGBD/AngularUpdate': '0.05', + 'RGBD/LinearUpdate': '0.05', + 'RGBD/CreateOccupancyGrid': 'false', + 'Mem/NotLinkedNodesKept': 'false', + 'Mem/STMSize': '30', + 'Reg/Strategy': '1', + 'Icp/CorrespondenceRatio': '0.2' + } + + remappings = [('imu', imu_topic), + ('odom', 'icp_odom')] + if rgbd_image_used: + remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic'))) + + arguments = [] + if localization: + rtabmap_parameters['Mem/IncrementalMemory'] = 'False' + rtabmap_parameters['Mem/InitWMWithAllNodes'] = 'True' + else: + arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db) + + nodes = [ + # Lidar deskewing + Node( + package='rtabmap_util', executable='lidar_deskewing', output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'fixed_frame_id': fixed_frame_id, + 'wait_for_transform': 0.2}], + remappings=[ + ('input_cloud', lidar_topic) + ]), + + # Lidar odometry + Node( + package='rtabmap_odom', executable='icp_odometry', output='screen', + parameters=[shared_parameters, icp_odometry_parameters], + remappings=remappings + [('scan_cloud', lidar_topic_deskewed)]), + + # Assemble deskewed scans based on icp odometry + Node( + package='rtabmap_util', executable='point_cloud_assembler', output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'assembling_time': LaunchConfiguration('assembling_time'), + 'fixed_frame_id': ""}], # This will make the node subscribing to icp odometry topic "odom" + remappings=[('cloud', lidar_topic_deskewed), + ('odom', 'icp_odom')]), + + # Update the map + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[shared_parameters, rtabmap_parameters, + {'subscribe_rgbd': rgbd_image_used, + 'topic_queue_size': 30, + 'sync_queue_size': 20,}], + remappings=remappings + [('scan_cloud', 'assembled_cloud')], + arguments=arguments), + + # Just for visualization + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=[shared_parameters, rtabmap_parameters], + remappings=remappings + [('scan_cloud', 'odom_filtered_input_scan')]) + ] + + if fixed_frame_from_imu: + # Create a stabilized base frame based on imu for lidar deskewing + nodes.append( + Node( + package='rtabmap_util', executable='imu_to_tf', output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'fixed_frame_id': fixed_frame_id, + 'base_frame_id': frame_id, + 'wait_for_transform_duration': 0.001}], + remappings=[('imu/data', imu_topic)])) + + return nodes + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use simulated clock.'), + + DeclareLaunchArgument( + 'frame_id', default_value='velodyne', + description='Base frame of the robot.'), + + DeclareLaunchArgument( + 'fixed_frame_id', default_value='', + description='Fixed frame used for lidar deskewing. If not set, we will generate one from IMU.'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Localization mode.'), + + DeclareLaunchArgument( + 'lidar_topic', default_value='/velodyne_points', + description='Name of the lidar PointCloud2 topic.'), + + DeclareLaunchArgument( + 'imu_topic', default_value='/imu/data', + description='Name of an IMU topic.'), + + DeclareLaunchArgument( + 'rgbd_image_topic', default_value='', + description='RGBD image topic (ignored if empty). Would be the output of a rtabmap_sync\'s rgbd_sync, stereo_sync or rgb_sync node.'), + + DeclareLaunchArgument( + 'voxel_size', default_value='0.1', + description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'), + + DeclareLaunchArgument( + 'assembling_time', default_value='1.0', + description='How much time (sec) we assemble lidar scans before sending them to mapping node.'), + + DeclareLaunchArgument( + 'qos', default_value='1', + description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'), + + OpaqueFunction(function=launch_setup), + ]) + + diff --git a/rtabmap_examples/launch/vlp16.launch.py b/rtabmap_examples/launch/vlp16.launch.py deleted file mode 100644 index a271241c5..000000000 --- a/rtabmap_examples/launch/vlp16.launch.py +++ /dev/null @@ -1,127 +0,0 @@ -# Example: -# $ ros2 launch rtabmap_examples vlp16.launch.py - -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource - -def generate_launch_description(): - - deskewing = LaunchConfiguration('deskewing') - qos = LaunchConfiguration('qos') - - return LaunchDescription([ - - # Launch arguments - DeclareLaunchArgument( - 'deskewing', default_value='true', - description='Enable lidar deskewing'), - - DeclareLaunchArgument( - 'qos', default_value='1', - description='Quality of Service: 0=system default, 1=reliable, 2=best effort'), - - # Nodes to launch - IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('velodyne_driver'), 'launch'), - '/velodyne_driver_node-VLP16-launch.py']), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('velodyne_pointcloud'), 'launch'), - '/velodyne_transform_node-VLP16-launch.py']), - ), - - Node( - package='rtabmap_odom', executable='icp_odometry', output='screen', - parameters=[{ - 'frame_id':'velodyne', - 'odom_frame_id':'odom', - 'wait_for_transform':0.2, - 'expected_update_rate':15.0, - 'deskewing':deskewing, - 'qos':qos, - # RTAB-Map's internal parameters are strings: - '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' - }], - remappings=[ - ('scan_cloud', '/velodyne_points') - ]), - - Node( - package='rtabmap_slam', executable='rtabmap', output='screen', - parameters=[{ - 'frame_id':'velodyne', - 'subscribe_depth':False, - 'subscribe_rgb':False, - 'subscribe_scan_cloud':True, - 'approx_sync':False, - 'wait_for_transform':0.2, - 'qos':qos, - # RTAB-Map's internal parameters are strings: - 'RGBD/ProximityMaxGraphDepth': '0', - 'RGBD/ProximityPathMaxNeighbors': '1', - 'RGBD/AngularUpdate': '0.05', - 'RGBD/LinearUpdate': '0.05', - 'RGBD/CreateOccupancyGrid': 'false', - 'Mem/NotLinkedNodesKept': 'false', - 'Mem/STMSize': '30', - 'Mem/LaserScanNormalK': '20', - 'Reg/Strategy': '1', - 'Icp/VoxelSize': '0.1', - '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' - }], - remappings=[ - ('scan_cloud', 'odom_filtered_input_scan') - ], - arguments=[ - '-d' # This will delete the previous database (~/.ros/rtabmap.db) - ]), - - Node( - package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[{ - 'frame_id':'velodyne', - 'odom_frame_id':'odom', - 'subscribe_odom_info':True, - 'subscribe_scan_cloud':True, - 'approx_sync':False, - 'qos':qos, - }], - remappings=[ - ('scan_cloud', 'odom_filtered_input_scan') - ]), - ]) - - diff --git a/rtabmap_examples/launch/vlp16_imu.launch.py b/rtabmap_examples/launch/vlp16_imu.launch.py deleted file mode 100644 index 82384e5bc..000000000 --- a/rtabmap_examples/launch/vlp16_imu.launch.py +++ /dev/null @@ -1,183 +0,0 @@ -# Example: -# $ ros2 launch rtabmap_examples vlp16_imu.launch.py - -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource - -def launch_setup(context, *args, **kwargs): - qos = LaunchConfiguration('qos') - scan_topic = LaunchConfiguration('scan_topic') - imu_topic = LaunchConfiguration('imu_topic') - imu_filtered_topic = ''.join([imu_topic.perform(context), "/filtered"]) - frame_id = LaunchConfiguration('frame_id') - frame_id_stabilized = ''.join([frame_id.perform(context), "_stabilized"]) - - return [ - ################################# - # Hardware specific nodes - BEGIN - ################################# - IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('velodyne_driver'), 'launch'), - '/velodyne_driver_node-VLP16-launch.py']), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('velodyne_pointcloud'), 'launch'), - '/velodyne_transform_node-VLP16-launch.py']), - ), - - # The IMU driver publishing /imu/data (adjust imu_topic argument if your imu publishes another topic name) - Node(package='imu_brick', executable='imu_brick_node'), # publishing /imu/data - - # Static transform between velodyne and imu frame - Node(package='tf2_ros', executable='static_transform_publisher', arguments=["0", "0", "0.05", "-1.57", "0", "0", "velodyne", "imu_link"]), - ################################# - # Hardware specific nodes - END - ################################# - - # Nodes below should work "as is" with different hardware - - # You can skip the imu filter node if your imu data has already the orientation computed - Node( - package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen', - parameters=[{ - 'use_mag':False, - 'world_frame':'enu', - 'publish_tf':False}], - remappings=[ - ('imu/data_raw', imu_topic), - ('imu/data', imu_filtered_topic) - ]), - - Node( - package='rtabmap_util', executable='imu_to_tf', output='screen', - parameters=[{ - 'fixed_frame_id':frame_id_stabilized, - 'base_frame_id':frame_id, - 'wait_for_transform_duration': 0.001}], - remappings=[ - ('imu/data', imu_filtered_topic) - ]), - - Node( - package='rtabmap_odom', executable='icp_odometry', output='screen', - parameters=[{ - 'frame_id':frame_id, - 'odom_frame_id':'odom', - 'guess_frame_id':frame_id_stabilized, - 'wait_for_transform':0.2, - 'expected_update_rate':15.0, - 'deskewing':True, # Scans will be deskewed internally based on guess_frame_id (i.e., our stabilized imu frame) - 'wait_imu_to_init': True, - 'qos':qos, - # RTAB-Map's internal parameters are strings: - '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' - }], - remappings=[ - ('scan_cloud', scan_topic), - ('imu', imu_filtered_topic) - ]), - - Node( - package='rtabmap_slam', executable='rtabmap', output='screen', - parameters=[{ - 'frame_id':frame_id, - 'subscribe_depth':False, - 'subscribe_rgb':False, - 'subscribe_scan_cloud':True, - 'approx_sync':False, - 'wait_for_transform':0.2, - 'qos':qos, - # RTAB-Map's internal parameters are strings: - 'RGBD/ProximityMaxGraphDepth': '0', - 'RGBD/ProximityPathMaxNeighbors': '1', - 'RGBD/AngularUpdate': '0.05', - 'RGBD/LinearUpdate': '0.05', - 'RGBD/CreateOccupancyGrid': 'false', - 'Mem/NotLinkedNodesKept': 'false', - 'Mem/STMSize': '30', - 'Mem/LaserScanNormalK': '20', - 'Reg/Strategy': '1', - 'Icp/VoxelSize': '0.1', - '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' - }], - remappings=[ - ('scan_cloud', 'odom_filtered_input_scan'), # Subscribe to deskewed scans from icp_odometry - ('imu', imu_filtered_topic) - ], - arguments=[ - '-d' # This will delete the previous database (~/.ros/rtabmap.db) - ]), - - Node( - package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[{ - 'frame_id':frame_id, - 'odom_frame_id':'odom', - 'subscribe_odom_info':True, - 'subscribe_scan_cloud':True, - 'approx_sync':False, - 'qos':qos, - }], - remappings=[ - ('scan_cloud', 'odom_filtered_input_scan') - ]), - ] - -def generate_launch_description(): - - return LaunchDescription([ - - # Launch arguments - DeclareLaunchArgument( - 'qos', default_value='1', - description='Quality of Service: 0=system default, 1=reliable, 2=best effort'), - - DeclareLaunchArgument( - 'frame_id', default_value='velodyne', - description='Base frame of the robot'), - - DeclareLaunchArgument( - 'imu_topic', default_value='/imu/data', - description='Imu topic name'), - - DeclareLaunchArgument( - 'scan_topic', default_value='/velodyne_points', - description='Scan point cloud topic name'), - - OpaqueFunction(function=launch_setup) - ]) - - diff --git a/rtabmap_examples/launch/vlp16_imu_assemble.launch.py b/rtabmap_examples/launch/vlp16_imu_assemble.launch.py deleted file mode 100644 index a5f8988d8..000000000 --- a/rtabmap_examples/launch/vlp16_imu_assemble.launch.py +++ /dev/null @@ -1,201 +0,0 @@ -# Example: -# $ ros2 launch rtabmap_examples vlp16_imu_assemble.launch.py - -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource - -def launch_setup(context, *args, **kwargs): - - qos = LaunchConfiguration('qos') - scan_topic = LaunchConfiguration('scan_topic') - deskewed_scan_topic = ''.join([scan_topic.perform(context), "/deskewed"]) - imu_topic = LaunchConfiguration('imu_topic') - imu_filtered_topic = ''.join([imu_topic.perform(context), "/filtered"]) - frame_id = LaunchConfiguration('frame_id') - frame_id_stabilized = ''.join([frame_id.perform(context), "_stabilized"]) - - return [ - ################################# - # Hardware specific nodes - BEGIN - ################################# - IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('velodyne_driver'), 'launch'), - '/velodyne_driver_node-VLP16-launch.py']), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('velodyne_pointcloud'), 'launch'), - '/velodyne_transform_node-VLP16-launch.py']), - ), - - # The IMU driver publishing /imu/data (adjust imu_topic argument if your imu publishes another topic name) - Node(package='imu_brick', executable='imu_brick_node'), - - # Static transform between velodyne and imu frame - Node(package='tf2_ros', executable='static_transform_publisher', arguments=["0", "0", "0.05", "-1.57", "0", "0", "velodyne", "imu_link"]), - ################################# - # Hardware specific nodes - END - ################################# - - # Nodes below should work "as is" with different hardware - - # You can skip the imu filter node if your imu data has already the orientation computed - Node( - package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen', - parameters=[{ - 'use_mag':False, - 'world_frame':'enu', - 'publish_tf':False}], - remappings=[ - ('imu/data_raw', imu_topic), - ('imu/data', imu_filtered_topic) - ]), - - Node( - package='rtabmap_util', executable='imu_to_tf', output='screen', - parameters=[{ - 'fixed_frame_id':frame_id_stabilized, - 'base_frame_id':frame_id, - 'wait_for_transform_duration': 0.001}], - remappings=[ - ('imu/data', imu_filtered_topic) - ]), - - # External Deskewing - Node( - package='rtabmap_util', executable='lidar_deskewing', output='screen', - parameters=[{ - 'fixed_frame_id':frame_id_stabilized}], - remappings=[ - ('input_cloud', scan_topic) - ]), - - Node( - package='rtabmap_odom', executable='icp_odometry', output='screen', - parameters=[{ - 'frame_id':frame_id, - 'odom_frame_id':'odom', - 'guess_frame_id':frame_id_stabilized, - 'wait_for_transform':0.2, - 'expected_update_rate':15.0, - 'wait_imu_to_init': True, - 'qos':qos, - # RTAB-Map's internal parameters are strings: - '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' - }], - remappings=[ - ('scan_cloud', deskewed_scan_topic), # Subscribing to deskewed scan topic - ('imu', imu_filtered_topic) - ]), - - #Assemble deskewed scans based on icp odometry - Node( - package='rtabmap_util', executable='point_cloud_assembler', output='screen', - parameters=[{'assembling_time': 1.0, - 'fixed_frame_id': ""}], - remappings=[('cloud', deskewed_scan_topic)]), - - Node( - package='rtabmap_slam', executable='rtabmap', output='screen', - parameters=[{ - 'frame_id':frame_id, - 'subscribe_depth':False, - 'subscribe_rgb':False, - 'subscribe_scan_cloud':True, - 'approx_sync':False, - 'wait_for_transform':0.2, - 'qos':qos, - # RTAB-Map's internal parameters are strings: - 'Rtabmap/DetectionRate': '0', # Rate fixed by assembling time above (1 Hz) - 'RGBD/ProximityMaxGraphDepth': '0', - 'RGBD/ProximityPathMaxNeighbors': '1', - 'RGBD/AngularUpdate': '0.05', - 'RGBD/LinearUpdate': '0.05', - 'RGBD/CreateOccupancyGrid': 'false', - 'Mem/NotLinkedNodesKept': 'false', - 'Mem/STMSize': '30', - 'Mem/LaserScanNormalK': '20', - 'Reg/Strategy': '1', - 'Icp/VoxelSize': '0.1', - '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' - }], - remappings=[ - ('scan_cloud', 'assembled_cloud'), # We subscribe to assembled scans - ('imu', imu_filtered_topic) - ], - arguments=[ - '-d' # This will delete the previous database (~/.ros/rtabmap.db) - ]), - - Node( - package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[{ - 'frame_id':frame_id, - 'odom_frame_id':'odom', - 'subscribe_odom_info':True, - 'subscribe_scan_cloud':True, - 'approx_sync':False, - 'qos':qos, - }], - remappings=[ - ('scan_cloud', deskewed_scan_topic) - ]), - ] - -def generate_launch_description(): - - return LaunchDescription([ - - # Launch arguments - DeclareLaunchArgument( - 'qos', default_value='1', - description='Quality of Service: 0=system default, 1=reliable, 2=best effort'), - - DeclareLaunchArgument( - 'frame_id', default_value='velodyne', - description='Base frame of the robot'), - - DeclareLaunchArgument( - 'imu_topic', default_value='/imu/data', - description='Imu topic name'), - - DeclareLaunchArgument( - 'scan_topic', default_value='/velodyne_points', - description='Scan point cloud topic name'), - - OpaqueFunction(function=launch_setup) - ]) - - diff --git a/rtabmap_examples/launch/vlp16_zed.launch.py b/rtabmap_examples/launch/vlp16_zed.launch.py new file mode 100644 index 000000000..67267e1ff --- /dev/null +++ b/rtabmap_examples/launch/vlp16_zed.launch.py @@ -0,0 +1,121 @@ +# Example using zed odometry for lidar deskewing: +# $ ros2 launch rtabmap_examples vlp16_zed.launch.py camera_model:=zed2i +# +# To use only zed's imu for deskewing: +# $ ros2 launch rtabmap_examples vlp16_zed.launch.py camera_model:=zed2i use_zed_odometry:=false +# + + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription, LaunchContext +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + +import tempfile + +def launch_setup(context: LaunchContext, *args, **kwargs): + + assemble = LaunchConfiguration('assemble').perform(context) + assemble = assemble == 'true' or assemble == 'True' + + lidar3d_launch_file = 'lidar3d.launch.py' + if assemble: + lidar3d_launch_file = 'lidar3d_assemble.launch.py' + + use_zed_odometry = LaunchConfiguration('use_zed_odometry').perform(context) + use_zed_odometry = use_zed_odometry == 'true' or use_zed_odometry == 'True' + + fixed_frame_id = '' + if use_zed_odometry: + fixed_frame_id = 'odom' + + # Hack to override grab_resolution parameter without changing any files + with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file: + zed_override_file.write("---\n"+ + "/**:\n"+ + " ros__parameters:\n"+ + " general:\n"+ + " grab_resolution: 'VGA'") + + return [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('velodyne_driver'), 'launch'), + '/velodyne_driver_node-VLP16-launch.py']), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('velodyne_pointcloud'), 'launch'), + '/velodyne_transform_node-VLP16-launch.py']), + ), + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('zed_wrapper'), 'launch'), + '/zed_camera.launch.py']), + launch_arguments={'camera_model': LaunchConfiguration('camera_model'), + 'ros_params_override_path': zed_override_file.name, + 'publish_tf': LaunchConfiguration('use_zed_odometry'), # publish VIO frame + 'publish_map_tf': 'false'}.items(), + ), + + # Static transform between zed and velodyne frame (zed will be our base frame because VIO is already linked to it) + Node(package='tf2_ros', executable='static_transform_publisher', arguments=["0", "0", "-0.05", "0", "0", "0", "zed_camera_link", "velodyne"]), + + # Sync rgb/depth/camera_info together + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[{'approx_sync': False}], + remappings=[('rgb/image', '/zed/zed_node/rgb/image_rect_color'), + ('rgb/camera_info', '/zed/zed_node/rgb/camera_info'), + ('depth/image', '/zed/zed_node/depth/depth_registered')]), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('rtabmap_examples'), 'launch'), + '/', lidar3d_launch_file]), + launch_arguments={'voxel_size': LaunchConfiguration('voxel_size'), + 'localization': LaunchConfiguration('localization'), + 'frame_id': 'zed_camera_link', + 'lidar_topic': 'velodyne_points', + 'imu_topic': '/zed/zed_node/imu/data', + 'rgbd_image_topic': 'rgbd_image', + 'fixed_frame_id': fixed_frame_id}.items()), + ] + +def generate_launch_description(): + return LaunchDescription([ + # Launch arguments + DeclareLaunchArgument( + 'camera_model', default_value='', + description="[REQUIRED] The model of the camera. Using a wrong camera model can disable camera features. Valid choices are: ['zed', 'zedm', 'zed2', 'zed2i', 'zedx', 'zedxm', 'virtual']"), + + DeclareLaunchArgument( + 'use_zed_odometry', default_value='true', + description='Use ZED\'s odometry for deskewing.'), + + DeclareLaunchArgument( + 'qos', default_value='1', + description='Quality of Service: 0=system default, 1=reliable, 2=best effort'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Localization mode.'), + + DeclareLaunchArgument( + 'voxel_size', default_value='0.1', + description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'), + + DeclareLaunchArgument( + 'assemble', default_value='false', + description='Assemble ALL lidar scans.'), + + OpaqueFunction(function=launch_setup), + ]) \ No newline at end of file diff --git a/rtabmap_examples/launch/vlp16_zed_vio_assemble.launch.py b/rtabmap_examples/launch/vlp16_zed_vio_assemble.launch.py deleted file mode 100644 index ea77bf84b..000000000 --- a/rtabmap_examples/launch/vlp16_zed_vio_assemble.launch.py +++ /dev/null @@ -1,185 +0,0 @@ -# Example: -# $ ros2 launch rtabmap_examples vlp16_zed_vio_assemble.launch.py - -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource - -import tempfile - -def generate_launch_description(): - - qos = LaunchConfiguration('qos') - - # Hack to override grab_resolution parameter without changing any files - with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file: - zed_override_file.write("---\n"+ - "/**:\n"+ - " ros__parameters:\n"+ - " general:\n"+ - " grab_resolution: 'VGA'") - - shared_icp_parameters ={ - 'Icp/PointToPlane': 'true', - 'Icp/Iterations': '10', - 'Icp/VoxelSize': '0.1', - 'Icp/Epsilon': '0.001', - 'Icp/PointToPlaneK': '20', - 'Icp/PointToPlaneRadius': '0', - 'Icp/MaxTranslation': '3', - 'Icp/MaxCorrespondenceDistance': '1', - 'Icp/Strategy': '1', - 'Icp/OutlierRatio': '0.7', - } - - return LaunchDescription([ - - # Launch arguments - DeclareLaunchArgument( - 'qos', default_value='1', - description='Quality of Service: 0=system default, 1=reliable, 2=best effort'), - - DeclareLaunchArgument( - 'camera_model', default_value='', - description="[REQUIRED] The model of the camera. Using a wrong camera model can disable camera features. Valid choices are: ['zed', 'zedm', 'zed2', 'zed2i', 'zedx', 'zedxm', 'virtual']"), - - ################################# - # Hardware specific nodes - BEGIN - ################################# - IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('velodyne_driver'), 'launch'), - '/velodyne_driver_node-VLP16-launch.py']), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('velodyne_pointcloud'), 'launch'), - '/velodyne_transform_node-VLP16-launch.py']), - ), - - # Launch camera driver - IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join( - get_package_share_directory('zed_wrapper'), 'launch'), - '/zed_camera.launch.py']), - launch_arguments={'camera_model': LaunchConfiguration('camera_model'), - 'ros_params_override_path': zed_override_file.name, - 'publish_tf': 'true', # publish VIO frame - 'publish_map_tf': 'false'}.items(), - ), - - # Static transform between zed and velodyne frame (zed will be our base frame because VIO is already linked to it) - Node(package='tf2_ros', executable='static_transform_publisher', arguments=["0", "0", "-0.05", "0", "0", "0", "zed_camera_link", "velodyne"]), - ################################# - # Hardware specific nodes - END - ################################# - - # Nodes below should work "as is" with different hardware - - # Sync rgb/depth/camera_info together - Node( - package='rtabmap_sync', executable='rgbd_sync', output='screen', - parameters=[{'approx_sync': False}], - remappings=[('rgb/image', '/zed/zed_node/rgb/image_rect_color'), - ('rgb/camera_info', '/zed/zed_node/rgb/camera_info'), - ('depth/image', '/zed/zed_node/depth/depth_registered')]), - - # External Deskewing using VIO - Node( - package='rtabmap_util', executable='lidar_deskewing', output='screen', - parameters=[{ - 'fixed_frame_id':'odom', - 'wait_for_transform':0.1}], - remappings=[ - ('input_cloud', 'velodyne_points') - ]), - - Node( - package='rtabmap_odom', executable='icp_odometry', output='screen', - parameters=[{ - 'frame_id':'zed_camera_link', - 'odom_frame_id':'icp_odom', - 'guess_frame_id':'odom', - 'wait_for_transform':0.2, - 'expected_update_rate':15.0, - 'wait_imu_to_init': True, - 'qos':qos, - # RTAB-Map's internal parameters are strings: - 'Odom/ScanKeyFrameThr': '0.4', - 'OdomF2M/ScanSubtractRadius': '0.1', - 'OdomF2M/ScanMaxSize': '15000', - 'OdomF2M/BundleAdjustment': 'false', - 'Icp/CorrespondenceRatio': '0.01' - }, shared_icp_parameters], - remappings=[ - ('scan_cloud', 'velodyne_points/deskewed'), # Subscribing to deskewed scan topic - ('imu', '/zed/zed_node/imu/data'), - ('odom', 'icp_odom') - ]), - - #Assemble deskewed scans based on icp odometry - Node( - package='rtabmap_util', executable='point_cloud_assembler', output='screen', - parameters=[{'assembling_time': 1.0, - 'fixed_frame_id': ""}], - remappings=[('cloud', 'velodyne_points/deskewed'), - ('odom', 'icp_odom')]), - - Node( - package='rtabmap_slam', executable='rtabmap', output='screen', - parameters=[{ - 'frame_id':'zed_camera_link', - 'subscribe_rgbd': True, - 'subscribe_depth':False, - 'subscribe_rgb':False, - 'subscribe_scan_cloud':True, - 'approx_sync':True, - 'wait_for_transform':0.2, - 'qos':qos, - 'topic_queue_size': 30, - 'sync_queue_size': 20, - # RTAB-Map's internal parameters are strings: - 'Rtabmap/DetectionRate': '0', # Rate fixed by assembling time above (1 Hz) - 'RGBD/ProximityMaxGraphDepth': '0', - 'RGBD/ProximityPathMaxNeighbors': '1', - 'RGBD/AngularUpdate': '0.05', - 'RGBD/LinearUpdate': '0.05', - 'RGBD/CreateOccupancyGrid': 'false', - 'Mem/NotLinkedNodesKept': 'false', - 'Mem/STMSize': '30', - 'Mem/LaserScanNormalK': '20', - 'Reg/Strategy': '1', - 'Icp/CorrespondenceRatio': '0.2' - }, shared_icp_parameters], - remappings=[ - ('scan_cloud', 'assembled_cloud'), # We subscribe to assembled scans - ('imu', '/zed/zed_node/imu/data'), - ('odom', '/icp_odom') - ], - arguments=[ - '-d' # This will delete the previous database (~/.ros/rtabmap.db) - ]), - - Node( - package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[{ - 'frame_id':'zed_camera_link', - 'odom_frame_id':'icp_odom', - 'subscribe_odom_info':True, - 'subscribe_scan_cloud':True, - 'approx_sync':False, - 'qos':qos, - }], - remappings=[ - ('scan_cloud', 'velodyne_points/deskewed'), - ]), - ] - - ) \ No newline at end of file