Uniformized "approx_sync" parameter name across all nodes. Updated rgbd_mapping.launch and stereo_mapping.launch to include rtabmap.launch instead of redefining all nodes.

2026-10-04 00:37:46 +08:00 · 2016-06-30 12:49:20 -04:00
parent 88f164e6bf
commit 77faedf9d2
7 changed files with 191 additions and 251 deletions
@@ -1,5 +1,7 @@

 <launch>
+  <!-- Backward compatibility launch file, use rtabmap.launch instead -->
+  
  <!-- Your RGB-D sensor should be already started with "depth_registration:=true".
        Examples:
           $ roslaunch freenect_launch freenect.launch depth_registration:=true 
@@ -17,17 +19,15 @@
  <arg name="rviz_cfg"                default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
  
  <arg name="frame_id"                default="camera_link"/>   <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
-  <arg name="time_threshold"          default="0"/>             <!-- (ms) If not 0 ms, memory management is used to keep processing time on this fixed limit. -->
-  <arg name="optimize_from_last_node" default="false"/>         <!-- Optimize the map from the last node. Should be true on multi-session mapping and when time threshold is set -->
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="rtabmap_args"            default=""/>              <!-- delete_db_on_start, udebug -->
  <arg name="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
+  <arg name="approx_sync"             default="true"/>         <!-- if timestamps of the input topics are not synchronized -->
   
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_registered_topic"  default="/camera/depth_registered/image_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="compressed"              default="false"/>
-  <arg name="convert_depth_to_mm"     default="true"/>
   
  <arg name="subscribe_scan"          default="false"/>         <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
  <arg name="scan_topic"              default="/scan"/>
@@ -41,102 +41,35 @@
  <arg name="namespace"               default="rtabmap"/>
  <arg name="wait_for_transform"      default="0.2"/>
  
-  <!-- Nodes -->
-  <group ns="$(arg namespace)">
+  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
+    <arg name="rtabmapviz"              value="$(arg rtabmapviz)" /> 
+    <arg name="rviz"                    value="$(arg rviz)" />
+    <arg name="localization"            value="$(arg localization)"/>
+    <arg name="gui_cfg"                 value="$(arg rtabmapviz_cfg)" />
+    <arg name="rviz_cfg"                value="$(arg rviz_cfg)" />
  
-    <node if="$(arg compressed)" name="republish_rgb"   type="republish" pkg="image_transport" args="compressed in:=$(arg rgb_topic) raw out:=$(arg rgb_topic)" />
-    <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=$(arg depth_registered_topic) raw out:=$(arg depth_registered_topic)" />
-  
-    <!-- Odometry -->
-    <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
-      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
-      <remap from="depth/image"     to="$(arg depth_registered_topic)"/>
-      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
-	  
-	  <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
-	  <param name="wait_for_transform_duration" type="double"   value="$(arg wait_for_transform)"/>
-	  
-      <param name="Odom/FillInfoData"           type="string" value="true"/> 
-    </node>
-  
-    <!-- Visual SLAM (robot side) -->
-    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
-      <param name="subscribe_depth"             type="bool"   value="true"/>
-      <param name="subscribe_scan"              type="bool"   value="$(arg subscribe_scan)"/>
-      <param name="subscribe_scan_cloud"        type="bool"   value="$(arg subscribe_scan_cloud)"/>
-	  <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
-	  <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
-	  <param name="database_path"               type="string" value="$(arg database_path)"/>
-	
-      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
-      <remap from="depth/image"     to="$(arg depth_registered_topic)"/>
-      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
-      <remap from="scan"            to="$(arg scan_topic)"/>
-      <remap from="scan_cloud"      to="$(arg scan_cloud_topic)"/>
-      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
-      
-      <param name="Rtabmap/TimeThr"              type="string" value="$(arg time_threshold)"/>
-      <param name="RGBD/OptimizeFromGraphEnd"    type="string" value="$(arg optimize_from_last_node)"/>
-      <param name="Mem/SaveDepth16Format"        type="string" value="$(arg convert_depth_to_mm)"/>
-      
-      <!-- localization mode -->
-	  <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
-	  <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
-	  <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
+    <arg name="frame_id"                value="$(arg frame_id)"/>
+    <arg name="namespace"               value="$(arg namespace)"/>
+    <arg name="database_path"           value="$(arg database_path)"/>
+    <arg name="wait_for_transform"      value="$(arg wait_for_transform)"/>
+    <arg name="rtabmap_args"            value="$(arg rtabmap_args)"/>  
+    <arg name="launch_prefix"           value="$(arg launch_prefix)"/>          
+    <arg name="approx_sync"             value="$(arg approx_sync)"/>

