initial port of rtabmap.launch.py

2026-10-03 16:27:46 +08:00 · 2021-07-11 19:49:07 -04:00
parent 96c0733b0b
commit 11d8ebc339
9 changed files with 454 additions and 37 deletions
@@ -85,6 +85,7 @@ protected:
 	virtual void flushCallbacks() {}
 	tf2_ros::Buffer & tfBuffer() {return *tfBuffer_;}
 	const double & waitForTransform() const {return waitForTransform_;}
+	const int & queueSize() const {return queueSize_;}

 private:
 	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync);
@@ -113,6 +114,7 @@ private:
 	bool publishTf_;
 	double waitForTransform_;
 	bool publishNullWhenLost_;
+	int queueSize_;
 	rtabmap::ParametersMap parameters_;

 	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odomPub_;
@@ -114,7 +114,6 @@ private:
 	message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
 	typedef message_filters::sync_policies::ExactTime<rtabmap_ros::msg::RGBDImage, rtabmap_ros::msg::RGBDImage, rtabmap_ros::msg::RGBDImage, rtabmap_ros::msg::RGBDImage> MyExactSync4Policy;
 	message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
-	int queueSize_;
 };

 }
@@ -76,7 +76,6 @@ private:
 	typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo> MyExactSyncPolicy;
 	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
 	rclcpp::Subscription<rtabmap_ros::msg::RGBDImage>::SharedPtr rgbdSub_;
-	int queueSize_;
 };

 }
