diff --git a/rtabmap_demos/launch/stereo_outdoor_demo_composition.launch.py b/rtabmap_demos/launch/stereo_outdoor_demo_composition.launch.py new file mode 100644 index 00000000..4510d659 --- /dev/null +++ b/rtabmap_demos/launch/stereo_outdoor_demo_composition.launch.py @@ -0,0 +1,201 @@ +# Requirements: +# Download one or both rosbags: +# * stereo_outdoorA.db3: https://drive.google.com/file/d/1O7mCXg_sw4tZY1S88a-n96O6OulmqvqI/view?usp=drive_link +# * stereo_outdoorB.db3: https://drive.google.com/file/d/1mSu7418Fkbe-hIz2-3Mi936PrWuD2un_/view?usp=drive_link +# +# This is the "composition" variant of stereo_outdoor_demo.launch.py: the whole +# pipeline (image_proc rectification, stereo synchronization, visual odometry +# and SLAM) runs as composable nodes in a single component container +# (rtabmap_container). We can set 'use_intra_process_comms' on all of them. +# That way images are passed between rectify -> disparity/sync -> odometry -> +# SLAM by pointer, without inter-process serialization/copies. +# +# Example: +# +# SLAM: +# $ ros2 launch rtabmap_demos stereo_outdoor_demo_composition.launch.py rviz:=true rtabmap_viz:=true +# +# Rosbag: +# $ ros2 bag play stereo_outdoorA.db3 --clock +# when done, you can play the secon bag: +# $ ros2 bag play stereo_outdoorB.db3 --clock +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node, SetParameter, ComposableNodeContainer, LoadComposableNodes +from launch_ros.descriptions import ComposableNode +import os +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + localization = LaunchConfiguration('localization') + + parameters={ + 'frame_id':'base_footprint', + 'subscribe_rgbd':True, + 'approx_sync':False, # odom is generated from images, so we can exactly sync all inputs + 'map_negative_poses_ignored':True, + 'subscribe_odom_info': True, + # RTAB-Map's internal parameters should be strings + 'OdomF2M/MaxSize': '1000', + 'GFTT/MinDistance': '10', + 'GFTT/QualityLevel': '0.00001', + #'Kp/DetectorStrategy': '6', # Uncommment to match ros1 noetic results, but opencv should be built with xfeatures2d + #'Vis/FeatureType': '6' # Uncommment to match ros1 noetic results, but opencv should be built with xfeatures2d + } + + remappings=[ + ('rgbd_image', '/stereo_camera/rgbd_image'), + ('odom', '/vo')] + + # Enable zero-copy intra-process communication between all composable nodes + # loaded in the container. + intra_process = [{'use_intra_process_comms': True}] + + config_rviz = os.path.join( + get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz' + ) + + # ---- image_proc rectification per camera ---- + def image_proc_nodes(side, color=False): + ns = 'stereo_camera/' + side + rectify = ComposableNode( + package='image_proc', plugin='image_proc::RectifyNode', + name='rectify_color_node' if color else 'rectify_mono_node', namespace=ns, + remappings=[ + ('image', 'image_color' if color else 'image_mono'), + ('camera_info', 'camera_info_throttle'), + ('image_rect', 'image_rect_color' if color else 'image_rect')], + extra_arguments=intra_process) + return [ + ComposableNode( + package='image_proc', plugin='image_proc::DebayerNode', + name='debayer_node', namespace=ns, + extra_arguments=intra_process), + rectify, + ] + + # ---- rtabmap pipeline (always-on nodes) ---- + rtabmap_nodes = [ + # Synchronize stereo data together in a single topic + # Issue: stereo_img_proc doesn't produce color and + # grayscale images exactly the same (there is a small + # vertical shift with color), we should use grayscale for + # left and right images to get similar results than on ros1 noetic. + ComposableNode( + package='rtabmap_sync', plugin='rtabmap_sync::StereoSync', + namespace='stereo_camera', + remappings=[ + ('left/image_rect', 'left/image_rect'), + ('right/image_rect', 'right/image_rect'), + ('left/camera_info', 'left/camera_info_throttle'), + ('right/camera_info', 'right/camera_info_throttle')], + extra_arguments=intra_process), + + # Visual odometry + ComposableNode( + package='rtabmap_odom', plugin='rtabmap_odom::StereoOdometry', + parameters=[parameters], + remappings=remappings, + extra_arguments=intra_process), + ] + + # Name of the shared component container. + container_name = '/rtabmap_container' + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument('rtabmap_viz', default_value='false', description='Launch RTAB-Map UI (optional).'), + DeclareLaunchArgument('rviz', default_value='true', description='Launch RVIZ (optional).'), + DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'), + DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'), + + SetParameter(name='use_sim_time', value=True), + + # Nodes to launch + + # Uncompress images for stereo_image_rect and remap to expected names from stereo_image_proc. + Node( + package='image_transport', executable='republish', name='republish_left', output='screen', + namespace='stereo_camera', + arguments=['compressed', 'raw'], + remappings=[('in/compressed', 'left/image_raw_throttle/compressed'), + ('out', 'left/image_raw')]), + Node( + package='image_transport', executable='republish', name='republish_right', output='screen', + namespace='stereo_camera', + arguments=['compressed', 'raw'], + remappings=[('in/compressed', 'right/image_raw_throttle/compressed'), + ('out', 'right/image_raw')]), + + # Single component container holding the whole pipeline. All nodes set + # use_intra_process_comms=True, so images are passed by pointer. + ComposableNodeContainer( + name='rtabmap_container', + namespace='', + package='rclcpp_components', + executable='component_container', + output='screen', + composable_node_descriptions= + image_proc_nodes('left') + + image_proc_nodes('right') + + rtabmap_nodes), + + # SLAM mode (loaded into the shared container): + # Note: latch=False. rtabmap latches its map/cloud/octomap/mapGraph + # topics by default (transient_local QoS), which is incompatible with + # intra-process comms ("intraprocess communication allowed only with + # volatile durability"). Setting latch=False makes those topics volatile + # so the node can join the zero-copy container. Trade-off: viewers that + # start after a map is published won't get the retained last message, + # but rtabmap republishes the map as it updates. + LoadComposableNodes( + condition=UnlessCondition(localization), + target_container=container_name, + composable_node_descriptions=[ + ComposableNode( + package='rtabmap_slam', plugin='rtabmap_slam::CoreWrapper', + parameters=[parameters, + {'delete_db_on_start': True, # Equivalent of '-d': delete the previous database (~/.ros/rtabmap.db) + 'latch': False}], + remappings=remappings, + extra_arguments=intra_process), + ]), + + # Localization mode (loaded into the shared container): + LoadComposableNodes( + condition=IfCondition(localization), + target_container=container_name, + composable_node_descriptions=[ + ComposableNode( + package='rtabmap_slam', plugin='rtabmap_slam::CoreWrapper', + parameters=[parameters, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True', + 'latch': False}], # volatile QoS, see SLAM-mode note above + remappings=remappings, + extra_arguments=intra_process), + ]), + + # Visualization: + # Note: rtabmap_viz is launched as a standalone node, not as a component + # in the container above. It is a Qt application and its UI must run in + # the process main thread, while components are loaded in container + # worker threads. So it cannot be composed and does not benefit from + # intra-process comms here (the same applies to rviz2). + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration("rtabmap_viz")), + parameters=[parameters, + {"odometry_node_name": 'stereo_odometry'}], + remappings=remappings), + Node( + package='rviz2', executable='rviz2', name="rviz2", output='screen', + condition=IfCondition(LaunchConfiguration("rviz")), + arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]), + ]) diff --git a/rtabmap_examples/launch/realsense_d435i_color_composition.launch.py b/rtabmap_examples/launch/realsense_d435i_color_composition.launch.py new file mode 100644 index 00000000..3c1c0111 --- /dev/null +++ b/rtabmap_examples/launch/realsense_d435i_color_composition.launch.py @@ -0,0 +1,130 @@ +# Requirements: +# A realsense D435i +# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) +# Example: +# $ ros2 launch rtabmap_examples realsense_d435i_color_composition.launch.py +# +# This is the "composition" variant of realsense_d435i_color.launch.py: the +# camera driver, IMU filter, RGB-D odometry and SLAM all run as composable +# nodes in a single component container (rtabmap_container) with +# use_intra_process_comms enabled, so messages can be passed by pointer instead +# of being serialized/copied between processes. +# +# As in the non-composed example, the color stream is used as RGB and paired +# with the depth aligned to color (align_depth.enable), with the IR emitter on. +# +# Notes: +# * Unlike the non-composed example, we do NOT include realsense2's rs_launch.py: +# that launch file always starts the camera as a standalone node and exposes +# no way to load it into an existing container. Instead we instantiate the +# camera component (realsense2_camera::RealSenseNodeFactory) ourselves, the +# same way realsense's own rs_intra_process_demo_launch.py does. +# * ComposableNode has no "arguments" field, so the args/odom_args/-d +# command-line mechanism of the non-composed example is not available here. +# To override rtabmap parameters, add them directly to the 'parameters' dict +# below. '-d' (delete database on start) becomes the 'delete_db_on_start' +# parameter. +# +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.conditions import IfCondition +from launch_ros.actions import Node, ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + parameters={ + 'frame_id':'camera_link', + 'subscribe_depth':True, + 'subscribe_odom_info':True, + 'approx_sync':False, + 'wait_imu_to_init':True} + + remappings=[ + ('imu', '/imu/data'), + ('rgb/image', '/camera/color/image_raw'), + ('rgb/camera_info', '/camera/color/camera_info'), + ('depth/image', '/camera/aligned_depth_to_color/image_raw')] + + # Enable zero-copy intra-process communication on every composable node. + intra_process = [{'use_intra_process_comms': True}] + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'unite_imu_method', default_value='2', + description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'), + DeclareLaunchArgument( + 'rtabmap_viz', default_value='true', description='Launch RTAB-Map UI (optional).'), + + # Single component container holding the whole pipeline. + ComposableNodeContainer( + name='rtabmap_container', + namespace='', + package='rclcpp_components', + executable='component_container', + output='screen', + composable_node_descriptions=[ + + # Camera driver (replaces the rs_launch.py include). + ComposableNode( + package='realsense2_camera', plugin='realsense2_camera::RealSenseNodeFactory', + name='camera', namespace='', + parameters=[{ + 'enable_gyro': True, + 'enable_accel': True, + 'unite_imu_method': LaunchConfiguration('unite_imu_method'), + 'align_depth.enable': True, + 'enable_sync': True, + 'rgb_camera.profile': '640x360x30', + 'depth_module.emitter_enabled': 1}], # Make sure IR emitter is enabled + extra_arguments=intra_process), + + # Compute quaternion of the IMU + ComposableNode( + package='imu_filter_madgwick', plugin='ImuFilterMadgwickRos', + name='imu_filter', namespace='', + parameters=[{'use_mag': False, + 'world_frame':'enu', + 'publish_tf':False}], + remappings=[('imu/data_raw', '/camera/imu')], + extra_arguments=intra_process), + + # RGB-D odometry (color + depth aligned to color) + ComposableNode( + package='rtabmap_odom', plugin='rtabmap_odom::RGBDOdometry', + parameters=[parameters], + remappings=remappings, + extra_arguments=intra_process), + + # SLAM + # Note: latch=False. rtabmap latches its map/cloud/octomap/mapGraph + # topics by default (transient_local QoS), which is incompatible + # with intra-process comms ("intraprocess communication allowed + # only with volatile durability"). latch=False makes them volatile + # so the node can join the zero-copy container. + ComposableNode( + package='rtabmap_slam', plugin='rtabmap_slam::CoreWrapper', + parameters=[parameters, + {'delete_db_on_start': True, # Equivalent of '-d' + 'latch': False}], + remappings=remappings, + extra_arguments=intra_process), + ]), + + # Visualization: + # Note: rtabmap_viz is launched as a standalone node, not as a component. + # It is a Qt application and its UI must run in the process main thread, + # while components run in container worker threads, so it cannot be + # composed (the same applies to rviz2). + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration('rtabmap_viz')), + parameters=[parameters], + remappings=remappings), + ]) diff --git a/rtabmap_examples/launch/realsense_d435i_infra_composition.launch.py b/rtabmap_examples/launch/realsense_d435i_infra_composition.launch.py new file mode 100644 index 00000000..92bb747e --- /dev/null +++ b/rtabmap_examples/launch/realsense_d435i_infra_composition.launch.py @@ -0,0 +1,132 @@ +# Requirements: +# A realsense D435i +# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) +# Example: +# $ ros2 launch rtabmap_examples realsense_d435i_infra_composition.launch.py +# +# This is the "composition" variant of realsense_d435i_infra.launch.py: the +# camera driver, IMU filter, RGB-D odometry and SLAM all run as composable +# nodes in a single component container (rtabmap_container) with +# use_intra_process_comms enabled, so messages can be passed by pointer instead +# of being serialized/copied between processes. +# +# As in the non-composed example, the left infrared image (infra1) is used as +# the grayscale "RGB" input and paired with the depth stream. This works because +# on the D435i the depth is computed in the left-infrared frame, so infra1 and +# depth share the same intrinsics/frame (already registered, no align needed). +# +# Notes: +# * Unlike the non-composed example, we do NOT include realsense2's rs_launch.py: +# that launch file always starts the camera as a standalone node and exposes +# no way to load it into an existing container. Instead we instantiate the +# camera component (realsense2_camera::RealSenseNodeFactory) ourselves, the +# same way realsense's own rs_intra_process_demo_launch.py does. +# * ComposableNode has no "arguments" field, so the args/odom_args/-d +# command-line mechanism of the non-composed example is not available here. +# To override rtabmap parameters, add them directly to the 'parameters' dict +# below. '-d' (delete database on start) becomes the 'delete_db_on_start' +# parameter. +# +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.conditions import IfCondition +from launch_ros.actions import Node, ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + parameters={ + 'frame_id':'camera_link', + 'subscribe_depth':True, + 'subscribe_odom_info':True, + 'approx_sync':False, + 'wait_imu_to_init':True} + + remappings=[ + ('imu', '/imu/data'), + ('rgb/image', '/camera/infra1/image_rect_raw'), + ('rgb/camera_info', '/camera/infra1/camera_info'), + ('depth/image', '/camera/depth/image_rect_raw')] + + # Enable zero-copy intra-process communication on every composable node. + intra_process = [{'use_intra_process_comms': True}] + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'unite_imu_method', default_value='2', + description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'), + DeclareLaunchArgument( + 'rtabmap_viz', default_value='true', description='Launch RTAB-Map UI (optional).'), + + # Single component container holding the whole pipeline. + ComposableNodeContainer( + name='rtabmap_container', + namespace='', + package='rclcpp_components', + executable='component_container', + output='screen', + composable_node_descriptions=[ + + # Camera driver (replaces the rs_launch.py include). + ComposableNode( + package='realsense2_camera', plugin='realsense2_camera::RealSenseNodeFactory', + name='camera', namespace='', + parameters=[{ + 'enable_gyro': True, + 'enable_accel': True, + 'unite_imu_method': LaunchConfiguration('unite_imu_method'), + 'enable_infra1': True, + 'enable_infra2': True, + 'enable_sync': True, + 'depth_module.emitter_enabled': 0}], # Hack to disable IR emitter + extra_arguments=intra_process), + + # Compute quaternion of the IMU + ComposableNode( + package='imu_filter_madgwick', plugin='ImuFilterMadgwickRos', + name='imu_filter', namespace='', + parameters=[{'use_mag': False, + 'world_frame':'enu', + 'publish_tf':False}], + remappings=[('imu/data_raw', '/camera/imu')], + extra_arguments=intra_process), + + # RGB-D odometry (infra1 as grayscale RGB + depth) + ComposableNode( + package='rtabmap_odom', plugin='rtabmap_odom::RGBDOdometry', + parameters=[parameters], + remappings=remappings, + extra_arguments=intra_process), + + # SLAM + # Note: latch=False. rtabmap latches its map/cloud/octomap/mapGraph + # topics by default (transient_local QoS), which is incompatible + # with intra-process comms ("intraprocess communication allowed + # only with volatile durability"). latch=False makes them volatile + # so the node can join the zero-copy container. + ComposableNode( + package='rtabmap_slam', plugin='rtabmap_slam::CoreWrapper', + parameters=[parameters, + {'delete_db_on_start': True, # Equivalent of '-d' + 'latch': False}], + remappings=remappings, + extra_arguments=intra_process), + ]), + + # Visualization: + # Note: rtabmap_viz is launched as a standalone node, not as a component. + # It is a Qt application and its UI must run in the process main thread, + # while components run in container worker threads, so it cannot be + # composed (the same applies to rviz2). + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration('rtabmap_viz')), + parameters=[parameters], + remappings=remappings), + ]) diff --git a/rtabmap_examples/launch/realsense_d435i_stereo_composition.launch.py b/rtabmap_examples/launch/realsense_d435i_stereo_composition.launch.py new file mode 100644 index 00000000..0417859f --- /dev/null +++ b/rtabmap_examples/launch/realsense_d435i_stereo_composition.launch.py @@ -0,0 +1,128 @@ +# Requirements: +# A realsense D435i +# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) +# Example: +# $ ros2 launch rtabmap_examples realsense_d435i_stereo_composition.launch.py +# +# This is the "composition" variant of realsense_d435i_stereo.launch.py: the +# camera driver, IMU filter, stereo odometry and SLAM all run as composable +# nodes in a single component container (rtabmap_container) with +# use_intra_process_comms enabled, so messages can be passed by pointer instead +# of being serialized/copied between processes. +# +# Notes: +# * Unlike the non-composed example, we do NOT include realsense2's rs_launch.py: +# that launch file always starts the camera as a standalone node and exposes +# no way to load it into an existing container. Instead we instantiate the +# camera component (realsense2_camera::RealSenseNodeFactory) ourselves, the +# same way realsense's own rs_intra_process_demo_launch.py does. +# * ComposableNode has no "arguments" field, so the args/odom_args/-d +# command-line mechanism of the non-composed example is not available here. +# To override rtabmap parameters, add them directly to the 'parameters' dict +# below. '-d' (delete database on start) becomes the 'delete_db_on_start' +# parameter. +# +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.conditions import IfCondition +from launch_ros.actions import Node, ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + parameters={ + 'frame_id':'camera_link', + 'subscribe_stereo':True, + 'subscribe_odom_info':True, + 'wait_imu_to_init':True} + + remappings=[ + ('imu', '/imu/data'), + ('left/image_rect', '/camera/infra1/image_rect_raw'), + ('left/camera_info', '/camera/infra1/camera_info'), + ('right/image_rect', '/camera/infra2/image_rect_raw'), + ('right/camera_info', '/camera/infra2/camera_info')] + + # Enable zero-copy intra-process communication on every composable node. + intra_process = [{'use_intra_process_comms': True}] + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'unite_imu_method', default_value='2', + description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'), + DeclareLaunchArgument( + 'rtabmap_viz', default_value='true', description='Launch RTAB-Map UI (optional).'), + + # Single component container holding the whole pipeline. + ComposableNodeContainer( + name='rtabmap_container', + namespace='', + package='rclcpp_components', + executable='component_container', + output='screen', + composable_node_descriptions=[ + + # Camera driver (replaces the rs_launch.py include). + ComposableNode( + package='realsense2_camera', plugin='realsense2_camera::RealSenseNodeFactory', + name='camera', namespace='', + parameters=[{ + 'enable_gyro': True, + 'enable_accel': True, + 'unite_imu_method': LaunchConfiguration('unite_imu_method'), + 'enable_infra1': True, + 'enable_infra2': True, + 'enable_sync': True, + 'depth_module.emitter_enabled': 0}], # Hack to disable IR emitter + extra_arguments=intra_process), + + # Compute quaternion of the IMU + ComposableNode( + package='imu_filter_madgwick', plugin='ImuFilterMadgwickRos', + name='imu_filter', namespace='', + parameters=[{'use_mag': False, + 'world_frame':'enu', + 'publish_tf':False}], + remappings=[('imu/data_raw', '/camera/imu')], + extra_arguments=intra_process), + + # Stereo odometry + ComposableNode( + package='rtabmap_odom', plugin='rtabmap_odom::StereoOdometry', + parameters=[parameters], + remappings=remappings, + extra_arguments=intra_process), + + # SLAM + # Note: latch=False. rtabmap latches its map/cloud/octomap/mapGraph + # topics by default (transient_local QoS), which is incompatible + # with intra-process comms ("intraprocess communication allowed + # only with volatile durability"). latch=False makes them volatile + # so the node can join the zero-copy container. + ComposableNode( + package='rtabmap_slam', plugin='rtabmap_slam::CoreWrapper', + parameters=[parameters, + {'delete_db_on_start': True, # Equivalent of '-d' + 'latch': False}], + remappings=remappings, + extra_arguments=intra_process), + ]), + + # Visualization: + # Note: rtabmap_viz is launched as a standalone node, not as a component. + # It is a Qt application and its UI must run in the process main thread, + # while components run in container worker threads, so it cannot be + # composed (the same applies to rviz2). + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + condition=IfCondition(LaunchConfiguration('rtabmap_viz')), + parameters=[parameters, + {'odometry_node_name': "stereo_odometry"}], + remappings=remappings), + ]) diff --git a/rtabmap_examples/launch/zed.launch.py b/rtabmap_examples/launch/zed.launch.py index 80ce57a5..2b95d161 100644 --- a/rtabmap_examples/launch/zed.launch.py +++ b/rtabmap_examples/launch/zed.launch.py @@ -23,13 +23,26 @@ remappings = [] def launch_setup(context: LaunchContext, *args, **kwargs): - # Hack to override grab_resolution parameter without changing any files + use_zed_odometry = LaunchConfiguration('use_zed_odometry').perform(context) in ["True", "true"] + + # Override some ZED parameters without changing any files: + # * grab_resolution: VGA + # * pos_tracking_enabled: disabled when rtabmap computes the odometry, so + # the ZED node does not publish the odom->camera_link TF (which would + # conflict with rtabmap's odometry). We still set publish_tf:=true below + # so the ZED node keeps broadcasting the IMU TF, which rtabmap needs + # (wait_imu_to_init). sensors.publish_imu_tf is ignored when publish_tf + # is false, and the IMU frame is not in the ZED URDF, so this is the only + # way to get the IMU TF while rtabmap owns the odometry. + pos_tracking_enabled = 'true' if use_zed_odometry else 'false' 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'") + " grab_resolution: 'VGA'\n"+ + " pos_tracking:\n"+ + " pos_tracking_enabled: "+pos_tracking_enabled) parameters=[{'frame_id':'zed_camera_link', 'subscribe_rgbd':True, @@ -38,7 +51,7 @@ def launch_setup(context: LaunchContext, *args, **kwargs): remappings=[('imu', '/zed/zed_node/imu/data')] - if LaunchConfiguration('use_zed_odometry').perform(context) in ["True", "true"]: + if use_zed_odometry: remappings.append(('odom', '/zed/zed_node/odom')) else: parameters.append({'subscribe_odom_info': True}) @@ -51,7 +64,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs): '/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_tf': 'true', + 'publish_imu_tf': 'true', 'publish_map_tf': 'false'}.items(), ), @@ -59,8 +73,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs): Node( package='rtabmap_sync', executable='rgbd_sync', output='screen', parameters=parameters, - remappings=[('rgb/image', '/zed/zed_node/rgb/image_rect_color'), - ('rgb/camera_info', '/zed/zed_node/rgb/camera_info'), + remappings=[('rgb/image', '/zed/zed_node/rgb/color/rect/image'), + ('rgb/camera_info', '/zed/zed_node/rgb/color/rect/camera_info'), ('depth/image', '/zed/zed_node/depth/depth_registered')]), # Visual odometry diff --git a/rtabmap_examples/launch/zed_composition.launch.py b/rtabmap_examples/launch/zed_composition.launch.py new file mode 100644 index 00000000..2045a271 --- /dev/null +++ b/rtabmap_examples/launch/zed_composition.launch.py @@ -0,0 +1,164 @@ +# Requirements: +# A ZED camera +# Install zed ros2 wrapper package (https://github.com/stereolabs/zed-ros2-wrapper) +# Example: +# $ ros2 launch rtabmap_examples zed_composition.launch.py camera_model:=zed2i +# +# This is the "composition" variant of zed.launch.py: the ZED driver, RGB-D +# synchronization, visual odometry and SLAM all run as composable nodes in a +# single component container with use_intra_process_comms enabled, so messages +# can be passed by pointer instead of being serialized/copied between processes. +# +# Notes: +# * The ZED wrapper's zed_camera.launch.py already creates its own component +# container ("zed_container") and loads the ZedCamera component into it with +# intra-process comms enabled by default (enable_ipc:=true). So instead of +# creating our own container, we let the ZED wrapper create it and load the +# rtabmap nodes into the SAME container (/zed/zed_container) with +# LoadComposableNodes. This assumes the default ZED namespace ("zed"), which +# is independent of camera_model. +# * ComposableNode has no "arguments" or "condition" field. So the '-d' +# argument becomes the 'delete_db_on_start' parameter, and the conditional +# odometry node is included in Python depending on use_zed_odometry. +# +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription, LaunchContext +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node, LoadComposableNodes +from launch_ros.descriptions import ComposableNode +from launch.actions import IncludeLaunchDescription, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource + +import tempfile + +def launch_setup(context: LaunchContext, *args, **kwargs): + + use_zed_odometry = LaunchConfiguration('use_zed_odometry').perform(context) in ["True", "true"] + + # Override some ZED parameters without changing any files: + # * grab_resolution: VGA + # * pos_tracking_enabled: disabled when rtabmap computes the odometry, so + # the ZED node does not publish the odom->camera_link TF (which would + # conflict with rtabmap's odometry). We still set publish_tf:=true below + # so the ZED node keeps broadcasting the IMU TF, which rtabmap needs + # (wait_imu_to_init). sensors.publish_imu_tf is ignored when publish_tf + # is false, and the IMU frame is not in the ZED URDF, so this is the only + # way to get the IMU TF while rtabmap owns the odometry. + pos_tracking_enabled = 'true' if use_zed_odometry else 'false' + 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'\n"+ + " pos_tracking:\n"+ + " pos_tracking_enabled: "+pos_tracking_enabled) + + # ZED topics are /zed/zed_node/* and the container created by the wrapper is + # /zed/zed_container (default ZED namespace "zed"). + zed_ns = '/zed/zed_node' + zed_container = '/zed/zed_container' + + parameters=[{'frame_id':'zed_camera_link', + 'subscribe_rgbd':True, + 'approx_sync':False, + 'wait_imu_to_init':True}] + + remappings=[('imu', zed_ns + '/imu/data')] + + if use_zed_odometry: + remappings.append(('odom', zed_ns + '/odom')) + else: + parameters.append({'subscribe_odom_info': True}) + + # Enable zero-copy intra-process communication on every composable node. + intra_process = [{'use_intra_process_comms': True}] + + # rtabmap nodes loaded into the ZED container. + composable_nodes = [ + # Sync rgb/depth/camera_info together + ComposableNode( + package='rtabmap_sync', plugin='rtabmap_sync::RGBDSync', + parameters=parameters, + remappings=[('rgb/image', zed_ns + '/rgb/color/rect/image'), + ('rgb/camera_info', zed_ns + '/rgb/color/rect/camera_info'), + ('depth/image', zed_ns + '/depth/depth_registered')], + extra_arguments=intra_process), + ] + + # Visual odometry (only when not using ZED's own odometry). ComposableNode + # has no 'condition', so we add it here based on use_zed_odometry. + if not use_zed_odometry: + composable_nodes.append( + ComposableNode( + package='rtabmap_odom', plugin='rtabmap_odom::RGBDOdometry', + parameters=parameters, + remappings=remappings, + extra_arguments=intra_process)) + + # VSLAM + # Note: latch=False. rtabmap latches its map/cloud/octomap/mapGraph topics + # by default (transient_local QoS), which is incompatible with intra-process + # comms ("intraprocess communication allowed only with volatile durability"). + # latch=False makes them volatile so the node can join the container. + composable_nodes.append( + ComposableNode( + package='rtabmap_slam', plugin='rtabmap_slam::CoreWrapper', + parameters=parameters + [{'delete_db_on_start': True, # Equivalent of '-d' + 'latch': False}], + remappings=remappings, + extra_arguments=intra_process)) + + return [ + # Launch camera driver. It creates the "zed_container" component + # container (enable_ipc:=true by default) and loads the ZedCamera + # component into it. + 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 must be true so the ZED node broadcasts the + # IMU TF (gated by publish_tf). The odom->camera_link TF is + # disabled via pos_tracking_enabled=false (override file) + # when rtabmap computes the odometry. + 'publish_tf': 'true', + 'publish_imu_tf': 'true', + 'publish_map_tf': 'false'}.items(), + ), + + # Load the rtabmap pipeline into the ZED container. + LoadComposableNodes( + target_container=zed_container, + composable_node_descriptions=composable_nodes), + + # Visualization + # Note: rtabmap_viz is a Qt application; its UI must run in the process + # main thread, while components run in container worker threads. So it + # cannot be composed and stays a standalone node. + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=parameters, + remappings=remappings) + ] + + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_zed_odometry', default_value='false', + description='Use zed\'s computed odometry instead of using rtabmap\'s odometry.'), + + 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']"), + + OpaqueFunction(function=launch_setup) + ])