-      <!-- when 2D scan is set -->
-      <param if="$(arg subscribe_scan)" name="Optimizer/Slam2D"        type="string" value="true"/>
-	  <param if="$(arg subscribe_scan)" name="Icp/CorrespondenceRatio" type="string" value="0.25"/>
-	  <param if="$(arg subscribe_scan)" name="Reg/Strategy"            type="string" value="1"/> 
-	  <param if="$(arg subscribe_scan)" name="Reg/Force3DoF"           type="string" value="true"/>
-
-	  <!-- when 3D scan is set -->
-	  <param if="$(arg subscribe_scan_cloud)" name="Reg/Strategy"      type="string" value="1"/>
-    </node>
+    <arg name="rgb_topic"               value="$(arg rgb_topic)" />
+    <arg name="depth_topic"             value="$(arg depth_registered_topic)" />
+    <arg name="camera_info_topic"       value="$(arg camera_info_topic)" />
+    <arg name="compressed"              value="$(arg compressed)"/>                                                                                
+   
+    <arg name="subscribe_scan"          value="$(arg subscribe_scan)"/>
+    <arg name="scan_topic"              value="$(arg scan_topic)"/>
  
-    <!-- Visualisation RTAB-Map -->
-    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="$(arg rtabmapviz_cfg)" output="screen" launch-prefix="$(arg launch_prefix)">
-      <param name="subscribe_depth"       type="bool"   value="true"/>
-      <param name="subscribe_scan"        type="bool"   value="$(arg subscribe_scan)"/>
-      <param name="subscribe_scan_cloud"  type="bool"   value="$(arg subscribe_scan_cloud)"/>
-      <param name="subscribe_odom_info"   type="bool"   value="$(arg visual_odometry)"/>
-      <param name="frame_id"              type="string" value="$(arg frame_id)"/>
-      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
-    
-      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
-      <remap from="depth/image"     to="$(arg depth_registered_topic)"/>
-      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
-      <remap from="scan"            to="$(arg scan_topic)"/>
-      <remap from="scan_cloud"      to="$(arg scan_cloud_topic)"/>
-      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
-    </node>
-  
-  </group>
-  
-  <!-- Visualization RVIZ -->
-  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="$(arg rviz_cfg)"/>
-  <!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
-  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
-  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
-    <remap from="rgb/image_in"        to="$(arg rgb_topic)"/>
-    <remap from="depth/image_in"      to="$(arg depth_registered_topic)"/>
-    <remap from="rgb/camera_info_in"  to="$(arg camera_info_topic)"/>
-    <remap if="$(arg visual_odometry)"     from="odom_in"  to="rtabmap/odom"/>
-    <remap unless="$(arg visual_odometry)" from="odom_in"  to="$(arg odom_topic)"/>
-    
-    <remap from="rgb/image_out"       to="data_odom_sync/image"/>
-    <remap from="depth/image_out"     to="data_odom_sync/depth"/>
-    <remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/>
-    <remap from="odom_out"            to="odom_sync"/>
-  </node>
-  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
-    <remap from="rgb/image"       to="data_odom_sync/image"/>
-    <remap from="depth/image"     to="data_odom_sync/depth"/>
-    <remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
-    <remap from="cloud"           to="voxel_cloud" />
-
-    <param name="decimation" type="double" value="2"/>
-    <param name="voxel_size" type="double" value="0.02"/>
-  </node>
+    <arg name="subscribe_scan_cloud"    value="$(arg subscribe_scan_cloud)"/>
+    <arg name="scan_cloud_topic"        value="$(arg scan_cloud_topic)"/>
+   
+    <arg name="visual_odometry"         value="$(arg visual_odometry)"/>          
+    <arg name="odom_topic"              value="$(arg odom_topic)"/>         
+    <arg name="odom_args"               value="$(arg rtabmap_args)"/>
+  </include>

 </launch>
