fixed MapCloud rviz plugin with rtabmap namespace property. rtabmap.launch.py: added "rviz" argument to start RVIZ with preconfigured displays. point_cloud_xyzrgb: added RGBA encoding support for stereo.

launch/ros2/rtabmap.launch.py

@@ -3,6 +3,8 @@
 # "stdbuf -o L ros2 launch rtabmap_ros rtabmap.launch.py ..."
 #
 
+import os
+
 from launch import LaunchDescription, Substitution, LaunchContext
 from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, LogInfo, OpaqueFunction
 from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PythonExpression
@@ -10,6 +12,7 @@ from launch.conditions import IfCondition, UnlessCondition
 from launch_ros.actions import Node
 from launch_ros.actions import SetParameter
 from typing import Text
+from ament_index_python.packages import get_package_share_directory
 
 #Based on https://answers.ros.org/question/363763/ros2-how-best-to-conditionally-include-a-prefix-in-a-launchpy-file/
 class ConditionalText(Substitution):
@@ -304,9 +307,37 @@ def launch_setup(context, *args, **kwargs):
             condition=IfCondition(LaunchConfiguration("rtabmapviz")),
             arguments=[LaunchConfiguration("gui_cfg")],
             prefix=LaunchConfiguration('launch_prefix'),
-            namespace=LaunchConfiguration('namespace'))]
+            namespace=LaunchConfiguration('namespace')),
+        Node(
+            package='rviz2', executable='rviz2', output='screen',
+            condition=IfCondition(LaunchConfiguration("rviz")),
+            arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]),
+        Node(
+            package='rtabmap_ros', executable='point_cloud_xyzrgb', output='screen',
+            condition=IfCondition(LaunchConfiguration("rviz")),
+            parameters=[{
+                "decimation": 4,
+                "voxel_size": 0.0,
+                "approx_sync": LaunchConfiguration('approx_sync')
+            }],
+            remappings=[
+                ('left/image', LaunchConfiguration('left_image_topic_relay')),
+                ('right/image', LaunchConfiguration('right_image_topic_relay')),
+                ('left/camera_info', LaunchConfiguration('left_camera_info_topic')),
+                ('right/camera_info', LaunchConfiguration('right_camera_info_topic')),
+                ('rgb/image', LaunchConfiguration('rgb_topic_relay')),
+                ('depth/image', LaunchConfiguration('depth_topic_relay')),
+                ('rgb/camera_info', LaunchConfiguration('camera_info_topic')),
+                ('rgbd_image', LaunchConfiguration('rgbd_topic_relay')),
+                ('cloud', 'voxel_cloud')]),
+        ]
 
 def generate_launch_description():
+    
+    config_rviz = os.path.join(
+        get_package_share_directory('rtabmap_ros'), 'launch', 'config', 'rgbd.rviz'
+    )
+    
     return LaunchDescription([
         
         # Arguments
@@ -314,13 +345,15 @@ def generate_launch_description():
 
         DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'),
         DeclareLaunchArgument('rtabmapviz',   default_value='true',  description='Launch RTAB-Map UI (optional).'),
-        
+        DeclareLaunchArgument('rviz',         default_value='false', description='Launch RVIZ (optional).'),
+
         DeclareLaunchArgument('use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true'),
-        
+
         # Config files
-        DeclareLaunchArgument('cfg',     default_value='',                        description='To change RTAB-Map\'s parameters, set the path of config file (*.ini) generated by the standalone app.'),
-        DeclareLaunchArgument('gui_cfg', default_value='~/.ros/rtabmap_gui.ini',  description='Configuration path of rtabmapviz.'),
-        
+        DeclareLaunchArgument('cfg',      default_value='',                        description='To change RTAB-Map\'s parameters, set the path of config file (*.ini) generated by the standalone app.'),
+        DeclareLaunchArgument('gui_cfg',  default_value='~/.ros/rtabmap_gui.ini',  description='Configuration path of rtabmapviz.'),
+        DeclareLaunchArgument('rviz_cfg', default_value=config_rviz,               description='Configuration path of rviz2.'),
+
         DeclareLaunchArgument('frame_id',       default_value='base_link',          description='Fixed frame id of the robot (base frame), you may set "base_link" or "base_footprint" if they are published. For camera-only config, this could be "camera_link".'),
         DeclareLaunchArgument('odom_frame_id',  default_value='',                   description='If set, TF is used to get odometry instead of the topic.'),
         DeclareLaunchArgument('map_frame_id',   default_value='map',                description='Output map frame id (TF).'),