@@ -0,0 +1,395 @@
+
+from launch import LaunchDescription, Substitution, LaunchContext
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, LogInfo, OpaqueFunction
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PythonExpression
+from launch.conditions import IfCondition, UnlessCondition
+from launch_ros.actions import Node
+from typing import Text
+
+#Based on https://answers.ros.org/question/363763/ros2-how-best-to-conditionally-include-a-prefix-in-a-launchpy-file/
+class ConditionalText(Substitution):
+    def __init__(self, text_if, text_else, condition):
+        self.text_if = text_if
+        self.text_else = text_else
+        self.condition = condition
+
+    def perform(self, context: 'LaunchContext') -> Text:
+        if self.condition:
+            return self.text_if
+        else:
+            return self.text_else
+            
+class ConditionalBool(Substitution):
+    def __init__(self, text_if, text_else, condition):
+        self.text_if = text_if
+        self.text_else = text_else
+        self.condition = condition
+
+    def perform(self, context: 'LaunchContext') -> bool:
+        if self.condition:
+            return self.text_if
+        else:
+            return self.text_else
+            
+def launch_setup(context, *args, **kwargs):      
+               
+    return [
+        DeclareLaunchArgument('depth', default_value=ConditionalText('false', 'true', IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true'"]))._predicate_func(context)), description=''),
+        DeclareLaunchArgument('subscribe_rgb', default_value=LaunchConfiguration('depth'), description=''),
+        
+        #These arguments should not be modified directly, see referred topics without "_relay" suffix above
+        DeclareLaunchArgument('rgb_topic_relay',      default_value=ConditionalText(''.join([LaunchConfiguration('rgb_topic').perform(context), "_relay"]), LaunchConfiguration('rgb_topic').perform(context), LaunchConfiguration('compressed')), description='Should not be modified manually!'),
+        DeclareLaunchArgument('depth_topic_relay',      default_value=ConditionalText(''.join([LaunchConfiguration('depth_topic').perform(context), "_relay"]), LaunchConfiguration('depth_topic').perform(context), LaunchConfiguration('compressed')), description='Should not be modified manually!'),
+        DeclareLaunchArgument('left_image_topic_relay',      default_value=ConditionalText(''.join([LaunchConfiguration('left_image_topic').perform(context), "_relay"]), LaunchConfiguration('left_image_topic').perform(context), LaunchConfiguration('compressed')), description='Should not be modified manually!'),
+        DeclareLaunchArgument('right_image_topic_relay',      default_value=ConditionalText(''.join([LaunchConfiguration('right_image_topic').perform(context), "_relay"]), LaunchConfiguration('right_image_topic'), LaunchConfiguration('compressed')), description='Should not be modified manually!'),
+        DeclareLaunchArgument('rgbd_topic_relay',      default_value=ConditionalText(LaunchConfiguration('rgbd_topic').perform(context), ''.join([LaunchConfiguration('rgbd_topic').perform(context), "_relay"]), LaunchConfiguration('rgbd_sync')), description='Should not be modified manually!'),
+    
+        # Relays RGB-Depth
+        Node(
+            package='image_transport', executable='republish', name='republish_rgb',
+            condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' != 'true' and ('", LaunchConfiguration('subscribe_rgbd'), "' != 'true' or '", LaunchConfiguration('rgbd_sync'),"'=='true') and '", LaunchConfiguration('compressed'), "' == 'true'"])),
+            remappings=[
+                (['in/', LaunchConfiguration('rgb_image_transport')], [LaunchConfiguration('rgb_topic'), '/', LaunchConfiguration('rgb_image_transport')]),
+                ('out', LaunchConfiguration('rgb_topic_relay'))], 
+            arguments=[LaunchConfiguration('rgb_image_transport'), 'raw'],
+            namespace=LaunchConfiguration('namespace')),
+        Node(
+            package='image_transport', executable='republish', name='republish_depth',
+            condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' != 'true' and ('", LaunchConfiguration('subscribe_rgbd'), "' != 'true' or '", LaunchConfiguration('rgbd_sync'),"'=='true') and '", LaunchConfiguration('compressed'), "' == 'true'"])),
+            remappings=[
+                (['in/', LaunchConfiguration('depth_image_transport')], [LaunchConfiguration('depth_topic'), '/', LaunchConfiguration('depth_image_transport')]),
+                ('out', LaunchConfiguration('depth_topic_relay'))], 
+            arguments=[LaunchConfiguration('depth_image_transport'), 'raw'],
+            namespace=LaunchConfiguration('namespace')),
+        Node(
+            package='rtabmap_ros', executable='rgbd_sync', output="screen",
+            condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' != 'true' and '", LaunchConfiguration('rgbd_sync'), "' == 'true'"])),
+            parameters=[{
+                "approx_sync": LaunchConfiguration('approx_rgbd_sync'),
+                "queue_size": LaunchConfiguration('queue_size'),
+                "depth_scale": LaunchConfiguration('depth_scale')}],
+            remappings=[
+                ("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'))],
+            namespace=LaunchConfiguration('namespace')),
+            
+        # Relays Stereo
+        Node(
+            package='image_transport', executable='republish', name='republish_left',
+            condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true' and ('", LaunchConfiguration('subscribe_rgbd'), "' != 'true' or '", LaunchConfiguration('rgbd_sync'),"'=='true') and '", LaunchConfiguration('compressed'), "' == 'true'"])),
+            remappings=[
+                (['in/', LaunchConfiguration('rgb_image_transport')], [LaunchConfiguration('left_image_topic'), '/', LaunchConfiguration('rgb_image_transport')]),
+                ('out', LaunchConfiguration('left_image_topic_relay'))], 
+            arguments=[LaunchConfiguration('rgb_image_transport'), 'raw'],
+            namespace=LaunchConfiguration('namespace')),
+        Node(
+            package='image_transport', executable='republish', name='republish_right',
+            condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true' and ('", LaunchConfiguration('subscribe_rgbd'), "' != 'true' or '", LaunchConfiguration('rgbd_sync'),"'=='true') and '", LaunchConfiguration('compressed'), "' == 'true'"])),
+            remappings=[
+                (['in/', LaunchConfiguration('rgb_image_transport')], [LaunchConfiguration('right_image_topic'), '/', LaunchConfiguration('rgb_image_transport')]),
+                ('out', LaunchConfiguration('right_image_topic_relay'))], 
+            arguments=[LaunchConfiguration('rgb_image_transport'), 'raw'],
+            namespace=LaunchConfiguration('namespace')),
+        Node(
+            package='rtabmap_ros', executable='stereo_sync', output="screen",
+            condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true' and '", LaunchConfiguration('rgbd_sync'), "' == 'true'"])),
+            parameters=[{
+                "approx_sync": LaunchConfiguration('approx_rgbd_sync'),
+                "queue_size": LaunchConfiguration('queue_size')}],
+            remappings=[
+                ("left/image_rect", LaunchConfiguration('left_image_topic_relay')),
+                ("right/image_rect", LaunchConfiguration('right_image_topic_relay')),
+                ("left/camera_info", LaunchConfiguration('left_camera_info_topic')),
+                ("right/camera_info", LaunchConfiguration('right_camera_info_topic')),
+                ("rgbd_image", LaunchConfiguration('rgbd_topic_relay'))],
+            namespace=LaunchConfiguration('namespace')),
+            
+        # Relay rgbd_image
+        Node(
+            package='rtabmap_ros', executable='rgbd_relay', output="screen",
+            condition=IfCondition(PythonExpression(["'", LaunchConfiguration('rgbd_sync'), "' != 'true' and '", LaunchConfiguration('subscribe_rgbd'), "' == 'true' and '", LaunchConfiguration('compressed'), "' != 'true'"])),
+            remappings=[
+                ("rgbd_image", LaunchConfiguration('rgbd_topic'))],
+            namespace=LaunchConfiguration('namespace')),
+        Node(
+            package='rtabmap_ros', executable='rgbd_relay', output="screen",
+            condition=IfCondition(PythonExpression(["'", LaunchConfiguration('rgbd_sync'), "' != 'true' and '", LaunchConfiguration('subscribe_rgbd'), "' == 'true' and '", LaunchConfiguration('compressed'), "' == 'true'"])),
+            parameters=[{
+                "uncompress": True}],
+            remappings=[
+                ("rgbd_image", [LaunchConfiguration('rgbd_topic'), "/compressed"]),
+                ([LaunchConfiguration('rgbd_topic'), "/compressed_relay"], LaunchConfiguration('rgbd_topic_relay'))],
+            namespace=LaunchConfiguration('namespace')),
+                
+        # RGB-D odometry
+        Node(
+            package='rtabmap_ros', executable='rgbd_odometry', output="screen",
+            condition=IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' != 'true' and '", LaunchConfiguration('visual_odometry'), "' == 'true' and '", LaunchConfiguration('stereo'), "' != 'true'"])),
+            parameters=[{
+                "frame_id": LaunchConfiguration('frame_id'),
+                "odom_frame_id": LaunchConfiguration('vo_frame_id'),
+                "publish_tf": LaunchConfiguration('publish_tf_odom'),
+                "ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id'),
+                "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id'),
+                "wait_for_transform_duration": LaunchConfiguration('wait_for_transform'),
+                "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'),
+                "approx_sync": LaunchConfiguration('approx_sync'),
+                "config_path": LaunchConfiguration('cfg'),
+                "queue_size": LaunchConfiguration('queue_size'),
+                "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'),
+                "guess_frame_id": LaunchConfiguration('odom_guess_frame_id'),
+                "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'),
+                "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}],
+            remappings=[
+                ("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')),
+                ("odom", LaunchConfiguration('odom_topic')),
+                ("imu", LaunchConfiguration('imu_topic'))],
+            arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")],
+            prefix=LaunchConfiguration('launch_prefix'),
+            namespace=LaunchConfiguration('namespace')),
+        
+        # Stereo odometry
+        Node(
+            package='rtabmap_ros', executable='stereo_odometry', output="screen",
+            condition=IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' != 'true' and '", LaunchConfiguration('visual_odometry'), "' == 'true' and '", LaunchConfiguration('stereo'), "' == 'true'"])),
+            parameters=[{
+                "frame_id": LaunchConfiguration('frame_id'),
+                "odom_frame_id": LaunchConfiguration('vo_frame_id'),
+                "publish_tf": LaunchConfiguration('publish_tf_odom'),
+                "ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id'),
+                "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id'),
+                "wait_for_transform_duration": LaunchConfiguration('wait_for_transform'),
+                "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'),
+                "approx_sync": LaunchConfiguration('approx_sync'),
+                "config_path": LaunchConfiguration('cfg'),
+                "queue_size": LaunchConfiguration('queue_size'),
+                "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'),
+                "guess_frame_id": LaunchConfiguration('odom_guess_frame_id'),
+                "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'),
+                "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}],
+            remappings=[
+                ("left/image_rect", LaunchConfiguration('left_image_topic_relay')),
+                ("right/image_rect", LaunchConfiguration('right_image_topic_relay')),
+                ("left/camera_info", LaunchConfiguration('left_camera_info_topic')),
+                ("right/camera_info", LaunchConfiguration('right_camera_info_topic')),
+                ("rgbd_image", LaunchConfiguration('rgbd_topic_relay')),
+                ("odom", LaunchConfiguration('odom_topic')),
+                ("imu", LaunchConfiguration('imu_topic'))],
+            arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")],
+            prefix=LaunchConfiguration('launch_prefix'),
+            namespace=LaunchConfiguration('namespace')),
+            
+        # ICP odometry
+        Node(
+            package='rtabmap_ros', executable='icp_odometry', output="screen",
+            condition=IfCondition(LaunchConfiguration('icp_odometry')),
+            parameters=[{
+                "frame_id": LaunchConfiguration('frame_id'),
+                "odom_frame_id": LaunchConfiguration('vo_frame_id'),
+                "publish_tf": LaunchConfiguration('publish_tf_odom'),
+                "ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id'),
+                "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id'),
+                "wait_for_transform_duration": LaunchConfiguration('wait_for_transform'),
+                "wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'),
+                "approx_sync": LaunchConfiguration('approx_sync'),
+                "config_path": LaunchConfiguration('cfg'),
+                "queue_size": LaunchConfiguration('queue_size'),
+                "guess_frame_id": LaunchConfiguration('odom_guess_frame_id'),
+                "guess_min_translation": LaunchConfiguration('odom_guess_min_translation'),
+                "guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}],
+            remappings=[
+                ("scan", LaunchConfiguration('scan_topic')),
+                ("scan_cloud", LaunchConfiguration('scan_cloud_topic')),
+                ("odom", LaunchConfiguration('odom_topic')),
+                ("imu", LaunchConfiguration('imu_topic'))],
+            arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")],
+            prefix=LaunchConfiguration('launch_prefix'),
+            namespace=LaunchConfiguration('namespace')),
+
+        Node(
+            package='rtabmap_ros', executable='rtabmap', output="screen",
+            parameters=[{
+                "subscribe_depth": LaunchConfiguration('depth'),
+                "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'),
+                "subscribe_rgb": LaunchConfiguration('subscribe_rgb'),
+                "subscribe_stereo": LaunchConfiguration('stereo'),
+                "subscribe_scan": LaunchConfiguration('subscribe_scan'),
+                "subscribe_scan_cloud": LaunchConfiguration('subscribe_scan_cloud'),
+                "subscribe_user_data": LaunchConfiguration('subscribe_user_data'),
+                "subscribe_odom_info": ConditionalBool(True, False, IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' == 'true' or '", LaunchConfiguration('visual_odometry'), "' == 'true'"]))._predicate_func(context)).perform(context),
+                "frame_id": LaunchConfiguration('frame_id'),
+                "map_frame_id": LaunchConfiguration('map_frame_id'),
+                "odom_frame_id": LaunchConfiguration('odom_frame_id'),
+                "publish_tf": LaunchConfiguration('publish_tf_map'),
+                "ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id'),
+                "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id'),
+                "odom_tf_angular_variance": LaunchConfiguration('odom_tf_angular_variance'),
+                "odom_tf_linear_variance": LaunchConfiguration('odom_tf_linear_variance'),
+                "odom_sensor_sync": LaunchConfiguration('odom_sensor_sync'),
+                "wait_for_transform_duration": LaunchConfiguration('wait_for_transform'),
+                "database_path": LaunchConfiguration('database_path'),
+                "approx_sync": LaunchConfiguration('approx_sync'),
+                "config_path": LaunchConfiguration('cfg'),
+                "queue_size": LaunchConfiguration('queue_size'),
+                "scan_normal_k": LaunchConfiguration('scan_normal_k'),
+                "landmark_linear_variance": LaunchConfiguration('tag_linear_variance'),
+                "landmark_angular_variance": LaunchConfiguration('tag_angular_variance'),
+                "Mem/IncrementalMemory": ConditionalText("true", "false", IfCondition(PythonExpression(["'", LaunchConfiguration('localization'), "' != 'true'"]))._predicate_func(context)).perform(context),
+                "Mem/InitWMWithAllNodes": ConditionalText("true", "false", IfCondition(PythonExpression(["'", LaunchConfiguration('localization'), "' == 'true'"]))._predicate_func(context)).perform(context)
+            }],
+            remappings=[
+                ("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')),
+                ("left/image_rect", LaunchConfiguration('left_image_topic_relay')),
+                ("right/image_rect", LaunchConfiguration('right_image_topic_relay')),
+                ("left/camera_info", LaunchConfiguration('left_camera_info_topic')),
+                ("right/camera_info", LaunchConfiguration('right_camera_info_topic')),
+                ("scan", LaunchConfiguration('scan_topic')),
+                ("scan_cloud", LaunchConfiguration('scan_cloud_topic')),
+                ("user_data", LaunchConfiguration('user_data_topic')),
+                ("user_data_async", LaunchConfiguration('user_data_async_topic')),
+                ("gps/fix", LaunchConfiguration('gps_topic')),
+                ("tag_detections", LaunchConfiguration('tag_topic')),
+                ("odom", LaunchConfiguration('odom_topic')),
+                ("imu", LaunchConfiguration('imu_topic'))],
+            arguments=[LaunchConfiguration("args")],
+            prefix=LaunchConfiguration('launch_prefix'),
+            namespace=LaunchConfiguration('namespace')),
+
+        Node(
+            package='rtabmap_ros', executable='rtabmapviz', output='screen',
+            parameters=[{
+                "subscribe_depth": LaunchConfiguration('depth'),
+                "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'),
+                "subscribe_rgb": LaunchConfiguration('subscribe_rgb'),
+                "subscribe_stereo": LaunchConfiguration('stereo'),
+                "subscribe_scan": LaunchConfiguration('subscribe_scan'),
+                "subscribe_scan_cloud": LaunchConfiguration('subscribe_scan_cloud'),
+                "subscribe_user_data": LaunchConfiguration('subscribe_user_data'),
+                "subscribe_odom_info": ConditionalBool(True, False, IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' == 'true' or '", LaunchConfiguration('visual_odometry'), "' == 'true'"]))._predicate_func(context)).perform(context),
+                "frame_id": LaunchConfiguration('frame_id'),
+                "odom_frame_id": LaunchConfiguration('odom_frame_id'),
+                "wait_for_transform_duration": LaunchConfiguration('wait_for_transform'),
+                "approx_sync": LaunchConfiguration('approx_sync'),
+                "queue_size": LaunchConfiguration('queue_size')
+            }],
+            remappings=[
+                ("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')),
+                ("left/image_rect", LaunchConfiguration('left_image_topic_relay')),
+                ("right/image_rect", LaunchConfiguration('right_image_topic_relay')),
+                ("left/camera_info", LaunchConfiguration('left_camera_info_topic')),
+                ("right/camera_info", LaunchConfiguration('right_camera_info_topic')),
+                ("scan", LaunchConfiguration('scan_topic')),
+                ("scan_cloud", LaunchConfiguration('scan_cloud_topic')),
+                ("odom", LaunchConfiguration('odom_topic'))],
+            condition=IfCondition(LaunchConfiguration("rtabmapviz")),
+            arguments=[LaunchConfiguration("gui_cfg")],
+            prefix=LaunchConfiguration('launch_prefix'),
+            namespace=LaunchConfiguration('namespace'))]
+
+def generate_launch_description():
+    return LaunchDescription([
+        # Set env var to print messages to stdout immediately
+        SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'),
+        
+        # Arguments
+        DeclareLaunchArgument('stereo', default_value='false', description='Use stereo input instead of RGB-D.'),
+
+        DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'),
+        DeclareLaunchArgument('rtabmapviz',   default_value='true',  description='Launch RTAB-Map UI (optional).'),
+        
+        # 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('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='',                   description='Output map frame id (TF).'),
+        DeclareLaunchArgument('publish_tf_map', default_value='true',               description='Publish TF between map and odomerty.'),
+        DeclareLaunchArgument('namespace',      default_value='rtabmap',            description=''),
+        DeclareLaunchArgument('database_path',  default_value='~/.ros/rtabmap.db',  description='Where is the map saved/loaded.'),
+        DeclareLaunchArgument('queue_size',     default_value='10',                 description=''),
+        DeclareLaunchArgument('wait_for_transform', default_value='0.2',            description=''),
+        DeclareLaunchArgument('args',           default_value='',                   description='Can be used to pass RTAB-Map\'s parameters or other flags like --udebug and --delete_db_on_start/-d'),
+        DeclareLaunchArgument('launch_prefix',  default_value='',                   description='For debugging purpose, it fills prefix tag of the nodes, e.g., "xterm -e gdb -ex run --args"'),
+        DeclareLaunchArgument('output',         default_value='screen',             description='Control node output (screen or log).'),
+        
+        DeclareLaunchArgument('ground_truth_frame_id',      default_value='', description='e.g., "world"'),
+        DeclareLaunchArgument('ground_truth_base_frame_id', default_value='', description='e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree)'),
+        
+        DeclareLaunchArgument('approx_sync',  default_value='false',       description='If timestamps of the input topics should be synchronized using approximate or exact time policy.'),
+
+        # RGB-D related topics
+        DeclareLaunchArgument('rgb_topic',           default_value='/camera/rgb/image_rect_color',       description=''),
+        DeclareLaunchArgument('depth_topic',         default_value='/camera/depth_registered/image_raw', description=''),
+        DeclareLaunchArgument('camera_info_topic',   default_value='/camera/rgb/camera_info',            description=''),
+        
+        # Stereo related topics
+        DeclareLaunchArgument('stereo_namespace',        default_value='/stereo_camera', description=''),
+        DeclareLaunchArgument('left_image_topic',        default_value=[LaunchConfiguration('stereo_namespace'), '/left/image_rect_color'], description=''),
+        DeclareLaunchArgument('right_image_topic',       default_value=[LaunchConfiguration('stereo_namespace'), '/right/image_rect'], description='Use grayscale image for efficiency'),
+        DeclareLaunchArgument('left_camera_info_topic',  default_value=[LaunchConfiguration('stereo_namespace'), '/left/camera_info'], description=''),
+        DeclareLaunchArgument('right_camera_info_topic', default_value=[LaunchConfiguration('stereo_namespace'), '/right/camera_info'], description=''),
+        
+        # Use Pre-sync RGBDImage format
+        DeclareLaunchArgument('rgbd_sync',        default_value='false',      description='Pre-sync rgb_topic, depth_topic, camera_info_topic.'),
+        DeclareLaunchArgument('approx_rgbd_sync', default_value='true',       description='false=exact synchronization.'),
+        DeclareLaunchArgument('subscribe_rgbd',   default_value=LaunchConfiguration('rgbd_sync'), description='Already synchronized RGB-D related topic, e.g., with rtabmap_ros/rgbd_sync nodelet.'),
+        DeclareLaunchArgument('rgbd_topic',       default_value='rgbd_image', description=''),
+        DeclareLaunchArgument('depth_scale',      default_value='1.0',        description=''),
+        
+        # Image topic compression
+        DeclareLaunchArgument('compressed',      default_value='false', description='If you want to subscribe to compressed image topics'),
+        DeclareLaunchArgument('rgb_image_transport',      default_value='compressed', description='Common types: compressed, theora (see "rosrun image_transport list_transports")'),
+        DeclareLaunchArgument('depth_image_transport',      default_value='compressedDepth', description='Depth compatible types: compressedDepth (see "rosrun image_transport list_transports")'),
+       
+        # LiDAR
+        DeclareLaunchArgument('subscribe_scan',       default_value='false',       description=''),
+        DeclareLaunchArgument('scan_topic',           default_value='/scan',       description=''),
+        DeclareLaunchArgument('subscribe_scan_cloud', default_value='false',       description=''),
+        DeclareLaunchArgument('scan_cloud_topic',     default_value='/scan_cloud', description=''),
+        DeclareLaunchArgument('scan_normal_k',        default_value='0',           description=''),
+        
+        # Odometry
+        DeclareLaunchArgument('visual_odometry',            default_value='true',  description='Launch rtabmap visual odometry node.'),
+        DeclareLaunchArgument('icp_odometry',               default_value='false', description='Launch rtabmap icp odometry node.'),
+        DeclareLaunchArgument('odom_topic',                 default_value='odom',  description='Odometry topic name.'),
+        DeclareLaunchArgument('vo_frame_id',                default_value=LaunchConfiguration('odom_topic'), description='Visual/Icp odometry frame ID for TF.'),
+        DeclareLaunchArgument('publish_tf_odom',            default_value='true',  description=''),
+        DeclareLaunchArgument('odom_tf_angular_variance',   default_value='1.0',   description='If TF is used to get odometry, this is the default angular variance'),
+        DeclareLaunchArgument('odom_tf_linear_variance',    default_value='1.0',   description='If TF is used to get odometry, this is the default linear variance'),
+        DeclareLaunchArgument('odom_args',                  default_value='',      description='More arguments for odometry (overwrite same parameters in rtabmap_args).'),
+        DeclareLaunchArgument('odom_sensor_sync',           default_value='false', description=''),
+        DeclareLaunchArgument('odom_guess_frame_id',        default_value='',      description=''),
+        DeclareLaunchArgument('odom_guess_min_translation', default_value='0.0',   description=''),
+        DeclareLaunchArgument('odom_guess_min_rotation',    default_value='0.0',   description=''),
+        
+        # imu
+        DeclareLaunchArgument('imu_topic',        default_value='/imu/data', description='Used with VIO approaches and for SLAM graph optimization (gravity constraints).'),
+        DeclareLaunchArgument('wait_imu_to_init', default_value='false',     description=''),
+        
+        # User Data
+        DeclareLaunchArgument('subscribe_user_data',   default_value='false',            description='User data synchronized subscription.'),
+        DeclareLaunchArgument('user_data_topic',       default_value='/user_data',       description=''),
+        DeclareLaunchArgument('user_data_async_topic', default_value='/user_data_async', description='User data async subscription (rate should be lower than map update rate).'),
+        
+        #GPS
+        DeclareLaunchArgument('gps_topic',  default_value='/gps/fix', description='GPS async subscription. This is used for SLAM graph optimization and loop closure candidates selection.'),
+
+        # Tag/Landmark
+        DeclareLaunchArgument('tag_topic',            default_value='/tag_detections', description='AprilTag topic async subscription. This is used for SLAM graph optimization and loop closure detection. Landmark poses are also published accordingly to current optimized map.'),
+        DeclareLaunchArgument('tag_linear_variance',  default_value='0.0001',          description=''),
+        DeclareLaunchArgument('tag_angular_variance', default_value='9999.0',            description='>=9999 means rotation is ignored in optimization, when rotation estimation of the tag is not reliable or not computed.'),
+        OpaqueFunction(function=launch_setup)
+    ])
+
@@ -279,7 +279,19 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	}

 	//parse input arguments
-	std::vector<std::string> argList = get_node_options().arguments();
+	std::vector<std::string> tmpList = get_node_options().arguments();
+	std::vector<std::string> argList;
+	for(unsigned int i=0; i<tmpList.size(); ++i)
+	{
+	    // Issue with ros2 launch files in which we cannot pass a 
+	    // list of strings as argument (they will appear in same string)
+	    std::list<std::string> v = uSplit(tmpList[i]);
+	    for(std::list<std::string>::iterator iter=v.begin(); iter!=v.end(); ++iter)
+	    {
+	        argList.push_back(*iter);
+	    }
+	}
+	
 	char ** argv = new char*[argList.size()];
 	bool deleteDbOnStart = false;
 	for(unsigned int i=0; i<argList.size(); ++i)
@@ -76,6 +76,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	publishTf_(true),
 	waitForTransform_(0.1), // 100 ms
 	publishNullWhenLost_(true),
+	queueSize_(5),
 	paused_(false),
 	resetCountdown_(0),
 	resetCurrentCount_(0),
@@ -123,6 +124,9 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o

 	waitIMUToinit_ = this->declare_parameter("wait_imu_to_init", waitIMUToinit_);
 	
+	queueSize_ = this->declare_parameter("queue_size", queueSize_);
+
+
 	if(publishTf_ && !guessFrameId_.empty() && guessFrameId_.compare(odomFrameId_) == 0)
 	{
 		RCLCPP_WARN(this->get_logger(), "\"publish_tf\" and \"guess_frame_id\" cannot be used "
@@ -145,6 +149,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	RCLCPP_INFO(this->get_logger(), "Odometry: guess_min_time         = %f", guessMinTime_);
 	RCLCPP_INFO(this->get_logger(), "Odometry: expected_update_rate   = %f Hz", expectedUpdateRate_);
 	RCLCPP_INFO(this->get_logger(), "Odometry: wait_imu_to_init       = %s", waitIMUToinit_?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "Odometry: queue_size             = %s", queueSize_?"true":"false");

 	configPath_ = uReplaceChar(configPath_, '~', UDirectory::homeDir());
 	if(configPath_.size() && configPath_.at(0) != '/')
@@ -241,7 +246,19 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams)
 		}
 	}

-	std::vector<std::string> argList = this->get_node_options().arguments();
+	std::vector<std::string> tmpList = this->get_node_options().arguments();
+	std::vector<std::string> argList;
+	for(unsigned int i=0; i<tmpList.size(); ++i)
+	{
+	    // Issue with ros2 launch files in which we cannot pass a 
+	    // list of strings as argument (they will appear in same string)
+	    std::list<std::string> v = uSplit(tmpList[i]);
+	    for(std::list<std::string>::iterator iter=v.begin(); iter!=v.end(); ++iter)
+	    {
+	        argList.push_back(*iter);
+	    }
+	}
+	
 	char ** argv = new char*[argList.size()];
 	for(unsigned int i=0; i<argList.size(); ++i)
 	{
@@ -315,11 +332,10 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams)

 	odomStrategy_ = 0;
 	Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_);
+
 	if(waitIMUToinit_ || odometry_->canProcessAsyncIMU())
 	{
-		int queueSize = 10;
-		queueSize = this->declare_parameter("queue_size", queueSize);
-		imuSub_ = create_subscription<sensor_msgs::msg::Imu>("imu", queueSize*5, std::bind(&OdometryROS::callbackIMU, this, std::placeholders::_1));
+		imuSub_ = create_subscription<sensor_msgs::msg::Imu>("imu", queueSize_*5, std::bind(&OdometryROS::callbackIMU, this, std::placeholders::_1));
 		RCLCPP_INFO(this->get_logger(), "odometry: Subscribing to IMU topic %s", imuSub_->get_topic_name());
 	}

@@ -53,8 +53,7 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) :
 		approxSync3_(0),
 		exactSync3_(0),
 		approxSync4_(0),
-		exactSync4_(0),
-		queueSize_(5)
+		exactSync4_(0)
 {
 	OdometryROS::init(false, true, false);
 }
@@ -77,7 +76,6 @@ void RGBDOdometry::onOdomInit()
 	bool approxSync = true;
 	bool subscribeRGBD = false;
 	approxSync = this->declare_parameter("approx_sync", approxSync);
-	queueSize_ = this->declare_parameter("queue_size", queueSize_);
 	subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
 	rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
 	if(rgbdCameras <= 0)
@@ -90,7 +88,6 @@ void RGBDOdometry::onOdomInit()
 	}

 	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync    = %s", approxSync?"true":"false");
-	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: queue_size     = %d", queueSize_);
 	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
 	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgbd_cameras   = %d", rgbdCameras);

@@ -115,7 +112,7 @@ void RGBDOdometry::onOdomInit()
 				if(approxSync)
 				{
 					approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
-							MyApproxSync2Policy(queueSize_),
+							MyApproxSync2Policy(queueSize()),
 							rgbd_image1_sub_,
 							rgbd_image2_sub_);
 					approxSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -123,7 +120,7 @@ void RGBDOdometry::onOdomInit()
 				else
 				{
 					exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
-							MyExactSync2Policy(queueSize_),
+							MyExactSync2Policy(queueSize()),
 							rgbd_image1_sub_,
 							rgbd_image2_sub_);
 					exactSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -139,7 +136,7 @@ void RGBDOdometry::onOdomInit()
 				if(approxSync)
 				{
 					approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
-							MyApproxSync3Policy(queueSize_),
+							MyApproxSync3Policy(queueSize()),
 							rgbd_image1_sub_,
 							rgbd_image2_sub_,
 							rgbd_image3_sub_);
@@ -148,7 +145,7 @@ void RGBDOdometry::onOdomInit()
 				else
 				{
 					exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
-							MyExactSync3Policy(queueSize_),
+							MyExactSync3Policy(queueSize()),
 							rgbd_image1_sub_,
 							rgbd_image2_sub_,
 							rgbd_image3_sub_);
@@ -166,7 +163,7 @@ void RGBDOdometry::onOdomInit()
 				if(approxSync)
 				{
 					approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
-							MyApproxSync4Policy(queueSize_),
+							MyApproxSync4Policy(queueSize()),
 							rgbd_image1_sub_,
 							rgbd_image2_sub_,
 							rgbd_image3_sub_,
@@ -176,7 +173,7 @@ void RGBDOdometry::onOdomInit()
 				else
 				{
 					exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
-							MyExactSync4Policy(queueSize_),
+							MyExactSync4Policy(queueSize()),
 							rgbd_image1_sub_,
 							rgbd_image2_sub_,
 							rgbd_image3_sub_,
@@ -211,12 +208,12 @@ void RGBDOdometry::onOdomInit()

 		if(approxSync)
 		{
-			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize()), image_mono_sub_, image_depth_sub_, info_sub_);
 			approxSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 		}
 		else
 		{
-			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize()), image_mono_sub_, image_depth_sub_, info_sub_);
 			exactSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 		}

@@ -490,20 +487,20 @@ void RGBDOdometry::flushCallbacks()
 	if(approxSync_)
 	{
 		delete approxSync_;
-		approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+		approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize()), image_mono_sub_, image_depth_sub_, info_sub_);
 		approxSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 	}
 	if(exactSync_)
 	{
 		delete exactSync_;
-		exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+		exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize()), image_mono_sub_, image_depth_sub_, info_sub_);
 		exactSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 	}
 	if(approxSync2_)
 	{
 		delete approxSync2_;
 		approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
-				MyApproxSync2Policy(queueSize_),
+				MyApproxSync2Policy(queueSize()),
 				rgbd_image1_sub_,
 				rgbd_image2_sub_);
 		approxSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -512,7 +509,7 @@ void RGBDOdometry::flushCallbacks()
 	{
 		delete exactSync2_;
 		exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
-				MyExactSync2Policy(queueSize_),
+				MyExactSync2Policy(queueSize()),
 				rgbd_image1_sub_,
 				rgbd_image2_sub_);
 		exactSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -521,7 +518,7 @@ void RGBDOdometry::flushCallbacks()
 	{
 		delete approxSync3_;
 		approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
-				MyApproxSync3Policy(queueSize_),
+				MyApproxSync3Policy(queueSize()),
 				rgbd_image1_sub_,
 				rgbd_image2_sub_,
 				rgbd_image3_sub_);
@@ -531,7 +528,7 @@ void RGBDOdometry::flushCallbacks()
 	{
 		delete exactSync3_;
 		exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
-				MyExactSync3Policy(queueSize_),
+				MyExactSync3Policy(queueSize()),
 				rgbd_image1_sub_,
 				rgbd_image2_sub_,
 				rgbd_image3_sub_);
@@ -541,7 +538,7 @@ void RGBDOdometry::flushCallbacks()
 	{
 		delete approxSync4_;
 		approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
-				MyApproxSync4Policy(queueSize_),
+				MyApproxSync4Policy(queueSize()),
 				rgbd_image1_sub_,
 				rgbd_image2_sub_,
 				rgbd_image3_sub_,
@@ -552,7 +549,7 @@ void RGBDOdometry::flushCallbacks()
 	{
 		delete exactSync4_;
 		exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
-				MyExactSync4Policy(queueSize_),
+				MyExactSync4Policy(queueSize()),
 				rgbd_image1_sub_,
 				rgbd_image2_sub_,
 				rgbd_image3_sub_,
@@ -49,8 +49,7 @@ namespace rtabmap_ros
 StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :
 		rtabmap_ros::OdometryROS("stereo_odometry", options),
 		approxSync_(0),
-		exactSync_(0),
-		queueSize_(5)
+		exactSync_(0)
 {
 	OdometryROS::init(true, true, false);
 }
@@ -66,11 +65,9 @@ void StereoOdometry::onOdomInit()
 	bool approxSync = false;
 	bool subscribeRGBD = false;
 	approxSync = this->declare_parameter("approx_sync", approxSync);
-	queueSize_ = this->declare_parameter("queue_size", queueSize_);
 	subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);

 	RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync = %s", approxSync?"true":"false");
-	RCLCPP_INFO(this->get_logger(), "StereoOdometry: queue_size = %d", queueSize_);
 	RCLCPP_INFO(this->get_logger(), "StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");

 	std::string subscribedTopicsMsg;
@@ -93,12 +90,12 @@ void StereoOdometry::onOdomInit()

 		if(approxSync)
 		{
-			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize()), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
 			approxSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
 		}
 		else
 		{
-			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize()), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
 			exactSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
 		}

@@ -311,13 +308,13 @@ void StereoOdometry::flushCallbacks()
 	if(approxSync_)
 	{
 		delete approxSync_;
-		approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+		approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize()), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
 		approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
 	}
 	if(exactSync_)
 	{
 		delete exactSync_;
-		exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+		exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize()), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
 		exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
 	}
 }
@@ -75,8 +75,8 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
 	image_transport::TransportHints hints(this);
 	imageLeftSub_.subscribe(this, "left/image_rect", hints.getTransport(), rmw_qos_profile_sensor_data);
 	imageRightSub_.subscribe(this, "right/image_rect", hints.getTransport(), rmw_qos_profile_sensor_data);
-	cameraInfoLeftSub_.subscribe(this, "camera_info", rmw_qos_profile_sensor_data);
-	cameraInfoRightSub_.subscribe(this, "camera_info", rmw_qos_profile_sensor_data);
+	cameraInfoLeftSub_.subscribe(this, "left/camera_info", rmw_qos_profile_sensor_data);
+	cameraInfoRightSub_.subscribe(this, "right/camera_info", rmw_qos_profile_sensor_data);

 	subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
 						get_name(),