@@ -37,7 +37,8 @@
  <arg name="wait_for_transform"      default="0.2"/>
  <arg name="rtabmap_args"            default=""/>              <!-- delete_db_on_start, udebug -->
  <arg name="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
-  
+  <arg name="approx_sync"             default="true"/>         <!-- if timestamps of the input topics are not synchronized -->
+   
  <!-- RGB-D related topics -->
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
@@ -49,7 +50,6 @@
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />      <!-- using grayscale image for efficiency -->
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
-  <arg name="approx_sync"             default="false"/>         <!-- if timestamps of the stereo images are not synchronized -->
  
  <arg name="compressed"              default="false"/>         <!-- If you want to subscribe to compressed image topics -->
                                                                <!-- For depth_topic, "compressedDepth" image_transport is used. --> 
@@ -91,6 +91,7 @@
 	  
        <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
        <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
+        <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
        <param name="config_path"                 type="string" value="$(arg cfg)"/>
        <param name="queue_size"                  type="int" value="$(arg queue_size)"/>
      </node>
@@ -125,7 +126,7 @@
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
      <param name="database_path"        type="string" value="$(arg database_path)"/>
-      <param name="stereo_approx_sync"   type="bool"   value="$(arg approx_sync)"/>
+      <param name="approx_sync"          type="bool"   value="$(arg approx_sync)"/>
      <param name="config_path"          type="string" value="$(arg cfg)"/>
      <param name="queue_size"           type="int" value="$(arg queue_size)"/>
      
@@ -189,7 +190,7 @@

    <param name="decimation"  type="double" value="2"/>
    <param name="voxel_size"  type="double" value="0.02"/>
-    <param if="$(arg stereo)" name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
+    <param name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
  </node>

 </launch>
@@ -1,5 +1,7 @@

 <launch>
+   <!-- Backward compatibility launch file, use "rtabmap.launch rgbd:=false stereo:=true" instead -->
+
   <!-- Your camera should be calibrated and publishing rectified left and right 
        images + corresponding camera_info msgs. You can use stereo_image_proc for image rectification.
        Example:
@@ -17,20 +19,17 @@
  <arg name="rviz_cfg"                default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
  
  <arg name="frame_id"                default="base_link"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
-  <arg name="time_threshold"          default="0"/>             <!-- (ms) If not 0 ms, memory management is used to keep processing time on this fixed limit. -->
-  <arg name="optimize_from_last_node" default="false"/>         <!-- Optimize the map from the last node. Should be true on multi-session mapping and when time threshold is set -->
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
-  <arg name="rtabmap_args"            default=""/>              <!-- delete_db_on_start, udebug -->
+  <arg name="rtabmap_args"            default=""/>   <!-- delete_db_on_start, udebug -->
  <arg name="launch_prefix"           default=""/>
+  <arg name="approx_sync"             default="false"/>         <!-- if timestamps of the input topics are not synchronized -->
  
  <arg name="stereo_namespace"        default="/stereo_camera"/>
  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />      <!-- using grayscale image for efficiency -->
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
-  <arg name="approximate_sync"        default="false"/>         <!-- if timestamps of the stereo images are not synchronized -->
  <arg name="compressed"              default="false"/>
-  <arg name="convert_depth_to_mm"     default="true"/>
   
  <arg name="subscribe_scan"          default="false"/>         <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
  <arg name="scan_topic"              default="/scan"/>
@@ -44,98 +43,40 @@
  <arg name="namespace"               default="rtabmap"/>
  <arg name="wait_for_transform"      default="0.2"/>
  
-  <!-- Nodes -->
-  <group ns="$(arg namespace)">
+  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
+    <arg name="rgbd"                    value="false"/>
+    <arg name="stereo"                  value="true"/>
+    <arg name="rtabmapviz"              value="$(arg rtabmapviz)" /> 
+    <arg name="rviz"                    value="$(arg rviz)" />
+    <arg name="localization"            value="$(arg localization)"/>
+    <arg name="gui_cfg"                 value="$(arg rtabmapviz_cfg)" />
+    <arg name="rviz_cfg"                value="$(arg rviz_cfg)" />
  
