Added some composition examples

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")]]),
+    ])

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),
+    ])

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),
+    ])

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),
+    ])

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

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)
+    ])