-    <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="compressed in:=$(arg left_image_topic) raw out:=$(arg left_image_topic)" />
-    <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="compressed in:=$(arg right_image_topic) raw out:=$(arg right_image_topic)" />
-  
-    <!-- Odometry -->
-    <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
-      <remap from="left/image_rect"        to="$(arg left_image_topic)"/>
-      <remap from="right/image_rect"       to="$(arg right_image_topic)"/>
-      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
-      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
-	  
-      <param name="frame_id"                 type="string" value="$(arg frame_id)"/>
-      <param name="wait_for_transform_duration"       type="double"   value="$(arg wait_for_transform)"/>
-      <param name="approx_sync"              type="bool"   value="$(arg approximate_sync)"/>
-	  
-      <param name="Odom/FillInfoData"        type="string" value="true"/> 
-    </node>
-  
-    <!-- Visual SLAM (robot side) -->
-    <!-- args: "delete_db_on_start" and "udebug" -->
-    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
-      <param name="subscribe_depth"              type="bool"   value="false"/>
-      <param name="subscribe_stereo"             type="bool"   value="true"/>
-      <param name="subscribe_scan"               type="bool"   value="$(arg subscribe_scan)"/>
-      <param name="subscribe_scan_cloud"         type="bool"   value="$(arg subscribe_scan_cloud)"/>
-      <param name="frame_id"                     type="string" value="$(arg frame_id)"/>
-      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
-      <param name="database_path"                type="string" value="$(arg database_path)"/>
-      <param name="stereo_approx_sync"           type="bool"   value="$(arg approximate_sync)"/>
-	
-      <remap from="left/image_rect"        to="$(arg left_image_topic)"/>
-      <remap from="right/image_rect"       to="$(arg right_image_topic)"/>
-      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
-      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
-      <remap from="scan"                   to="$(arg scan_topic)"/>
-      <remap from="scan_cloud"      to="$(arg scan_cloud_topic)"/>
-      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
-      
-      <param name="Rtabmap/TimeThr"              type="string" value="$(arg time_threshold)"/>
-      <param name="RGBD/OptimizeFromGraphEnd"    type="string" value="$(arg optimize_from_last_node)"/>
-      <param name="Mem/SaveDepth16Format"        type="string" value="$(arg convert_depth_to_mm)"/>
-      
-      <!-- localization mode -->
-	  <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
-	  <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
-	  <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
+    <arg name="frame_id"                value="$(arg frame_id)"/>
+    <arg name="namespace"               value="$(arg namespace)"/>
+    <arg name="database_path"           value="$(arg database_path)"/>
+    <arg name="wait_for_transform"      value="$(arg wait_for_transform)"/>
+    <arg name="rtabmap_args"            value="$(arg rtabmap_args)"/>  
+    <arg name="launch_prefix"           value="$(arg launch_prefix)"/>          
+    <arg name="approx_sync"             value="$(arg approx_sync)"/>

-      <!-- when 2D scan is set -->
-      <param if="$(arg subscribe_scan)" name="Optimizer/Slam2D"        type="string" value="true"/>
-	  <param if="$(arg subscribe_scan)" name="Icp/CorrespondenceRatio" type="string" value="0.25"/>
-	  <param if="$(arg subscribe_scan)" name="Reg/Strategy"            type="string" value="1"/> 
-	  <param if="$(arg subscribe_scan)" name="Reg/Force3DoF"           type="string" value="true"/>
-
-	  <!-- when 3D scan is set -->
-	  <param if="$(arg subscribe_scan_cloud)" name="Reg/Strategy"      type="string" value="1"/>
-    </node>
+    <arg name="stereo_namespace"        value="$(arg stereo_namespace)"/>
+    <arg name="left_image_topic"        value="$(arg left_image_topic)" />
+    <arg name="right_image_topic"       value="$(arg right_image_topic)" />
+    <arg name="left_camera_info_topic"  value="$(arg left_camera_info_topic)" />
+    <arg name="right_camera_info_topic" value="$(arg right_camera_info_topic)" />
  
-    <!-- Visualisation RTAB-Map -->
-    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="$(arg rtabmapviz_cfg)" output="screen" launch-prefix="$(arg launch_prefix)">
-      <param name="subscribe_depth"           type="bool"   value="false"/>
-      <param name="subscribe_stereo"          type="bool"   value="true"/>
-      <param name="subscribe_scan"            type="bool"   value="$(arg subscribe_scan)"/>
-      <param name="subscribe_scan_cloud"      type="bool"   value="$(arg subscribe_scan_cloud)"/>
-      <param name="subscribe_odom_info"       type="bool"   value="$(arg visual_odometry)"/>
-      <param name="frame_id"                  type="string" value="$(arg frame_id)"/>
-      <param name="wait_for_transform_duration"   type="double"   value="$(arg wait_for_transform)"/>
-    
-      <remap from="left/image_rect"        to="$(arg left_image_topic)"/>
-      <remap from="right/image_rect"       to="$(arg right_image_topic)"/>
-      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
-      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
-      <remap from="scan"                   to="$(arg scan_topic)"/>
-      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
-      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
-    </node>
+    <arg name="compressed"              value="$(arg compressed)"/>                                                                                
+   
+    <arg name="subscribe_scan"          value="$(arg subscribe_scan)"/>
+    <arg name="scan_topic"              value="$(arg scan_topic)"/>
  
-  </group>
-  
-  <!-- Visualization RVIZ -->
-  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="$(arg rviz_cfg)"/>
-  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
-    <remap from="left/image"        to="$(arg left_image_topic)"/>
-    <remap from="right/image"       to="$(arg right_image_topic)"/>
-    <remap from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
-    <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
-    <remap from="cloud"             to="voxel_cloud" />
-
-    <param name="decimation"  type="double" value="2"/>
-    <param name="voxel_size"  type="double" value="0.02"/>
-    <param name="approx_sync" type="bool"   value="$(arg approximate_sync)"/>
-  </node>
+    <arg name="subscribe_scan_cloud"    value="$(arg subscribe_scan_cloud)"/>
+    <arg name="scan_cloud_topic"        value="$(arg scan_cloud_topic)"/>
+   
+    <arg name="visual_odometry"         value="$(arg visual_odometry)"/>          
+    <arg name="odom_topic"              value="$(arg odom_topic)"/>         
+    <arg name="odom_args"               value="$(arg rtabmap_args)"/>
+  </include>

 </launch>
@@ -132,7 +132,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 	double tfDelay = 0.05; // 20 Hz
 	double tfTolerance = 0.1; // 100 ms
 	std::string tfPrefix = "";
-	bool stereoApproxSync = false;
+	bool approxSync = true;

 	// ROS related parameters (private)
 	pnh.param("subscribe_depth",     subscribeDepth, subscribeDepth);
@@ -171,7 +171,17 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 	pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
 	pnh.param("depth_cameras",       depthCameras, depthCameras);
 	pnh.param("queue_size",          queueSize, queueSize);
-	pnh.param("stereo_approx_sync",  stereoApproxSync, stereoApproxSync);
+	if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync"))
+	{
+		ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed "
+				 "to \"approx_sync\"! Your value is still copied to "
+				 "corresponding parameter.");
+		pnh.param("stereo_approx_sync", approxSync, approxSync);
+	}
+	else
+	{
+		pnh.param("approx_sync", approxSync, approxSync);
+	}

 	pnh.param("publish_tf",          publishTf, publishTf);
 	pnh.param("tf_delay",            tfDelay, tfDelay);
@@ -227,6 +237,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 	ROS_INFO("rtabmap: tf_delay = %f", tfDelay);
 	ROS_INFO("rtabmap: tf_tolerance = %f", tfTolerance);
 	ROS_INFO("rtabmap: depth_cameras = %d", depthCameras);
+	ROS_INFO("rtabmap: approx_sync = %s", approxSync?"true":"false");

 	infoPub_ = nh.advertise<rtabmap_ros::Info>("info", 1);
 	mapDataPub_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1);
@@ -427,7 +438,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 	setLogWarnSrv_ = pnh.advertiseService("log_warning", &CoreWrapper::setLogWarn, this);
 	setLogErrorSrv_ = pnh.advertiseService("log_error", &CoreWrapper::setLogError, this);

-	setupCallbacks(subscribeDepth, subscribeScan2d, subscribeScan3d, subscribeStereo, queueSize, stereoApproxSync, depthCameras);
+	setupCallbacks(subscribeDepth, subscribeScan2d, subscribeScan3d, subscribeStereo, queueSize, approxSync, depthCameras);

 	int optimizeIterations = 0;
 	Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations);
@@ -2634,7 +2645,7 @@ void CoreWrapper::setupCallbacks(
 		bool subscribeScan3d,
 		bool subscribeStereo,
 		int queueSize,
-		bool stereoApproxSync,
+		bool approxSync,
 		int depthCameras)
 {
 	ros::NodeHandle nh; // public
@@ -2679,7 +2690,6 @@ void CoreWrapper::setupCallbacks(
 			odomSub_.subscribe(nh, "odom", 1);
 			if(subscribeScan2d)
 			{
-				ROS_INFO("Registering Depth+LaserScan callback...");
 				scanSub_.subscribe(nh, "scan", 1);
 				depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(
 						MyDepthScanSyncPolicy(queueSize),
@@ -2700,7 +2710,6 @@ void CoreWrapper::setupCallbacks(
 			}
 			else if(subscribeScan3d)
 			{
-				ROS_INFO("Registering Depth+LaserScan3d callback...");
 				scan3dSub_.subscribe(nh, "scan_cloud", 1);
 				depthScan3dSync_ = new message_filters::Synchronizer<MyDepthScan3dSyncPolicy>(
 						MyDepthScan3dSyncPolicy(queueSize),
@@ -2723,7 +2732,6 @@ void CoreWrapper::setupCallbacks(
 			{
 				if(depthCameras > 1)
 				{
-					ROS_INFO("Registering Depth2 callback...");
 					depth2Sync_ = new message_filters::Synchronizer<MyDepth2SyncPolicy>(
 							MyDepth2SyncPolicy(queueSize),
 							odomSub_,
@@ -2747,17 +2755,29 @@ void CoreWrapper::setupCallbacks(
 				}
 				else
 				{
-					ROS_INFO("Registering Depth callback...");
-					depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(
-							MyDepthSyncPolicy(queueSize),
-							*imageSubs_[0],
-							odomSub_,
-							*imageDepthSubs_[0],
-							*cameraInfoSubs_[0]);
-					depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
-
-					ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s",
+					if(approxSync)
+					{
+						depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(
+								MyDepthSyncPolicy(queueSize),
+								*imageSubs_[0],
+								odomSub_,
+								*imageDepthSubs_[0],
+								*cameraInfoSubs_[0]);
+						depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
+					}
+					else
+					{
+						depthExactSync_ = new message_filters::Synchronizer<MyDepthExactSyncPolicy>(
+								MyDepthExactSyncPolicy(queueSize),
+								*imageSubs_[0],
+								odomSub_,
+								*imageDepthSubs_[0],
+								*cameraInfoSubs_[0]);
+						depthExactSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
+					}
+					ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
 							ros::this_node::getName().c_str(),
+							approxSync?"approx":"exact",
 							imageSubs_[0]->getTopic().c_str(),
 							imageDepthSubs_[0]->getTopic().c_str(),
 							cameraInfoSubs_[0]->getTopic().c_str(),
@@ -2806,15 +2826,27 @@ void CoreWrapper::setupCallbacks(
 			}
 			else //!subscribeLaserScan
 			{
-				depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>(
-						MyDepthTFSyncPolicy(queueSize),
-						*imageSubs_[0],
-						*imageDepthSubs_[0],
-						*cameraInfoSubs_[0]);
-				depthTFSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3));
-
-				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s",
+				if(approxSync)
+				{
+					depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>(
+							MyDepthTFSyncPolicy(queueSize),
+							*imageSubs_[0],
+							*imageDepthSubs_[0],
+							*cameraInfoSubs_[0]);
+					depthTFSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3));
+				}
+				else
+				{
+					depthTFExactSync_ = new message_filters::Synchronizer<MyDepthTFExactSyncPolicy>(
+							MyDepthTFExactSyncPolicy(queueSize),
+							*imageSubs_[0],
+							*imageDepthSubs_[0],
+							*cameraInfoSubs_[0]);
+					depthTFExactSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3));
+				}
+				ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s",
 						ros::this_node::getName().c_str(),
+						approxSync?"approx":"exact",
 						imageSubs_[0]->getTopic().c_str(),
 						imageDepthSubs_[0]->getTopic().c_str(),
 						cameraInfoSubs_[0]->getTopic().c_str());
@@ -2886,9 +2918,8 @@ void CoreWrapper::setupCallbacks(
 			}
 			else //!subscribeLaserScan
 			{
-				if(stereoApproxSync)
+				if(approxSync)
 				{
-					ROS_INFO("Registering Stereo Approx callback...");
 					stereoApproxSync_ = new message_filters::Synchronizer<MyStereoApproxSyncPolicy>(
 							MyStereoApproxSyncPolicy(queueSize),
 							imageRectLeft_,
@@ -2900,7 +2931,6 @@ void CoreWrapper::setupCallbacks(
 				}
 				else
 				{
-					ROS_INFO("Registering Stereo Exact callback...");
 					stereoExactSync_ = new message_filters::Synchronizer<MyStereoExactSyncPolicy>(
 							MyStereoExactSyncPolicy(queueSize),
 							imageRectLeft_,
@@ -2911,8 +2941,9 @@ void CoreWrapper::setupCallbacks(
 					stereoExactSync_->registerCallback(boost::bind(&CoreWrapper::stereoCallback, this, _1, _2, _3, _4, _5));
 				}

-				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
+				ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
 						ros::this_node::getName().c_str(),
+						approxSync?"approx":"exact",
 						imageRectLeft_.getTopic().c_str(),
 						imageRectRight_.getTopic().c_str(),
 						cameraInfoLeft_.getTopic().c_str(),
@@ -2925,7 +2956,6 @@ void CoreWrapper::setupCallbacks(
 			// use odom from TF, so subscribe to sensors only
 			if(subscribeScan2d)
 			{
-				ROS_INFO("Registering Stereo+LaserScan2d+OdomTF callback...");
 				scanSub_.subscribe(nh, "scan", 1);
 				stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>(
 						MyStereoScanTFSyncPolicy(queueSize),
@@ -2946,7 +2976,6 @@ void CoreWrapper::setupCallbacks(
 			}
 			else if(subscribeScan3d)
 			{
-				ROS_INFO("Registering Stereo+LaserScan3d+OdomTF callback...");
 				scan3dSub_.subscribe(nh, "scan_cloud", 1);
 				stereoScan3dTFSync_ = new message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy>(
 						MyStereoScan3dTFSyncPolicy(queueSize),
@@ -2967,9 +2996,8 @@ void CoreWrapper::setupCallbacks(
 			}
 			else //!subscribeLaserScan
 			{
-				if(stereoApproxSync)
+				if(approxSync)
 				{
-					ROS_INFO("Registering Stereo+OdomTF Approx callback...");
 					stereoApproxTFSync_ = new message_filters::Synchronizer<MyStereoApproxTFSyncPolicy>(
 							MyStereoApproxTFSyncPolicy(queueSize),
 							imageRectLeft_,
@@ -2980,7 +3008,6 @@ void CoreWrapper::setupCallbacks(
 				}
 				else
 				{
-					ROS_INFO("Registering Stereo+OdomTF Exact callback...");
 					stereoExactTFSync_ = new message_filters::Synchronizer<MyStereoExactTFSyncPolicy>(
 							MyStereoExactTFSyncPolicy(queueSize),
 							imageRectLeft_,
@@ -2990,8 +3017,9 @@ void CoreWrapper::setupCallbacks(
 					stereoExactTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoTFCallback, this, _1, _2, _3, _4));
 				}

-				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s",
+				ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
 						ros::this_node::getName().c_str(),
+						approxSync?"approx":"exact",
 						imageRectLeft_.getTopic().c_str(),
 						imageRectRight_.getTopic().c_str(),
 						cameraInfoLeft_.getTopic().c_str(),
@@ -90,7 +90,7 @@ private:
 			bool subscribeScan3d,
 			bool subscribeStereo,
 			int queueSize,
-			bool stereoApproxSync,
+			bool approxSync,
 			int depthCameras);
 	void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom

@@ -338,6 +338,12 @@ private:
 			sensor_msgs::Image,
 			sensor_msgs::CameraInfo> MyDepthSyncPolicy;
 	message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
+	typedef message_filters::sync_policies::ExactTime<
+			sensor_msgs::Image,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthExactSyncPolicy;
+	message_filters::Synchronizer<MyDepthExactSyncPolicy> * depthExactSync_;

 	typedef message_filters::sync_policies::ApproximateTime<
 			sensor_msgs::Image,
@@ -403,6 +409,11 @@ private:
 			sensor_msgs::Image,
 			sensor_msgs::CameraInfo> MyDepthTFSyncPolicy;
 	message_filters::Synchronizer<MyDepthTFSyncPolicy> * depthTFSync_;
+	typedef message_filters::sync_policies::ExactTime<
+				sensor_msgs::Image,
+				sensor_msgs::Image,
+				sensor_msgs::CameraInfo> MyDepthTFExactSyncPolicy;
+	message_filters::Synchronizer<MyDepthTFExactSyncPolicy> * depthTFExactSync_;

 	typedef message_filters::sync_policies::ApproximateTime<
 			sensor_msgs::Image,
@@ -54,7 +54,8 @@ class RGBDOdometry : public rtabmap_ros::OdometryROS
 public:
 	RGBDOdometry(int argc, char * argv[]) :
 		rtabmap_ros::OdometryROS(argc, argv),
-		sync_(0),
+		approxSync_(0),
+		exactSync_(0),
 		sync2_(0),
 		queueSize_(5)
 	{
@@ -62,6 +63,8 @@ public:
 		ros::NodeHandle pnh("~");

 		int depthCameras = 1;
+		bool approxSync = true;
+		pnh.param("approx_sync", approxSync, approxSync);
 		pnh.param("queue_size", queueSize_, queueSize_);
 		pnh.param("depth_cameras", depthCameras, depthCameras);
 		if(depthCameras <= 0)
@@ -135,22 +138,35 @@ public:
 			image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
 			info_sub_.subscribe(rgb_nh, "camera_info", 1);

-			ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s",
+			if(approxSync)
+			{
+				approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+				approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
+			}
+			else
+			{
+				exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+				exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
+			}
+
+			ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s",
 					ros::this_node::getName().c_str(),
+					approxSync?"approx":"exact",
 					image_mono_sub_.getTopic().c_str(),
 					image_depth_sub_.getTopic().c_str(),
 					info_sub_.getTopic().c_str());
-
-			sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
-			sync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
 		}
 	}

 	virtual ~RGBDOdometry()
 	{
-		if(sync_)
+		if(approxSync_)
 		{
-			delete sync_;
+			delete approxSync_;
+		}
+		if(exactSync_)
+		{
+			delete exactSync_;
 		}
 		if(sync2_)
 		{
@@ -343,11 +359,17 @@ protected:
 	virtual void flushCallbacks()
 	{
 		// flush callbacks
-		if(sync_)
+		if(approxSync_)
 		{
-			delete sync_;
-			sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
-			sync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
+			delete approxSync_;
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+			approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
+		}
+		if(exactSync_)
+		{
+			delete exactSync_;
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+			exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
 		}
 		if(sync2_)
 		{
@@ -371,8 +393,10 @@ private:
 	image_transport::SubscriberFilter image_mono2_sub_;
 	image_transport::SubscriberFilter image_depth2_sub_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> info2_sub_;
-	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
-	message_filters::Synchronizer<MySyncPolicy> * sync_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
+	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
+	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
 	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySync2Policy;
 	message_filters::Synchronizer<MySync2Policy> * sync2_;
 	int queueSize_;
@@ -80,13 +80,6 @@ public:
 		cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
 		cameraInfoRight_.subscribe(right_nh, "camera_info", 1);

-		ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s",
-				ros::this_node::getName().c_str(),
-				imageRectLeft_.getTopic().c_str(),
-				imageRectRight_.getTopic().c_str(),
-				cameraInfoLeft_.getTopic().c_str(),
-				cameraInfoRight_.getTopic().c_str());
-
 		if(approxSync)
 		{
 			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
@@ -97,6 +90,15 @@ public:
 			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
 			exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
 		}
+
+
+		ROS_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
+				ros::this_node::getName().c_str(),
+				approxSync?"approx":"exact",
+				imageRectLeft_.getTopic().c_str(),
+				imageRectRight_.getTopic().c_str(),
+				cameraInfoLeft_.getTopic().c_str(),
+				cameraInfoRight_.getTopic().c_str());
 	}

 	virtual ~StereoOdometry()