remove trailing whitespace from launch files

2026-10-04 16:57:46 +08:00 · 2023-01-15 07:00:31 -08:00
parent a42f00191a
commit cec01bc0c8
87 changed files with 1024 additions and 1024 deletions
@@ -9,22 +9,22 @@
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
 	  <param name="frame_id" type="string" value="base_footprint"/>
-	
+
 	  <param name="subscribe_depth" type="bool" value="true"/>
 	  <param name="subscribe_laserScan" type="bool" value="true"/>
-	
+
 	  <remap from="odom" to="/base_controller/odom"/>
 	  <remap from="scan" to="/base_scan"/>
 	  <remap from="rgb/image" to="/camera/data_throttled_image"/>
  	  <remap from="depth/image" to="/camera/data_throttled_image_depth"/>
  	  <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
-  	  
+
  	  <param name="rgb/image_transport" type="string" value="compressed"/>
     	  <param name="depth/image_transport" type="string" value="compressedDepth"/>
-	
+
 	  <param name="queue_size" type="int" value="10"/>
-	
+
 	  <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
 	  <param name="RGBD/ScanMatchingSize" type="string" value="1"/>             <!-- Do odometry correction with consecutive laser scans -->
 	  <param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>   <!-- Local loop closure detection (using estimated position) with locations in WM -->
@@ -40,10 +40,10 @@
 	  <param name="RGBD/LinearUpdate" type="string" value="0.01"/>              <!-- Update map only if the robot is moving -->
 	  <param name="Rtabmap/TimeThr" type="string" value="700"/>
 	  <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
-	  <param name="Mem/RehearsedNodesKept" type="string" value="false"/>   
+	  <param name="Mem/RehearsedNodesKept" type="string" value="false"/>
    </node>
-  </group> 
+  </group>
-  
+
  <!-- send AZIMUT 3 urdf to param server -->
  <param name="robot_description" command="$(find xacro)/xacro.py '$(find az3_description)/robots/azimut_3_laser.urdf.xacro'" />
@@ -2,7 +2,7 @@
 <launch>
  <!-- rosbag record camera/data_throttled_image/compressed camera/data_throttled_image_depth/compressedDepth camera/data_throttled_camera_info tf base_scan /base_controller/odom -->
-  
+
  <include file="$(find az3_bringup)/az3_standalone.launch"/>
 <!-- To control with only one joystick -->
@@ -18,20 +18,20 @@
  <!-- OpenNI -->
  <include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
-  
+
  <!-- Throttling messages -->
  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
      <param name="rate" type="double" value="10.0"/>
-   
+
      <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
      <remap from="depth/image_in"     to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
-    
+
      <remap from="rgb/image_out"       to="data_throttled_image"/>
      <remap from="depth/image_out"     to="data_throttled_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
-    </node>   
+    </node>
  </group>
 </launch>
@@ -9,38 +9,38 @@
  <!-- OpenNI -->
  <include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
-  
+
  <!-- Throttling messages -->
  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
      <param name="rate" type="double" value="10.0"/>
-   
+
      <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
      <remap from="depth/image_in"     to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info_in" to="rgb/camera_info"/>
-    
+
      <remap from="rgb/image_out"       to="data_throttled_image"/>
      <remap from="depth/image_out"     to="data_throttled_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
-    </node>   
+    </node>
  </group>
  <node name="data_recorder" pkg="rtabmap_ros" type="data_recorder" output="screen">
-	<param name="output_file_name" value="az3_record.db" type="string"/>  
+	<param name="output_file_name" value="az3_record.db" type="string"/>
-	
+
 	<param name="frame_id" type="string" value="base_footprint"/>
-	
+
 	<param name="subscribe_odometry" type="bool" value="true"/>
 	<param name="subscribe_depth" type="bool" value="true"/>
 	<param name="subscribe_laserScan" type="bool" value="true"/>
-	
+
 	<remap from="odom" to="/base_controller/odom"/>
 	<remap from="scan" to="/base_scan"/>
 	<remap from="rgb/image" to="camera/data_throttled_image"/>
  	<remap from="depth/image" to="camera/data_throttled_image_depth"/>
  	<remap from="rgb/camera_info" to="camera/data_throttled_camera_info"/>
-  	
+
 	<param name="queue_size" type="int" value="10"/>
  </node>
@@ -17,9 +17,9 @@
    <remap from="mapData" to="rtabmap/mapData_relay"/>
    <remap from="grid_map" to="rtabmap/grid_map"/>
  </node>
-    
+
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/azimut3/config/azimut3.rviz"/>
-  
+
  <!-- Below, construct point cloud of the latest throttled data, disabled for bandwidth efficiency -->
  <node pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
@@ -27,7 +27,7 @@
    <remap from="depth/image"     to="/camera/data_throttled_image_depth_relay"/>
    <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info_relay"/>
    <remap from="cloud"           to="voxel_cloud" />
-    
+
    <param name="queue_size" type="int" value="10"/>
    <param name="voxel_size" type="double" value="0.01"/>
  </node>
@@ -5,7 +5,7 @@
  <!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
  <include file="$(find az3_bringup)/az3_standalone.launch"/>
  <!-- <include file="$(find az3_bringup)/joystick.launch"/> -->
-  
+
  <!-- OpenNI -->
  <include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
@@ -13,35 +13,35 @@
  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
      <param name="rate" type="double" value="5.0"/>
-   
+
      <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
      <remap from="depth/image_in"     to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
-    
+
      <remap from="rgb/image_out"       to="data_throttled_image"/>
      <remap from="depth/image_out"     to="data_throttled_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
-    </node>  
+    </node>
-  </group>  
+  </group>
  <!-- SLAM (robot side) -->
  <!-- args: "delete_db_on_start" and "udebug" -->
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
 	  <param name="frame_id" type="string" value="base_footprint"/>
-	
+
 	  <param name="subscribe_depth" type="bool" value="true"/>
 	  <param name="subscribe_laserScan" type="bool" value="true"/>
-	
+
 	  <remap from="odom" to="/base_controller/odom"/>
 	  <remap from="scan" to="/base_scan"/>
 	  <remap from="rgb/image" to="/camera/data_throttled_image"/>
  	  <remap from="depth/image" to="/camera/data_throttled_image_depth"/>
  	  <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
-	
+
 	  <param name="queue_size" type="int" value="10"/>
-	
+
 	  <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
 	  <param name="RGBD/ScanMatchingSize" type="string" value="1"/>             <!-- Do odometry correction with consecutive laser scans -->
 	  <param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>   <!-- Local loop closure detection (using estimated position) with locations in WM -->
@@ -57,7 +57,7 @@
 	  <param name="RGBD/LinearUpdate" type="string" value="0.01"/>              <!-- Update map only if the robot is moving -->
 	  <param name="Rtabmap/TimeThr" type="string" value="700"/>
 	  <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
-	  <param name="Mem/RehearsedNodesKept" type="string" value="false"/>   
+	  <param name="Mem/RehearsedNodesKept" type="string" value="false"/>
    </node>
  </group>
 </launch>
@@ -5,7 +5,7 @@
  <!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
  <include file="$(find az3_bringup)/az3_standalone.launch"/>
  <!-- <include file="$(find az3_bringup)/joystick.launch"/> -->
-  
+
  <!-- OpenNI -->
  <include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
@@ -13,16 +13,16 @@
  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
      <param name="rate" type="double" value="5.0"/>
-   
+
      <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
      <remap from="depth/image_in"     to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
-    
+
      <remap from="rgb/image_out"       to="data_throttled_image"/>
      <remap from="depth/image_out"     to="data_throttled_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
-    </node>  
+    </node>
-  </group>  
+  </group>
  <!-- SLAM (robot side) -->
  <!-- args: "delete_db_on_start" and "udebug" -->
@@ -30,14 +30,14 @@
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
 	  <param name="frame_id" type="string" value="base_footprint"/>
 	  <param name="subscribe_depth" type="bool" value="true"/>
-	
+
 	  <remap from="odom" to="/base_controller/odom"/>
 	  <remap from="rgb/image" to="/camera/data_throttled_image"/>
  	  <remap from="depth/image" to="/camera/data_throttled_image_depth"/>
  	  <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
-	
+
 	  <param name="queue_size" type="int" value="10"/>
-	
+
 	  <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
 	  <param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/>    <!-- Local loop closure detection with locations in STM -->
 	  <param name="Mem/BadSignaturesIgnored" type="string" value="false"/>      <!-- Don't ignore bad images for 3D node creation (e.g. white walls) -->
@@ -45,7 +45,7 @@
 	  <param name="RGBD/LinearUpdate" type="string" value="0.01"/>              <!-- Update map only if the robot is moving -->
 	  <param name="Rtabmap/TimeThr" type="string" value="700"/>
 	  <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
-	  <param name="Mem/RehearsedNodesKept" type="string" value="false"/>   
+	  <param name="Mem/RehearsedNodesKept" type="string" value="false"/>
    </node>
  </group>
 </launch>
@@ -20,7 +20,7 @@
    <remap from="odom" to="/base_controller/odom"/>
    <param name="Odom/MinInliers" type="string" value="10"/>
-    <param name="Odom/InlierDistance" type="string" value="0.01"/>    
+    <param name="Odom/InlierDistance" type="string" value="0.01"/>
    <param name="frame_id" type="string" value="base_footprint"/>
  </node>
@@ -7,7 +7,7 @@
  <!-- AZIMUT 3 bringup: launch motors/odometry -->
  <include file="$(find az3_bringup)/az3_standalone.launch"/>
-  
+
  <!-- OpenNI -->
  <include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
@@ -24,7 +24,7 @@
      <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
-      <remap from="goal_out"        to="current_goal"/>	
+      <remap from="goal_out"        to="current_goal"/>
      <remap from="move_base"       to="/planner/move_base"/>
      <remap from="grid_map"        to="/map"/>
@@ -32,8 +32,8 @@
      <param unless="$(arg localization)" name="Rtabmap/TimeThr"        type="string" value="500"/>
      <param if="$(arg localization)"     name="Mem/IncrementalMemory"  type="string" value="false"/>
      <param if="$(arg localization)"     name="Mem/InitWMWithAllNodes" type="string" value="true"/>
-      <param name="RGBD/PoseScanMatching"        type="string" value="true"/>           
+      <param name="RGBD/PoseScanMatching"        type="string" value="true"/>
-      <param name="RGBD/LocalRadius"             type="string" value="4"/>  
+      <param name="RGBD/LocalRadius"             type="string" value="4"/>
      <param name="Mem/RehearsalSimilarity"      type="string" value="0.30"/>
      <param name="Rtabmap/DetectionRate"        type="string" value="1"/>
      <param name="RGBD/OptimizeSlam2d"          type="string" value="true"/>
@@ -45,7 +45,7 @@
      <param name="LccIcp2/CorrespondenceRatio"  type="string" value="0.2"/>
    </node>
  </group>
-  
+
  <!-- teleop -->
  <node name="joy" pkg="joy" type="joy_node"/>
  <group ns="teleop">
@@ -60,7 +60,7 @@
     <remap from="obstacles_cloud"       to="/obstacles_cloud"/>
     <remap from="ground_cloud"          to="/ground_cloud"/>
     <remap from="map"                   to="/map"/>
-        
+
     <node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
        <param name="base_global_planner" value="navfn/NavfnROS"/>
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml"  command="load" ns="global_costmap" />
@@ -69,7 +69,7 @@
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml"     command="load" ns="global_costmap"/>
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
     </node>
-   		
+
     <param name="cmd_vel/abtr_priority" value="10"/>
  </group>
@@ -82,7 +82,7 @@
        args="/cmd_eta /teleop/cmd_eta"/>
  <node name="register_cmd_vel" pkg="abtr_priority" type="register"
        args="/cmd_vel /planner/cmd_vel"/>
-  
+
  <!-- Throttling messages -->
  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
@@ -91,7 +91,7 @@
      <remap from="rgb/image_in"        to="rgb/image_rect_color"/>
      <remap from="depth/image_in"      to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info_in"  to="depth_registered/camera_info"/>
-    
+
      <remap from="rgb/image_out"       to="throttled_image"/>
      <remap from="depth/image_out"     to="throttled_image_depth"/>
      <remap from="rgb/camera_info_out" to="throttled_camera_info"/>
@@ -107,26 +107,26 @@
      <param name="max_depth"   type="double" value="4.0"/>
      <param name="voxel_size"  type="double" value="0.02"/>
    </node>
-  
+
    <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection obstacle_nodelet_manager">
      <remap from="cloud"     to="cloudXYZ"/>
      <remap from="obstacles" to="/obstacles_cloud"/>
      <remap from="ground"    to="/ground_cloud"/>
-      <param name="frame_id"             type="string" value="base_footprint"/>		
+      <param name="frame_id"             type="string" value="base_footprint"/>
      <param name="map_frame_id"         type="string" value="map"/>
      <param name="wait_for_transform"   type="bool" value="true"/>
      <param name="min_cluster_size"     type="int" value="20"/>
      <param name="max_obstacles_height" type="double" value="0.4"/>
      <param name="ground_normal_angle"  type="double" value="0.1"/>
    </node>
-    
+
    <!-- scan from the camera -->
    <node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan" args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet camera_nodelet_manager">
      <remap from="image"       to="depth_registered/image_raw"/>
      <remap from="camera_info" to="depth_registered/camera_info"/>
      <remap from="scan"        to="/kinect_scan"/>
      <param name="range_max" type="double" value="4"/>
-    </node>   
+    </node>
  </group>
 </launch>
@@ -10,7 +10,7 @@
 	<!-- AZIMUT 3 bringup: launch motors and TF -->
 	<include file="$(find az3_bringup)/az3_standalone.launch"/>
-	
+
 	<node name="joy" pkg="joy" type="joy_node"/>
    <group ns="teleop">
        <remap from="joy" to="/joy"/>
@@ -24,7 +24,7 @@
        <remap from="base_scan" to="/base_scan"/>
        <remap from="map" to="/rtabmap/proj_map"/>
        <remap from="move_base_simple/goal" to="/planner_goal"/>
-        
+
        <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    	  <rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
     	  <rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
@@ -32,7 +32,7 @@
    	  <rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
    	  <rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
   		</node>
-   		
+
        <param name="cmd_vel/abtr_priority" value="10"/>
    </group>
@@ -45,7 +45,7 @@
        args="/cmd_eta /teleop/cmd_eta"/>
    <node name="register_cmd_vel" pkg="abtr_priority" type="register"
        args="/cmd_vel /planner/cmd_vel"/>
-  
+
 	<!-- Stereo camera -->
 	<node pkg="camera1394stereo" type="camera1394stereo_node" name="camera1394stereo_node" output="screen" >
 	    <param name="video_mode" value="format7_mode3" />
@@ -56,15 +56,15 @@
 	    <param name="camera_info_url_left" value="" />
 	    <param name="camera_info_url_right" value="" />
 	</node>
-  
+
  	<!-- TF transforms for the stereo camera -->
  	<arg name="pi/2" value="1.5707963267948966" />
 	<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
 	<node pkg="tf" type="static_transform_publisher" name="stereo_camera_base_link"
-	    args="$(arg optical_rotate) stereo_camera_base stereo_camera 100" />  
+	    args="$(arg optical_rotate) stereo_camera_base stereo_camera 100" />
 	<node pkg="tf" type="static_transform_publisher" name="base_to_stereo_camera_base_link"
-	    args="0.01 0.06 0.90 0 0.37 0 base_link stereo_camera_base 100" />  
+	    args="0.01 0.06 0.90 0 0.37 0 base_link stereo_camera_base 100" />
-	    
+
 	<!-- Run the ROS package stereo_image_proc for image rectification-->
 	<group ns="/stereo_camera" >
 	    <node pkg="nodelet" type="nodelet" name="stereo_nodelet"  args="manager"/>
@@ -75,11 +75,11 @@
 		    <remap from="right/image"      to="right/image_raw"/>
 		    <remap from="left/camera_info" to="left/camera_info"/>
 		    <remap from="right/camera_info" to="right/camera_info"/>
-		    
+
 		    <param name="queue_size" type="int" value="10"/>
 		    <param name="rate" type="double" value="20"/>
 		</node>
-	
+
 		<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
 			<remap from="left/image_raw"    to="left/image_raw_throttle"/>
 			<remap from="left/camera_info"  to="left/camera_info_throttle"/>
@@ -87,13 +87,13 @@
 			<remap from="right/camera_info" to="right/camera_info_throttle"/>
 			<param name="disparity_range" value="128"/>
 		</node>
-		
+
 		<!-- Create point cloud for the planner -->
 		<node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap_ros/point_cloud_xyz stereo_nodelet">
 			<remap from="disparity/image"       to="disparity"/>
 			<remap from="disparity/camera_info" to="right/camera_info_throttle"/>
 			<remap from="cloud"                 to="cloudXYZ"/>
-			
+
 			<param name="voxel_size" type="double" value="0.05"/>
 			<param name="decimation" type="int" value="4"/>
 			<param name="max_depth" type="double" value="4"/>
@@ -102,14 +102,14 @@
    			<remap from="cloud" to="cloudXYZ"/>
 			<remap from="obstacles" to="/planner_cloud"/>
-			<param name="frame_id" type="string" value="base_footprint"/>		
+			<param name="frame_id" type="string" value="base_footprint"/>
 			<param name="map_frame_id" type="string" value="map"/>
 			<param name="wait_for_transform" type="bool" value="true"/>
 			<param name="min_cluster_size" type="int" value="20"/>
 			<param name="max_obstacles_height" type="double" value="0.0"/>
  		</node>
 	</group>
-	
+
 	<!-- Visual Odometry -->
 	<node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
      <remap from="left/image_rect"       to="/stereo_camera/left/image_rect"/>
@@ -125,12 +125,12 @@
      <param name="Odom/MinInliers" type="string" value="10"/>
      <param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
      <param name="Odom/MaxDepth" type="string" value="10"/>
-      
+
      <param name="GFTT/MaxCorners" type="string" value="500"/>
      <param name="GFTT/MinDistance" type="string" value="5"/>
 	</node>
-  <group ns="rtabmap">   
+  <group ns="rtabmap">
      <!-- Visual SLAM: args: "delete_db_on_start" and "udebug" -->
      <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
         <param name="frame_id" type="string" value="base_footprint"/>
@@ -149,7 +149,7 @@
         <!-- RTAB-Map's parameters -->
         <param name="Rtabmap/TimeThr" type="string" value="700"/>
         <param name="Rtabmap/DetectionRate" type="string" value="1"/>
-         
+
         <param name="Kp/WordsPerImage" type="string" value="200"/>
         <param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
@@ -163,5 +163,5 @@
         <param name="LccReextract/MaxWords" type="string" value="500"/>
      </node>
   </group>
-  
+
 </launch>
@@ -31,24 +31,24 @@
      <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
-      <remap from="goal_out"  to="current_goal"/>	
+      <remap from="goal_out"  to="current_goal"/>
      <remap from="move_base" to="/planner/move_base"/>
      <remap from="grid_map"  to="/map"/>
      <!-- RTAB-Map's parameters -->
-      <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>           
+      <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
      <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
-  
+
-      <param name="Reg/Strategy" type="string" value="1"/>                    
+      <param name="Reg/Strategy" type="string" value="1"/>
-	 
+
-      <param name="RGBD/AngularUpdate" type="string" value="0.1"/> 
+      <param name="RGBD/AngularUpdate" type="string" value="0.1"/>
-      <param name="RGBD/LinearUpdate"  type="string" value="0.1"/> 
+      <param name="RGBD/LinearUpdate"  type="string" value="0.1"/>
      <param name="RGBD/LocalRadius"   type="string" value="5"/>
-	  
+
      <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
      <param name="Mem/NotLinkedNodesKept"  type="string" value="false"/>
      <param name="Mem/ImagePostDecimation"     type="string" value="4"/>
-      
+
      <param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="false"/>
      <param name="Rtabmap/TimeThr"                  type="string" value="600"/>
      <param name="Rtabmap/DetectionRate"            type="string" value="1"/>
@@ -57,16 +57,16 @@
      <param name="Optimizer/Slam2D"          type="string" value="true"/>
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
-      <param name="Optimizer/Strategy"        type="string" value="0"/>      
+      <param name="Optimizer/Strategy"        type="string" value="0"/>
- 
+
      <param name="Kp/DetectorStrategy"   type="string" value="0"/>
      <param name="Kp/MaxFeatures"        type="string" value="200"/>
      <param name="SURF/HessianThreshold" type="string" value="500"/>
-	  
+
      <param name="Reg/Force3DoF"  type="string" value="true"/>
      <param name="Vis/MaxDepth"   type="string" value="5"/>
      <param name="Vis/MinInliers" type="string" value="5"/>
-      <param name="Icp/CorrespondenceRatio" type="string" value="0.3"/>      
+      <param name="Icp/CorrespondenceRatio" type="string" value="0.3"/>
      <!-- localization mode -->
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
@@ -74,7 +74,7 @@
      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
    </node>
  </group>
-  
+
  <!-- teleop -->
  <node name="joy" pkg="joy" type="joy_node"/>
  <group ns="teleop">
@@ -98,7 +98,7 @@
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
     </node>
-   		
+
     <param name="cmd_vel/abtr_priority" value="10"/>
  </group>
@@ -111,17 +111,17 @@
        args="/cmd_eta /teleop/cmd_eta"/>
  <node name="register_cmd_vel" pkg="abtr_priority" type="register"
        args="/cmd_vel /planner/cmd_vel"/>
-  
+
  <!-- Throttling messages -->
  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
      <param name="rate" type="double" value="5"/>
      <param name="decimation" type="int" value="2"/>
-   
+
      <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
      <remap from="depth/image_in"     to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
-    
+
      <remap from="rgb/image_out"       to="data_resized_image"/>
      <remap from="depth/image_out"     to="data_resized_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_resized_camera_info"/>
@@ -136,17 +136,17 @@
      <param name="max_depth"  type="double" value="3.0"/>
      <param name="voxel_size" type="double" value="0.02"/>
    </node>
-  
+
    <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection camera_nodelet_manager">
      <remap from="cloud" to="cloudXYZ"/>
      <remap from="obstacles" to="/obstacles_cloud"/>
      <remap from="ground"    to="/ground_cloud"/>
-      <param name="frame_id"             type="string" value="base_footprint"/>		
+      <param name="frame_id"             type="string" value="base_footprint"/>
      <param name="map_frame_id"         type="string" value="map"/>
      <param name="wait_for_transform"   type="bool" value="true"/>
      <param name="min_cluster_size"     type="int" value="20"/>
      <param name="max_obstacles_height" type="double" value="0.4"/>
-    </node>  
+    </node>
  </group>
 </launch>
@@ -6,7 +6,7 @@
  <arg name="rviz" default="true" />
  <arg name="rtabmapviz" default="false" />
  <arg name="sub_data" default="false"/>
-  
+
   <!-- use a relay on this machine -->
  <node name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData /rtabmap/mapData_relay">
    <param name="lazy" type="bool" value="true"/>
@@ -14,10 +14,10 @@
  <node if="$(arg sub_data)" name="scan_relay" type="relay" pkg="topic_tools" args="/base_scan /base_scan_relay">
    <param name="lazy" type="bool" value="true"/>
  </node>
-  
+
  <node if="$(arg sub_data)" name="republish_rgb" type="republish" pkg="image_transport" args="theora in:=/camera/data_resized_image raw out:=/camera/data_resized_image_relay" />
  <node if="$(arg sub_data)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=/camera/data_resized_image_depth raw out:=/camera/data_resized_image_depth_relay" />
-  
+
  <node if="$(arg sub_data)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
    <remap from="rgb/image"       to="/camera/data_resized_image_relay"/>
    <remap from="depth/image"     to="/camera/data_resized_image_depth_relay"/>
@@ -25,24 +25,24 @@
    <remap from="cloud"           to="/voxel_cloud" />
  </node>
-  <group ns="rtabmap">  
+  <group ns="rtabmap">
  <!-- Visualisation RTAB-Map -->
  <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
      <param name="queue_size" type="int" value="10"/>
      <param name="frame_id" type="string" value="base_footprint"/>
      <remap from="odom" to="/base_controller/odom"/>
      <remap from="mapData" to="mapData_relay"/>
-      
+
      <param name="subscribe_depth" type="bool" value="$(arg sub_data)"/>
      <remap from="rgb/image" to="/camera/data_resized_image_relay"/>
      <remap from="depth/image" to="/camera/data_resized_image_depth_relay"/>
      <remap from="rgb/camera_info" to="/camera/data_resized_camera_info"/>
-      
+
      <param name="subscribe_laserScan" type="bool" value="$(arg sub_data)"/>
      <remap from="scan" to="/base_scan_relay"/>
  </node>
  </group>
-    
+
  <!-- Visualisation RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/azimut3/config/azimut3_nav.rviz"/>
 </launch>
@@ -5,8 +5,8 @@
  <!-- Localization-only mode -->
  <arg name="localization"      default="false"/>
  <arg     if="$(arg localization)" name="rtabmap_args"  default=""/>
-  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>  
+  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>
-  
+
  <!-- "Disable" odometry from azimut3 -->
  <group ns="base_controller">
      <param name="odom_frame_id" type="string" value="az3_odom"/>
@@ -30,8 +30,8 @@
      <param name="Vis/MinInliers" type="string" value="10"/>
      <param name="Vis/InlierDistance" type="string" value="0.1"/>
-      <param name="Vis/MaxDepth" type="string" value="4"/>    
+      <param name="Vis/MaxDepth" type="string" value="4"/>
-      <param name="Reg/Force3DoF" type="string" value="true"/>     
+      <param name="Reg/Force3DoF" type="string" value="true"/>
      <param name="frame_id" type="string" value="base_footprint"/>
    </node>
@@ -5,7 +5,7 @@
  <!-- Localization-only mode -->
  <arg name="localization"      default="false"/>
  <arg     if="$(arg localization)" name="rtabmap_args"  default=""/>
-  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>  
+  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>
  <!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
  <include file="$(find az3_bringup)/az3_standalone.launch"/>
@@ -31,24 +31,24 @@
      <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
-      <remap from="goal_out"  to="current_goal"/>	
+      <remap from="goal_out"  to="current_goal"/>
      <remap from="move_base" to="/planner/move_base"/>
      <remap from="proj_map"  to="/map"/>
      <!-- RTAB-Map's parameters -->
-      <param name="RGBD/NeighborLinkRefining" type="string" value="false"/>           
+      <param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
      <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
-  
+
-      <param name="Reg/Strategy"              type="string" value="0"/>                    
+      <param name="Reg/Strategy"              type="string" value="0"/>
-	 
+
-      <param name="RGBD/AngularUpdate"        type="string" value="0.1"/>    
+      <param name="RGBD/AngularUpdate"        type="string" value="0.1"/>
-      <param name="RGBD/LinearUpdate"         type="string" value="0.1"/> 
+      <param name="RGBD/LinearUpdate"         type="string" value="0.1"/>
      <param name="RGBD/LocalRadius"          type="string" value="5"/>
-	  
+
      <param name="Mem/RehearsalSimilarity"   type="string" value="0.45"/>
      <param name="Mem/NotLinkedNodesKept"    type="string" value="false"/>
      <param name="Mem/ImageDecimation"       type="string" value="1"/>
-      
+
      <param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="false"/>
      <param name="Rtabmap/TimeThr"           type="string" value="600"/>
      <param name="Rtabmap/DetectionRate"     type="string" value="1"/>
@@ -64,7 +64,7 @@
      <param name="Optimizer/Slam2D"           type="string" value="true"/>
      <param name="Optimizer/Iterations"      type="string" value="100"/>
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
-      <param name="Optimizer/Strategy"        type="string" value="1"/>      
+      <param name="Optimizer/Strategy"        type="string" value="1"/>
      <param name="Optimizer/Robust"          type="string" value="false"/>
      <param name="Optimizer/VarianceIgnored" type="string" value="true"/>
      <param name="RGBD/PlanStuckIterations"  type="string" value="10"/>
@@ -73,14 +73,14 @@
      <param name="Kp/MaxFeatures"            type="string" value="300"/>
      <param name="SURF/HessianThreshold"     type="string" value="500"/>
-      
+
      <!-- 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)"/> 
+      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
   </node>
  </group>
-  
+
  <!-- teleop -->
  <node name="joy" pkg="joy" type="joy_node"/>
  <group ns="teleop">
@@ -96,8 +96,8 @@
     <remap from="ground_cloud" to="/ground_cloud"/>
     <remap from="map" to="/map"/>
     <remap from="move_base_simple/goal" to="/planner_goal"/>
-     
+
-     <arg name="observation_sources" value="point_cloud_sensorA point_cloud_sensorB"/>   
+     <arg name="observation_sources" value="point_cloud_sensorA point_cloud_sensorB"/>
     <node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
        <param name="base_global_planner" value="navfn/NavfnROS"/>
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="global_costmap" />
@@ -108,7 +108,7 @@
        <param name="global_costmap/obstacle_layer/observation_sources" value="$(arg observation_sources)"/>
        <param name="local_costmap/obstacle_layer/observation_sources" value="$(arg observation_sources)"/>
     </node>
-   		
+
     <param name="cmd_vel/abtr_priority" value="10"/>
  </group>
@@ -121,17 +121,17 @@
        args="/cmd_eta /teleop/cmd_eta"/>
  <node name="register_cmd_vel" pkg="abtr_priority" type="register"
        args="/cmd_vel /planner/cmd_vel"/>
-  
+
  <!-- Throttling messages -->
  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
      <param name="rate" type="double" value="5"/>
      <param name="decimation" type="int" value="2"/>
-   
+
      <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
      <remap from="depth/image_in"     to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
-    
+
      <remap from="rgb/image_out"       to="data_resized_image"/>
      <remap from="depth/image_out"     to="data_resized_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_resized_camera_info"/>
@@ -146,17 +146,17 @@
      <param name="max_depth"  type="double" value="3.0"/>
      <param name="voxel_size" type="double" value="0.02"/>
    </node>
-  
+
    <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection camera_nodelet_manager">
      <remap from="cloud"     to="cloudXYZ"/>
      <remap from="obstacles" to="/obstacles_cloud"/>
      <remap from="ground"    to="/ground_cloud"/>
-      <param name="frame_id"             type="string" value="base_footprint"/>		
+      <param name="frame_id"             type="string" value="base_footprint"/>
      <param name="map_frame_id"         type="string" value="map"/>
      <param name="wait_for_transform"   type="bool" value="true"/>
      <param name="min_cluster_size"     type="int" value="20"/>
      <param name="max_obstacles_height" type="double" value="0.4"/>
-    </node>  
+    </node>
  </group>
 </launch>
@@ -15,5 +15,5 @@
  <!-- Xtion frame -->
  <node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
      args="0.057 0.087 0.185 0.0 0.0 0.0 /base_link /camera_link 100" />
-      
+
 </launch>
@@ -5,7 +5,7 @@
    <param name="settings_path" value="$(find rtabmap_ros)/launch/azimut3/config/azimut3_find_object.ini" type="str"/>
    <param name="subscribe_depth" value="true" type="bool"/>
    <param name="objects_path" value="$(find rtabmap_ros)/launch/data/books" type="str"/>
-		
+
    <remap from="rgb/image_rect_color" to="camera/data_throttled_image_relay"/>
    <remap from="depth_registered/image_raw" to="camera/data_throttled_image_depth_relay"/>
    <remap from="depth_registered/camera_info" to="camera/data_throttled_camera_info_relay"/>
@@ -5,7 +5,7 @@
  <!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
  <include file="$(find az3_bringup)/az3_standalone.launch"/>
  <!-- <include file="$(find az3_bringup)/joystick.launch"/> -->
-  
+
  <!-- OpenNI -->
  <include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
@@ -13,16 +13,16 @@
  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
      <param name="rate" type="double" value="5.0"/>
-   
+
      <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
      <remap from="depth/image_in"     to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
-    
+
      <remap from="rgb/image_out"       to="data_throttled_image"/>
      <remap from="depth/image_out"     to="data_throttled_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
-    </node>  
+    </node>
-  </group>  
+  </group>
  <!-- SLAM is done on client side...-->
 </launch>
@@ -3,8 +3,8 @@
 <launch>
  <!-- Remote teleop -->
-  <include file="$(find az3_bringup)/joystick.launch"/> 
+  <include file="$(find az3_bringup)/joystick.launch"/>
-  
+
  <!-- Visualization and SLAM nodes use same data, so just subscribe once and relay messages -->
  <node name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData /rtabmap/mapData_relay"/>
  <node name="odom_relay" type="relay" pkg="topic_tools" args="/base_controller/odom /base_controller/odom_relay"/>
@@ -12,25 +12,25 @@
  <node name="camera_info_relay" type="relay" pkg="topic_tools" args="/camera/data_throttled_camera_info /camera/data_throttled_camera_info_relay"/>
  <node name="republish_rgb" type="republish" pkg="image_transport" args="theora in:=/camera/data_throttled_image raw out:=/camera/data_throttled_image_relay" />
  <node name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=/camera/data_throttled_image_depth raw out:=/camera/data_throttled_image_depth_relay" />
-  
+
  <!-- SLAM client side -->
  <!-- args: "delete_db_on_start" and "udebug" -->
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
 	  <param name="frame_id" type="string" value="base_footprint"/>
-	
+
 	  <param name="subscribe_depth" type="bool" value="true"/>
 	  <param name="subscribe_laserScan" type="bool" value="true"/>
-	
+
 	  <remap from="odom" to="/base_controller/odom_relay"/>
 	  <remap from="scan" to="/base_scan_relay"/>
 	  <remap from="rgb/image" to="/camera/data_throttled_image_relay"/>
  	  <remap from="depth/image" to="/camera/data_throttled_image_depth_relay"/>
  	  <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info_relay"/>
-	
+
 	  <param name="queue_size" type="int" value="10"/>
-	
+
 	  <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
 	  <param name="RGBD/ScanMatchingSize" type="string" value="1"/>             <!-- Do odometry correction with consecutive laser scans -->
 	  <param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>   <!-- Local loop closure detection (using estimated position) with locations in WM -->
@@ -46,15 +46,15 @@
 	  <param name="RGBD/LinearUpdate" type="string" value="0.01"/>              <!-- Update map only if the robot is moving -->
 	  <param name="Rtabmap/TimeThr" type="string" value="700"/>
 	  <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
-	  <param name="Mem/RehearsedNodesKept" type="string" value="false"/>   
+	  <param name="Mem/RehearsedNodesKept" type="string" value="false"/>
    </node>
-    
+
    <!-- Grid map assembler for rviz -->
    <node pkg="rtabmap_ros" type="grid_map_assembler" name="grid_map_assembler" output="screen"/>
  </group>
-   
+
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/azimut3/config/azimut3.rviz"/>
-  
+
  <!-- Below, construct point cloud of the latest throttled data -->
  <node pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
@@ -62,7 +62,7 @@
    <remap from="depth/image"     to="/camera/data_throttled_image_depth_relay"/>
    <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info_relay"/>
    <remap from="cloud"           to="voxel_cloud" />
-    
+
    <param name="queue_size" type="int" value="10"/>
    <param name="voxel_size" type="double" value="0.01"/>
  </node>
@@ -5,7 +5,7 @@ TrajectoryPlannerROS:
  acc_lim_y:  0.75
  acc_lim_theta: 4
  # min_vel_x and max_rotational_vel were set to keep the ICR at
-  # minimal distance of 0.48 m. 
+  # minimal distance of 0.48 m.
  # Basically, max_rotational_vel * rho_min <= min_vel_x
  max_vel_x:  0.5
  min_vel_x:  0.24
@@ -17,7 +17,7 @@ TrajectoryPlannerROS:
  xy_goal_tolerance:  0.25
  yaw_goal_tolerance: 0.25
  latch_xy_goal_tolerance: true
-  
+
  # make sure that the minimum velocity multiplied by the sim_period is less than twice the tolerance on a goal. Otherwise, the robot will prefer to rotate in place just outside of range of its target position rather than moving towards the goal.
  sim_time: 1.5 # set between 1 and 2. The higher he value, the smoother the path (though more samples would be required).
  sim_granularity: 0.025
@@ -35,7 +35,7 @@ TrajectoryPlannerROS:
 #move_base
 controller_frequency: 10.0 #The robot can move faster when higher.
-#global planner 
+#global planner
 NavfnROS:
    allow_unknown: true
    visualize_potential: false
@@ -18,23 +18,23 @@ obstacle_layer:
  raytrace_range: 3.0
  max_obstacle_height: 0.4
  track_unknown_space: true
- 
+
  observation_sources: laser_scan_sensor point_cloud_sensorA point_cloud_sensorB
  laser_scan_sensor: {
-    data_type: LaserScan, 
+    data_type: LaserScan,
-    topic: base_scan, 
+    topic: base_scan,
-    expected_update_rate: 0.2, 
+    expected_update_rate: 0.2,
-    marking: true, 
+    marking: true,
    clearing: true
  }
  point_cloud_sensorA: {
    sensor_frame: base_footprint,
-    data_type: PointCloud2, 
+    data_type: PointCloud2,
-    topic: obstacles_cloud, 
+    topic: obstacles_cloud,
-    expected_update_rate: 0.5, 
+    expected_update_rate: 0.5,
-    marking: true, 
+    marking: true,
    clearing: true,
    min_obstacle_height: 0.04
  }
@@ -15,19 +15,19 @@ local_costmap:
  observation_sources: point_cloud_sensor
  laser_scan_sensor: {
-    data_type: LaserScan, 
+    data_type: LaserScan,
-    topic: base_scan, 
+    topic: base_scan,
-    expected_update_rate: 0.2, 
+    expected_update_rate: 0.2,
-    marking: true, 
+    marking: true,
    clearing: true}
  # assuming receiving a cloud from rtabmap/obstacles_detection node
  point_cloud_sensor: {
    sensor_frame: base_footprint,
-    data_type: PointCloud2, 
+    data_type: PointCloud2,
-    topic: openni_points, 
+    topic: openni_points,
-    expected_update_rate: 0.5, 
+    expected_update_rate: 0.5,
-    marking: true, 
+    marking: true,
    clearing: true,
    min_obstacle_height: -99999.0,
    max_obstacle_height: 0.5}
@@ -7,6 +7,6 @@ The number is the Kinect serial shown when launching kinect2_brige:
  [Freenect2Impl] 12 usb devices connected
  [Freenect2Impl] found valid Kinect v2 @4:3 with serial 506816242542
  [Freenect2Impl] found 1 devices
-  Kinect2 devices found: 
+  Kinect2 devices found:
    0: 506816242542 (selected)
  ...
@@ -7,25 +7,25 @@ Hannah Grace Mason, Joe Macinnes, Landon Chase Bentley
 Copyright (c) 2015-2018 Arizona Board of Regents
 All rights reserved.
-Permission is hereby granted, without written agreement and without 
+Permission is hereby granted, without written agreement and without
 license or royalty fees, to use, copy, modify, and distribute this
-software and its documentation for any purpose, provided that the 
+software and its documentation for any purpose, provided that the
-above copyright notice and the following two paragraphs appear in 
+above copyright notice and the following two paragraphs appear in
 all copies of this software.
-IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY 
+IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
-FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES 
+FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
-ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN 
+ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
-IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF 
+IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
 SUCH DAMAGE.
-THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, 
+THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
-INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY 
+INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
 AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
 IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
 TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
-Summary: 
+Summary:
 This file includes gazebo reference for front stereo camera simulation mounted
 on the top of the car. For more information and for the tutorials used to create
 this file, see
@@ -5,25 +5,25 @@ Author: Jonathan Sprinkle, Sam Taylor, Alex Warren
 Copyright (c) 2015 Arizona Board of Regents
 All rights reserved.
-Permission is hereby granted, without written agreement and without 
+Permission is hereby granted, without written agreement and without
 license or royalty fees, to use, copy, modify, and distribute this
-software and its documentation for any purpose, provided that the 
+software and its documentation for any purpose, provided that the
-above copyright notice and the following two paragraphs appear in 
+above copyright notice and the following two paragraphs appear in
 all copies of this software.
-IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY 
+IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
-FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES 
+FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
-ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN 
+ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
-IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF 
+IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
 SUCH DAMAGE.
-THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, 
+THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
-INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY 
+INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
 AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
 IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
 TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
-Summary: 
+Summary:
 This file includes the control interfaces for ROS-based control
 through Gazebo. For more information and for the tutorials used to create
 this file, see
@@ -5,5 +5,5 @@
  <xacro:VLP-16 parent="base_laser_mount" name="velodyne" topic="velodyne_points" gpu="true" organize_cloud="true">
    <origin xyz="0.025 0 0.175" rpy="0 0 0" />
  </xacro:VLP-16>
-  
+
 </robot>
@@ -10,17 +10,17 @@
  <arg name="subscribe_scan_cloud" default="false"/>
  <arg     if="$(arg subscribe_stereo)" name="approx_sync"  default="false"/>
  <arg unless="$(arg subscribe_stereo)" name="approx_sync"  default="true"/>
-  
+
  <arg name="frame_id"      default="camera_link"/>
  <arg name="odom_frame_id" default=""/>        <!-- use topic when not set, otherwise use TF if set -->
  <arg name="ground_truth_frame_id"   default=""/>     <!-- e.g., "world" -->
  <arg name="ground_truth_base_frame_id" default=""/>  <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
-  
+
  <arg name="output_path"   default="output.db"/>
  <arg name="record_in_RAM" default="false"/>
  <arg name="queue_size"    default="10"/>
-  <arg name="max_rate"      default="0"/>       <!-- Record as fast as possible --> 
+  <arg name="max_rate"      default="0"/>       <!-- Record as fast as possible -->
-  
+
  <arg name="rgb_topic"        default="camera/rgb/image_rect_color"/>
  <arg name="rgb_info_topic"   default="camera/rgb/camera_info"/>
  <arg name="depth_topic"      default="camera/depth_registered/image_raw"/>
@@ -32,27 +32,27 @@
  <arg name="odom_topic"       default="odom"/>
  <arg name="scan_topic"       default="scan"/>
  <arg name="scan_cloud_topic" default="scan_cloud"/>
-  
+
  <arg name="rgb_image_transport"  default="raw"/>
  <arg name="depth_image_transport"  default="raw"/>
  <node name="data_recorder" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
-  
+
    <!-- Disable any processing -->
 	<param name="Mem/RehearsalSimilarity" type="string" value="1.0"/>    <!-- deactivate rehearsal -->
 	<param name="Kp/MaxFeatures"          type="string" value="-1"/>     <!-- deactivate keypoints extraction -->
 	<param name="Rtabmap/MaxRetrieved"    type="string" value="0"/>      <!-- deactivate global retrieval -->
 	<param name="RGBD/MaxLocalRetrieved"  type="string" value="0"/>      <!-- deactivate local retrieval -->
-	<param name="Mem/MapLabelsAdded"      type="string" value="false"/>  <!-- don't create map labels --> 
+	<param name="Mem/MapLabelsAdded"      type="string" value="false"/>  <!-- don't create map labels -->
-	<param name="Rtabmap/MemoryThr"       type="string" value="2"/>      <!-- keep the WM empty --> 
+	<param name="Rtabmap/MemoryThr"       type="string" value="2"/>      <!-- keep the WM empty -->
-	<param name="Mem/STMSize"             type="string" value="1"/>      <!-- STM=1 --> 
+	<param name="Mem/STMSize"             type="string" value="1"/>      <!-- STM=1 -->
 	<param name="publish_tf"              type="bool"   value="false"/>  <!-- don't publish TF -->
-	<param name="RGBD/ProximityBySpace"   type="string" value="false"/> 
+	<param name="RGBD/ProximityBySpace"   type="string" value="false"/>
-	<param name="RGBD/LinearUpdate"       type="string" value="0"/> 
+	<param name="RGBD/LinearUpdate"       type="string" value="0"/>
 	<param name="RGBD/AngularUpdate"      type="string" value="0"/>
-	<param name="RGBD/CreateOccupancyGrid" type="string" value="false"/> 
+	<param name="RGBD/CreateOccupancyGrid" type="string" value="false"/>
 	<param unless="$(arg subscribe_odometry)" name="RGBD/Enabled" type="string" value="false"/>
-	
+
 	<param name="Rtabmap/DetectionRate"   type="string" value="$(arg max_rate)"/>
 	<param name="DbSqlite3/InMemory"  type="string" value="$(arg record_in_RAM)"/>
 	<param name="database_path"       type="string" value="$(arg output_path)"/>
@@ -65,16 +65,16 @@
 	<param name="subscribe_rgbd"      type="bool"   value="$(arg subscribe_rgbd)"/>
 	<param name="queue_size"          type="int"    value="$(arg queue_size)"/>
 	<param name="approx_sync"         type="bool"   value="$(arg approx_sync)"/>
-	
+
 	<!-- Hack to use a fake odom_frame_id=frame_id if subscribe_odometry = false -->
 	<param if="$(arg subscribe_odometry)" name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
 	<param unless="$(arg subscribe_odometry)" name="odom_frame_id" type="string" value="$(arg frame_id)"/>
-	
+
 	<param name="rgb/image_transport" type="string" value="$(arg rgb_image_transport)"/>
    <param name="depth/image_transport" type="string" value="$(arg depth_image_transport)"/>
    <param name="left/image_transport" type="string" value="$(arg rgb_image_transport)"/>
    <param name="right/image_transport" type="string" value="$(arg rgb_image_transport)"/>
-  
+
 	<remap from="rgb/image"         to="$(arg rgb_topic)"/>
 	<remap from="rgb/camera_info"   to="$(arg rgb_info_topic)"/>
  	<remap from="depth/image"       to="$(arg depth_topic)"/>
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <launch>
-	
+
 	<!-- APPEARANCE-BASED LOOP CLOSURE DETECTION VERSION -->
 	<!-- WARNING : Database is automatically deleted on each startup -->
 	<!--           See "delete_db_on_start" option below... -->
@@ -13,14 +13,14 @@
  <group ns="rtabmap">
 	<!-- args: "delete_db_on_start" and "udebug" -->
 	<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
-	
+
 	    <param name="subscribe_rgb"       type="bool" value="false"/>    <!-- must be false for appearance-based mode -->
 	    <param name="subscribe_depth"     type="bool" value="false"/>    <!-- must be false for appearance-based mode -->
        <param name="subscribe_odom"      type="bool" value="false"/>    <!-- must be false for appearance-based mode -->
        <param name="queue_size"          type="int" value="10"/>
-    
+
        <remap from="rgb/image" to="/image"/>                             <!-- connect to "image" topic of the camera below -->
-      
+
 		<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
 	    <param name="RGBD/Enabled"                   type="string" value="false"/>  <!-- False: appearance-based -->
 		<param name="Rtabmap/ImageBufferSize"        type="string" value="0"/>      <!-- process all images -->
@@ -32,28 +32,28 @@
 		<param name="Mem/UseOdomFeatures"            type="string" value="false"/>
 		<param name="Kp/DetectorStrategy"            type="string" value="0"/>      <!-- use SURF -->
 		<param name="SURF/HessianThreshold"          type="string" value="100"/>
-		
+
 		<!-- 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)"/>
 	</node>
-	
+
 	<!-- visualization of the "infoEx" topic sent by rtabmap node -->
 	<node name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen" args="-d $(find rtabmap_ros)/launch/config/appearance_gui.ini">
 	    <param name="subscribe_odom"      type="bool" value="false"/>    <!-- must be false for appearance-based mode -->
-	    
+
 	    <!-- This enables the GUI to pause a rtabmap_ros/camera when action "pause" is checked. -->
 	    <!-- NOTE: It is specific to rtabmap_ros/camera. Action "pause" in the GUI will still pause the rtabmap node. -->
-		<param name="camera_node_name" type="string" value="/camera"/>      
+		<param name="camera_node_name" type="string" value="/camera"/>
-		 
+
 	</node>
  </group>
-  
+
 	<!-- When parameter video_or_images_path is set, the camera uses the directory of images or the video file -->
 	<node name="camera" pkg="rtabmap_ros" type="camera" output="screen">
 		<remap from="image" to="image"/>
-		
+
 		<param name="device_id"            value="0" type="int"/>
 		<param name="video_or_images_path" value="$(find rtabmap_ros)/launch/data/demo_appearance" type="string"/>
 		<param name="frame_rate"           value="2.0" type="double"/>
@@ -4,10 +4,10 @@
    <!-- 1) Make sure rtabmap is built with libpointmatcher for lidar:=true option (lidar SLAM)
         2) Install https://github.com/jmscslgroup/catvehicle
-            We need a stereo camera and velodyne point cloud looking like the real 
+            We need a stereo camera and velodyne point cloud looking like the real
-            sensor (ring-like pattern), so we have to modifiy the default catvehicle 
+            sensor (ring-like pattern), so we have to modifiy the default catvehicle
-            urdf. First we have to install velodyne_simulator package to have the 
+            urdf. First we have to install velodyne_simulator package to have the
-            right gazebo plugin and copy this modified velodyne config: 
+            right gazebo plugin and copy this modified velodyne config:
               * sudo apt install ros-$ROS_DISTRO-velodyne-simulator
               * cp ~/catkin_ws/src/rtabmap_ros/launch/config/catvehicle_velodyne_points.gazebo ~/catkin_ws/src/catvehicle/urdf/.
            Secondly, we have to modify the triclops config to make it stereo:
@@ -19,14 +19,14 @@
         4) roslaunch catvehicle catvehicle_spawn.launch velodyne_points:=true triclops:=true Z:=5
         5) roslaunch catvehicle joystick.launch
         6) SLAM, 3 choices:
-            A) LiDAR-SLAM + Visual loop closure detection: 
+            A) LiDAR-SLAM + Visual loop closure detection:
              roslaunch rtabmap_ros demo_catvehicle_mapping.launch
-            B) LiDAR-SLAM without camera: 
+            B) LiDAR-SLAM without camera:
              roslaunch rtabmap_ros demo_catvehicle_mapping.launch camera:=false
-            C) Visual-SLAM without lidar: 
+            C) Visual-SLAM without lidar:
              roslaunch rtabmap_ros demo_catvehicle_mapping.launch lidar:=false
-         Note: Gazebo real-time factor should be equal or below 1. If it is over 1, 
+         Note: Gazebo real-time factor should be equal or below 1. If it is over 1,
               in gazebo client, select Physics, then change real time update rate to 500.
    -->
    <arg name="camera"     default="true" />
@@ -35,7 +35,7 @@
    <arg name="rtabmapviz" default="true" />
    <arg name="rviz"       default="true" />
    <arg name="light"      default="false" /> <!-- Don't record all scans if false -->
-    <arg name="altitude"   default="0" /> <!-- assemble occupancy grids by altitude (radius in meters, 0=disabled) --> 
+    <arg name="altitude"   default="0" /> <!-- assemble occupancy grids by altitude (radius in meters, 0=disabled) -->
    <arg name="localization"   default="false" />
    <!-- Common parameters -->
@@ -78,7 +78,7 @@
        <!-- LiDAR SLAM parameters -->
        <arg     if="$(arg lidar)" name="args" value="$(arg clear_db) $(arg common_args)
                --Reg/Strategy 1"/>
-        
+
        <!-- Visual SLAM parameters -->
        <arg unless="$(arg lidar)" name="args" value="$(arg clear_db) $(arg common_args)
                --Reg/Strategy 0
@@ -86,7 +86,7 @@
                --Grid/MaxGroundHeight 0.7
                --Grid/NoiseFilteringRadius 0.5"/>
-        <arg name="localization" value="$(arg localization)"/>        
+        <arg name="localization" value="$(arg localization)"/>
        <arg name="subscribe_scan_cloud" value="$(arg lidar)"/>
        <arg name="stereo"            value="$(arg camera)"/>
        <arg unless="$(arg camera)" name="depth"          value="false"/>
@@ -96,13 +96,13 @@
        <arg     if="$(arg lidar)" name="odom_topic" value="lidar_odom"/>
        <arg unless="$(arg lidar)" name="odom_topic" value="visual_odom"/>
-        
+
        <arg name="odom_guess_frame_id" value="catvehicle/odom"/>
        <arg name="frame_id"            value="catvehicle/base_link"/>
        <arg name="map_frame_id"        value="catvehicle/map"/>
        <arg     if="$(arg lidar)" name="vo_frame_id" value="catvehicle/lidar_odom"/>
        <arg unless="$(arg lidar)" name="vo_frame_id" value="catvehicle/visual_odom"/>
-        
+
        <arg name="stereo_namespace"  value="/catvehicle/triclops"/>
        <arg name="right_image_topic" value="/catvehicle/triclops/right/image_rect_color"/>
        <arg name="scan_cloud_topic"  value="/catvehicle/velodyne_points"/>
@@ -122,4 +122,4 @@
    </include>
    <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/catvehicle.rviz"/>
-</launch> 
+</launch>
@@ -6,26 +6,26 @@
  <!-- This demo works with demo_mapping.bag -->
  <param name="use_sim_time" type="bool" value="True"/>
-  
+
  <include file="$(find rtabmap_ros)/launch/data_recorder.launch">
    <arg name="subscribe_odometry"   value="true"/>
    <arg name="subscribe_depth"      value="true"/>
    <arg name="subscribe_stereo"     value="false"/>
    <arg name="subscribe_scan"       value="true"/>
-  
+
    <arg name="frame_id"      value="base_footprint"/>
    <arg name="odom_frame_id" value=""/>        <!-- use topic when not set, otherwise use TF if set -->
-  
+
    <arg name="output_path"   value="output.db"/>
    <arg name="record_in_RAM" value="false"/>
    <arg name="queue_size"    value="10"/>
    <arg name="max_rate"      value="0"/>
-  
+
    <arg name="odom_topic"    value="/az3/base_controller/odom"/>
    <arg name="scan_topic"    value="/jn0/base_scan"/>
  </include>
-  
+
-  <!-- uncompress image data --> 
+  <!-- uncompress image data -->
  <node name="cameraInfo_relay"  type="relay"     pkg="topic_tools"     args="/data_throttled_camera_info /camera/rgb/camera_info"/>
  <node name="republish_rgb"     type="republish" pkg="image_transport" args="compressed      in:=/data_throttled_image       raw out:=/camera/rgb/image_rect_color" />
  <node name="republish_depth"   type="republish" pkg="image_transport" args="compressedDepth in:=/data_throttled_image_depth raw out:=/camera/depth_registered/image_raw" />
@@ -4,7 +4,7 @@
  <!-- Choose visualization -->
  <arg name="rviz" default="true" />
-  <arg name="rtabmapviz" default="false" /> 
+  <arg name="rtabmapviz" default="false" />
  <arg name="save_objects" default="false"/>
  <arg name="localization" default="false"/>
  <arg name="save_objects_as_landmarks" default="false"/> <!-- apriltag_ros package should be installed and rtabmap_ros built with it -->
@@ -12,16 +12,16 @@
  <arg unless="$(arg localization)" name="rtabmap_args" default="--delete_db_on_start"/>
  <param name="use_sim_time" type="bool" value="True"/>
-  
+
  <!-- SLAM (robot side) -->
  <!-- args: "delete_db_on_start" and "udebug" -->
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
 	  <param name="frame_id" type="string" value="base_footprint"/>
-	
+
 	  <param name="subscribe_depth" type="bool" value="true"/>
 	  <param name="subscribe_scan" type="bool" value="true"/>
-	
+
 	  <remap from="odom" to="/base_controller/odom"/>
 	  <remap from="scan" to="/base_scan"/>
 	  <remap if="$(arg save_objects)" from="user_data_async" to="/objectsData"/>
@@ -29,14 +29,14 @@
 	  <remap from="rgb/image"       to="/camera/data_throttled_image"/>
  	  <remap from="depth/image"     to="/camera/data_throttled_image_depth"/>
  	  <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
-  	  
+
  	  <param name="rgb/image_transport"   type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/>
-	
+
 	  <param name="queue_size" type="int" value="10"/>
 	  <param name="landmark_linear_variance" type="double" value="0.1"/>
 	  <param name="landmark_angular_variance" type="double" value="0.5"/>
-	
+
 	  <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
 	  <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>  <!-- Do odometry correction with consecutive laser scans -->
 	  <param name="RGBD/ProximityBySpace"     type="string" value="true"/>  <!-- Proximity detection (using estimated position) with locations in WM -->
@@ -54,45 +54,45 @@
 	  <param name="Mem/RehearsalSimilarity"   type="string" value="0.45"/>
 	  <param name="Mem/NotLinkedNodesKept"    type="string" value="false"/>
 	  <param name="Optimizer/Slam2D"          type="string" value="true"/>
-	  <param name="Reg/Force3DoF"             type="string" value="true"/>   
+	  <param name="Reg/Force3DoF"             type="string" value="true"/>
 	  <param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
    </node>
-    
+
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
  	  <param name="subscribe_depth" type="bool" value="true"/>
      <param name="subscribe_scan"  type="bool" value="true"/>
      <param name="frame_id"        type="string" value="base_footprint"/>
-    
+
      <remap from="rgb/image"       to="/camera/data_throttled_image"/>
      <remap from="depth/image"     to="/camera/data_throttled_image_depth"/>
      <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
      <remap from="scan"            to="/base_scan"/>
      <remap from="odom"            to="/base_controller/odom"/>
-      
+
      <param name="rgb/image_transport"   type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/>
    </node>
-    
+
-  </group> 
+  </group>
-  
+
  <!-- send AZIMUT 3 urdf to param server -->
-  <!-- 
+  <!--
  <param name="robot_description" command="$(find xacro)/xacro.py '$(find az3_description)/robots/azimut_3_laser.urdf.xacro'" />
  -->
-  
+
  <!-- Visualisation -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_find_object.rviz" output="screen"/>
-  
+
  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
    <remap from="rgb/image"       to="/camera/data_throttled_image"/>
    <remap from="depth/image"     to="/camera/data_throttled_image_depth"/>
    <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
    <remap from="cloud"           to="voxel_cloud" />
-    
+
    <param name="rgb/image_transport"   type="string" value="compressed"/>
    <param name="depth/image_transport" type="string" value="compressedDepth"/>
-    
+
    <param name="queue_size" type="int" value="10"/>
    <param name="voxel_size" type="double" value="0.01"/>
  </node>
@@ -103,7 +103,7 @@
    <param name="settings_path"   value="$(find rtabmap_ros)/launch/config/find_object.ini" type="str"/>
    <param name="subscribe_depth" value="true" type="bool"/>
    <param name="objects_path"    value="$(find rtabmap_ros)/launch/data/books" type="str"/>
-		
+
    <remap from="rgb/image_rect_color"         to="/camera/data_throttled_image"/>
    <remap from="depth_registered/image_raw"   to="/camera/data_throttled_image_depth"/>
    <remap from="depth_registered/camera_info" to="/camera/data_throttled_camera_info"/>
@@ -111,13 +111,13 @@
    <param name="rgb/image_transport"              type="string" value="compressed"/>
    <param name="depth_registered/image_transport" type="string" value="compressedDepth"/>
  </node>
-  
+
  <!-- Save objects to database example -->
  <node if="$(arg save_objects)" name="save_objects_example" pkg="rtabmap_ros" type="save_objects_example" output="screen">
    <remap from="mapData" to="/rtabmap/mapData"/>
    <param name="frame_id" value="base_footprint"/>
  </node>
-  
+
  <!-- Convert objects to tags -->
  <node if="$(arg save_objects_as_landmarks)" name="objects_to_tags" pkg="rtabmap_ros" type="objects_to_tags.py" output="screen">
    <remap from="tag_detections" to="/rtabmap/tag_detections"/>
@@ -7,11 +7,11 @@
  <!-- rosbag filter demo_mapping.bag demo_mapping_no_odom.bag 'topic != "/tf" or topic == "/tf" and m.transforms[0].header.frame_id != "/odom"' -->
  <!-- WARNING : Database is automatically deleted on each startup                       -->
  <!--           See "delete_db_on_start" option below...                                -->
-  
+
  <!-- Choose visualization -->
  <arg name="rviz" default="true" />
  <arg name="rtabmapviz" default="false" />
-  
+
  <!-- Choose hector_slam or icp_odometry for odometry -->
  <arg name="hector" default="true" />
@@ -22,19 +22,19 @@
  <!-- Example with camera or not -->
  <arg name="camera" default="true" />
-  
+
  <!-- Limit lidar range if > 0 (has effect only when hector:=false) -->
  <arg name="max_range" default="0" />
-  
+
  <!-- Point to Plane ICP? (has effect only when hector:=false) -->
  <arg name="p2n" default="true" />
-  
+
  <!-- Use libpointmatcher for ICP? (has effect only when hector:=false) -->
  <arg name="pm" default="true" />
  <param name="use_sim_time" type="bool" value="True"/>
-  
+
-  <node if="$(arg hector)" pkg="tf" type="static_transform_publisher" name="scanmatcher_to_base_footprint" 
+  <node if="$(arg hector)" pkg="tf" type="static_transform_publisher" name="scanmatcher_to_base_footprint"
    args="0.0 0.0 0.0 0.0 0.0 0.0 /scanmatcher_frame /base_footprint 100" />
  <!-- Odometry from laser scans -->
@@ -45,36 +45,36 @@
    <param name="map_frame" value="hector_map" />
    <param name="base_frame" value="base_footprint" />
    <param name="odom_frame" value="odom" />
-    
+
    <!-- Tf use -->
    <param name="pub_map_odom_transform" value="false"/>
    <param name="pub_map_scanmatch_transform" value="true"/>
    <param name="pub_odometry" value="true"/>
-    
+
    <!-- Map size / start point -->
    <param name="map_resolution" value="0.050"/>
    <param name="map_size" value="2048"/>
    <param name="map_multi_res_levels" value="2" />
-    
+
    <!-- Map update parameters -->
    <param name="map_update_angle_thresh" value="0.06" />
-    
+
-    <!-- Advertising config --> 
+    <!-- Advertising config -->
    <param name="scan_topic" value="/jn0/base_scan"/>
  </node>
-  
+
  <!-- If argument "hector" is false, we use rtabmap's icp odometry to generate odometry for us -->
  <node unless="$(arg hector)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen" >
     <remap from="scan"      to="/jn0/base_scan"/>
     <remap from="odom"      to="/scanmatch_odom"/>
     <remap from="odom_info"      to="/rtabmap/odom_info"/>
-	  
+
     <param name="frame_id"        type="string" value="base_footprint"/>
     <param name="deskewing"       type="string" value="true"/>
     <param if="$(arg odom_guess)" name="odom_frame_id"   type="string" value="icp_odom"/>
     <param if="$(arg odom_guess)" name="guess_frame_id"  type="string" value="odom"/>
-     
+
     <param name="Icp/VoxelSize"     type="string" value="0.05"/>
     <param name="Icp/RangeMax"      type="string" value="$(arg max_range)"/>
     <param name="Icp/Epsilon"       type="string" value="0.001"/>
@@ -107,12 +107,12 @@
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="frame_id" type="string" value="base_footprint"/>
-	
+
      <param name="subscribe_rgb"   type="bool" value="false"/>
      <param name="subscribe_depth" type="bool" value="false"/>
      <param name="subscribe_rgbd"  type="bool" value="$(arg camera)"/>
      <param name="subscribe_scan"  type="bool" value="true"/>
-	
+
      <remap from="scan" to="/jn0/base_scan"/>
      <!-- As hector doesn't provide compatible covariance in the odometry topic, don't use the topic and fix the covariance -->
@@ -122,7 +122,7 @@
      <remap unless="$(arg hector)" from="odom" to="/scanmatch_odom"/>
      <param unless="$(arg hector)" name="subscribe_odom_info" type="bool" value="true"/>
-	
+
      <!-- RTAB-Map's parameters -->
      <param name="Reg/Strategy"       type="string" value="1"/>    <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
      <param name="Reg/Force3DoF"      type="string" value="true"/>
@@ -132,13 +132,13 @@
      <param name="Icp/RangeMax"             type="string" value="$(arg max_range)"/>
      <param name="Grid/RangeMax"            type="string" value="$(arg max_range)"/>
    </node>
-    
+
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
      <param name="subscribe_rgbd"      type="bool" value="$(arg camera)"/>
      <param name="subscribe_laserScan" type="bool" value="true"/>
      <param name="frame_id"            type="string" value="base_footprint"/>
-    
+
      <remap from="scan"            to="/jn0/base_scan"/>
      <!-- As hector doesn't provide compatible covariance in the odometry topic -->
@@ -147,9 +147,9 @@
      <remap unless="$(arg hector)" from="odom" to="/scanmatch_odom"/>
      <param unless="$(arg hector)" name="subscribe_odom_info" type="bool" value="true"/>
    </node>
-  
+
  </group>
-  
+
  <!-- Visualisation RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz" output="screen"/>
   <node if="$(arg camera)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
@@ -1,52 +1,52 @@
 <?xml version="1.0"?>
 <!-- -->
 <launch>
-   
+
  <!-- Bringup the Husky with SICK (2D LiDAR), realsense camera (RGB-D camera) and velodyne (3D LiDAR):
       $ export HUSKY_URDF_EXTRAS=$(rospack find rtabmap_ros)/launch/config/husky_velodyne_extra.urdf.xacro
       $ roslaunch husky_gazebo husky_playpen.launch realsense_enabled:=true
-       $ roslaunch husky_viz view_robot.launch 
+       $ roslaunch husky_viz view_robot.launch
       For ICP odometry examples, rtabmap should be built with libpointmatcher.
-       
+
       Examples:
         1) 6DoF mapping with 3D LiDAR
            $ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=false
-         
+
         2) 6DoF mapping with 3D LiDAR and RGB-D camera
            $ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=false camera:=true
-         
+
         3) 6DoF mapping with 3D LiDAR and RGB-D camera and ICP odometry (with wheel odometry as guess)
            $ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=false camera:=true icp_odometry:=true
-         
+
         4) 3DoF mapping with 3D LiDAR
            $ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true
-         
+
         5) 3DoF mapping with 3D LiDAR and RGB-D camera
            $ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true camera:=true
-            
+
         6) 3DoF mapping with 3D LiDAR and RGB-D camera and ICP odometry (with wheel odometry as guess)
            $ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true camera:=true icp_odometry:=true
-            
+
         7) 3DoF mapping with 2D LiDAR
            $ roslaunch rtabmap_ros demo_husky.launch lidar2d:=true slam2d:=true
-         
+
         8) 3DoF mapping with 2D LiDAR and RGB-D camera
            $ roslaunch rtabmap_ros demo_husky.launch lidar2d:=true slam2d:=true camera:=true
-            
+
         9) 3DoF mapping with 2D LiDAR and RGB-D camera and ICP odometry (with wheel odometry as guess)
            $ roslaunch rtabmap_ros demo_husky.launch lidar2d:=true slam2d:=true camera:=true icp_odometry:=true
-            
+
         10) 6DoF mapping with 3D LiDAR and RGB camera (depth generated by lidar projection) and ICP odometry (with wheel odometry as guess)
            $ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=false camera:=true icp_odometry:=true depth_from_lidar:=true rtabmapviz:=true
-            
+
         11) 3DoF mapping with 3D LiDAR and RGB camera (depth generated by lidar projection) and ICP odometry (with wheel odometry as guess)
            $ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true camera:=true icp_odometry:=true depth_from_lidar:=true rtabmapviz:=true
-            
+
       Issues:
         When setting icp_odometry:=true with navigation, sending a goal to move_base could cause errors like:
-            "Extrapolation Error: Lookup would require extrapolation into the future.  Requested 
+            "Extrapolation Error: Lookup would require extrapolation into the future.  Requested
-             time 1340.520000000 but the latest data is at time 1340.500000000, when looking up 
+             time 1340.520000000 but the latest data is at time 1340.500000000, when looking up
             transform from frame [odom] to frame [map]"
         To fix this change "global_frame: odom" to "global_frame: icp_odom" in "husky_navigation/config/costmap_local.yaml"
       -->
@@ -61,36 +61,36 @@
  <arg name="lidar3d_ray_tracing" default="true"/>
  <arg name="slam2d"       default="true"/>
  <arg name="depth_from_lidar" default="false"/>
-  
+
-  
+
  <arg     if="$(arg lidar3d)" name="cell_size" default="0.3"/>
  <arg unless="$(arg lidar3d)" name="cell_size" default="0.05"/>
-  
+
  <arg if="$(arg lidar2d)" name="lidar_args" default="
-      --Reg/Strategy 1 
+      --Reg/Strategy 1
-      --RGBD/NeighborLinkRefining true 
+      --RGBD/NeighborLinkRefining true
-      --Grid/CellSize $(arg cell_size) 
+      --Grid/CellSize $(arg cell_size)
-      --Icp/PointToPlaneRadius 0 
+      --Icp/PointToPlaneRadius 0
      --Icp/MaxTranslation 1"/>
-  
+
  <arg if="$(arg lidar3d)" name="lidar_args" default="
-      --Reg/Strategy 1 
+      --Reg/Strategy 1
-      --RGBD/NeighborLinkRefining true 
+      --RGBD/NeighborLinkRefining true
-      --ICP/PM true 
+      --ICP/PM true
-      --Icp/PMOutlierRatio 0.7 
+      --Icp/PMOutlierRatio 0.7
-      --Icp/VoxelSize $(arg cell_size) 
+      --Icp/VoxelSize $(arg cell_size)
-      --Icp/MaxCorrespondenceDistance 1 
+      --Icp/MaxCorrespondenceDistance 1
-      --Icp/PointToPlaneGroundNormalsUp 0.9 
+      --Icp/PointToPlaneGroundNormalsUp 0.9
-      --Icp/Iterations 10 
+      --Icp/Iterations 10
-      --Icp/Epsilon 0.001 
+      --Icp/Epsilon 0.001
-      --OdomF2M/ScanSubtractRadius $(arg cell_size) 
+      --OdomF2M/ScanSubtractRadius $(arg cell_size)
-      --OdomF2M/ScanMaxSize 15000 
+      --OdomF2M/ScanMaxSize 15000
-      --Grid/ClusterRadius 1 
+      --Grid/ClusterRadius 1
-      --Grid/RangeMax 20 
+      --Grid/RangeMax 20
-      --Grid/RayTracing $(arg lidar3d_ray_tracing) 
+      --Grid/RayTracing $(arg lidar3d_ray_tracing)
-      --Grid/CellSize $(arg cell_size) 
+      --Grid/CellSize $(arg cell_size)
-      --Icp/PointToPlaneRadius 0 
+      --Icp/PointToPlaneRadius 0
-      --Icp/PointToPlaneNormalK 10 
+      --Icp/PointToPlaneNormalK 10
      --Icp/MaxTranslation 1"/>
  <!--- Run rtabmap -->
@@ -106,17 +106,17 @@
    <arg name="frame_id"        value="base_link" />
    <arg name="rtabmapviz"      value="$(arg rtabmapviz)" />
    <arg name="gps_topic"       value="/navsat/fix"/>
-    
+
    <!-- 2D LiDAR -->
    <arg name="subscribe_scan"  value="$(arg lidar2d)" />
    <arg     if="$(arg lidar2d)" name="scan_topic"  value="/scan" />
    <arg unless="$(arg lidar2d)" name="scan_topic"  value="/scan_not_used" />
-    
+
    <!-- 3D LiDAR -->
    <arg name="subscribe_scan_cloud"  value="$(arg lidar3d)" />
    <arg     if="$(arg lidar3d)" name="scan_cloud_topic" value="/velodyne_points" />
    <arg unless="$(arg lidar3d)" name="scan_cloud_topic" value="/scan_cloud_not_used" />
-    
+
    <!-- If camera is used -->
    <arg name="depth"             value="$(eval camera and not depth_from_lidar)" />
    <arg name="subscribe_rgb"     value="$(eval camera)" />
@@ -125,14 +125,14 @@
    <arg name="camera_info_topic" value="/realsense/color/camera_info" />
    <arg name="depth_topic"       value="/realsense/depth/image_rect_raw" />
    <arg name="approx_rgbd_sync"  value="false" />
-    
+
    <!-- If depth generated from lidar projection (in case we have only a single RGB camera with a 3D lidar) -->
    <arg name="gen_depth"                  value="$(arg depth_from_lidar)" />
    <arg name="gen_depth_decimation"       value="4" />
    <arg name="gen_depth_fill_holes_size"  value="3" />
    <arg name="gen_depth_fill_iterations"  value="1" />
    <arg name="gen_depth_fill_holes_error" value="0.3" />
-    
+
    <!-- If icp_odometry is used -->
    <arg if="$(arg icp_odometry)" name="icp_odometry" value="true" />
    <arg if="$(arg icp_odometry)" name="odom_guess_frame_id" value="odom" />
@@ -19,7 +19,7 @@
                                                                      -> verticalFov = 32
                                                                      -> verticalResolution = 2
                                                      -> ROS_lidar    -> pointCloudEnabled = True
-                                                      
+
       Isaac Sim 2022 (Action Graph):
         To enable all camera streams (right, depth left and depth right):
           1. Stage view -> World-> Carter_ROS, right-click on ROS_Cameras -> Open graph
@@ -75,23 +75,23 @@
        <arg name="use_sim_time"      value="true"/>
        <arg name="tag_linear_variance" value="0.1"/>
        <arg name="tag_angular_variance" value="0.2"/>
-        
+
        <arg name="scan_cloud_assembling"             value="$(arg lidar3d)"/>
        <arg name="scan_cloud_assembling_fixed_frame" value="odom"/>
        <arg name="scan_cloud_assembling_range_max"   value="60"/>
        <arg name="scan_cloud_assembling_voxel_size"  value="$(arg cell_size)"/>
    </include>
-    
+
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
        <param name="TrajectoryPlannerROS/max_vel_theta" value="0.3"/>
        <param name="TrajectoryPlannerROS/min_vel_theta" value="-0.3"/>
-        <rosparam file="$(find carter_2dnav)/params/costmap_common_params.yaml" command="load" ns="global_costmap" /> 
+        <rosparam file="$(find carter_2dnav)/params/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find carter_2dnav)/params/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find carter_2dnav)/params/local_costmap_params.yaml" command="load" />
-        <rosparam file="$(find carter_2dnav)/params/global_costmap_params.yaml" command="load" /> 
+        <rosparam file="$(find carter_2dnav)/params/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find carter_2dnav)/params/base_local_planner_params.yaml" command="load" />
    </node>
    <node type="rviz" name="rviz" pkg="rviz" args="-d $(find carter_2dnav)/rviz/carter_2dnav.rviz" />
-</launch> 
+</launch>
@@ -3,7 +3,7 @@
 <launch>
   <!-- MULTI-SESSION MAPPING VERSION -->
-   
+
   <!-- Choose visualization -->
  <arg name="rviz" default="false" />
  <arg name="rtabmapviz" default="true" />
@@ -12,32 +12,32 @@
  <param name="use_sim_time" type="bool" value="True"/>
  <group ns="rtabmap">
-  
+
  <!-- SLAM (robot side) -->
  <!-- args: "delete_db_on_start" and "udebug" -->
  <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
 	<param name="frame_id" type="string" value="base_footprint"/>
-	
+
 	<param name="subscribe_depth" type="bool" value="true"/>
 	<param name="subscribe_scan"  type="bool" value="true"/>
-	
+
 	<remap from="odom" to="/base_controller/odom"/>
 	<remap from="scan" to="/base_scan"/>
 	<remap from="rgb/image"       to="/data_throttled_image"/>
  	<remap from="depth/image"     to="/data_throttled_image_depth"/>
  	<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
-  	
+
  	<param name="rgb/image_transport"   type="string" value="compressed"/>
    <param name="depth/image_transport" type="string" value="compressedDepth"/>
-	
+
 	<param name="queue_size" type="int" value="10"/>
-	
+
 	<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
-	<param name="RGBD/NeighborLinkRefining"  type="string" value="false"/> 
+	<param name="RGBD/NeighborLinkRefining"  type="string" value="false"/>
 	<param name="RGBD/ProximityBySpace"      type="string" value="false"/> <!-- Referred paper did only global loop closure detection -->
-	<param name="RGBD/ProximityByTime"       type="string" value="false"/> 
+	<param name="RGBD/ProximityByTime"       type="string" value="false"/>
-	<param name="RGBD/OptimizeFromGraphEnd"  type="string" value="true"/> 
+	<param name="RGBD/OptimizeFromGraphEnd"  type="string" value="true"/>
 	<param name="Reg/Strategy"               type="string" value="1"/>
 	<param name="Icp/Iterations"             type="string" value="30"/>
 	<param name="Icp/VoxelSize"              type="string" value="0"/>
@@ -51,33 +51,33 @@
 	<param name="Bayes/FullPredictionUpdate" type="string" value="true"/>
 	<param name="Kp/DetectorStrategy"        type="string" value="0"/>   <!-- use SURF -->
 	<param name="Kp/MaxFeatures"             type="string" value="400"/>
-	<param name="Reg/Force3DoF"              type="string" value="true"/> 
+	<param name="Reg/Force3DoF"              type="string" value="true"/>
 	<param name="RGBD/OptimizeMaxError"      type="string" value="0.25"/>
 	<param name="Optimizer/Strategy"         type="string" value="0"/>  <!-- TORO is the most stable for multi-session mapping -->
-	<param name="Optimizer/Iterations"       type="string" value="100"/> 
+	<param name="Optimizer/Iterations"       type="string" value="100"/>
 	<param name="Kp/IncrementalFlann"        type="string" value="false"/>  <!-- Referred paper didn't use incremental FLANN -->
 	<param name="Grid/FromDepth"             type="string" value="false"/>
  </node>
-  
+
  <!-- Visualisation RTAB-Map -->
  <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
  	  <param name="subscribe_depth" type="bool" value="true"/>
      <param name="subscribe_scan"  type="bool" value="true"/>
      <param name="queue_size"      type="int" value="10"/>
      <param name="frame_id"        type="string" value="base_footprint"/>
-    
+
      <remap from="rgb/image"       to="/data_throttled_image"/>
      <remap from="depth/image"     to="/data_throttled_image_depth"/>
      <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
      <remap from="scan"            to="/base_scan"/>
      <remap from="odom"            to="/base_controller/odom"/>
-      
+
      <param name="rgb/image_transport"   type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/>
  </node>
-  
+
  </group>
-    
+
  <!-- Visualisation RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz"/>
  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
@@ -85,10 +85,10 @@
    <remap from="depth/image"     to="/data_throttled_image_depth"/>
    <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
    <remap from="cloud"           to="voxel_cloud" />
-    
+
    <param name="rgb/image_transport"   type="string" value="compressed"/>
    <param name="depth/image_transport" type="string" value="compressedDepth"/>
-    
+
    <param name="queue_size" type="int" value="10"/>
    <param name="voxel_size" type="double" value="0.01"/>
  </node>
@@ -8,10 +8,10 @@
  <!-- Choose visualization -->
  <arg name="rviz" default="false" />
-  <arg name="rtabmapviz" default="true" /> 
+  <arg name="rtabmapviz" default="true" />
-  
+
  <param name="use_sim_time" type="bool" value="True"/>
-  
+
  <!-- Localization-only mode -->
  <arg name="localization"      default="false"/>
  <arg     if="$(arg localization)" name="rtabmap_args"  default=""/>
@@ -23,10 +23,10 @@
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
 	  <param name="frame_id"           type="string" value="base_footprint"/>
 	  <param name="wait_for_transform" type="bool" value="true"/>
-	
+
 	  <param name="subscribe_depth" type="bool" value="true"/>
 	  <param name="subscribe_scan"  type="bool" value="true"/>
-	
+
 	  <!-- As /az3/base_controller/odom topic doesn't provide covariances, we use TF to get odom and we fix the covariance -->
 	  <param name="odom_frame_id"            type="string" value="odom"/>
 	  <param name="odom_tf_linear_variance"  type="double" value="0.001"/>
@@ -37,10 +37,10 @@
 	  <remap from="rgb/image"       to="/data_throttled_image"/>
  	  <remap from="depth/image"     to="/data_throttled_image_depth"/>
  	  <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
-  	
+
 	  <param name="rgb/image_transport"   type="string" value="compressed"/>
 	  <param name="depth/image_transport" type="string" value="compressedDepth"/>
-	
+
 	  <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
 	  <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>  <!-- Do odometry correction with consecutive laser scans -->
 	  <param name="RGBD/ProximityBySpace"     type="string" value="true"/>  <!-- Local loop closure detection (using estimated position) with locations in WM -->
@@ -59,31 +59,31 @@
 	  <param name="Icp/PointToPlane"          type="string" value="false"/>
 	  <param name="Icp/MaxCorrespondenceDistance"  type="string" value="0.15"/>
 	  <param name="Icp/VoxelSize"             type="string" value="0.05"/>
-	  
+
 	  <!-- 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)"/>
   </node>
-    
+
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
  	  <param name="subscribe_depth"    type="bool" value="true"/>
      <param name="subscribe_scan"     type="bool" value="true"/>
      <param name="frame_id"           type="string" value="base_footprint"/>
      <param name="wait_for_transform" type="bool" value="true"/>
-    
+
      <remap from="rgb/image"       to="/data_throttled_image"/>
      <remap from="depth/image"     to="/data_throttled_image_depth"/>
      <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
      <remap from="scan"            to="/jn0/base_scan"/>
      <remap from="odom"            to="/az3/base_controller/odom"/>
-      
+
      <param name="rgb/image_transport"   type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/>
    </node>
  </group>
-  
+
  <!-- Visualisation RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz" output="screen"/>
   <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
@@ -91,10 +91,10 @@
    <remap from="depth/image"     to="/data_throttled_image_depth"/>
    <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
    <remap from="cloud"           to="voxel_cloud" />
-    
+
    <param name="rgb/image_transport"   type="string" value="compressed"/>
    <param name="depth/image_transport" type="string" value="compressedDepth"/>
-    
+
    <param name="queue_size" type="int" value="10"/>
    <param name="voxel_size" type="double" value="0.01"/>
  </node>
@@ -1,28 +1,28 @@
 <?xml version="1.0"?>
 <launch>
-   
+
   <!--
-      Demo of outdoor stereo mapping. 
+      Demo of outdoor stereo mapping.
-      From bag: 
+      From bag:
-      $ rosbag record 
+      $ rosbag record
-            /stereo_camera/left/image_raw_throttle/compressed 
+            /stereo_camera/left/image_raw_throttle/compressed
-            /stereo_camera/right/image_raw_throttle/compressed 
+            /stereo_camera/right/image_raw_throttle/compressed
-            /stereo_camera/left/camera_info_throttle 
+            /stereo_camera/left/camera_info_throttle
-            /stereo_camera/right/camera_info_throttle 
+            /stereo_camera/right/camera_info_throttle
            /tf
-    
+
      $ roslaunch rtabmap demo_stereo_outdoor.launch
      $ rosbag play -.-clock stereo_oudoorA.bag
   -->
-   
+
   <!-- Choose visualization -->
   <arg name="rviz" default="true" />
   <arg name="rtabmapviz" default="false" />
   <arg name="local_bundle" default="true" />
   <arg name="stereo_sync" default="false" />
-    
+
   <param name="use_sim_time" type="bool" value="True"/>
-   
+
   <!-- Just to uncompress images for stereo_image_rect -->
   <node name="republish_left"  type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/left/image_raw_throttle raw out:=/stereo_camera/left/image_raw_throttle_relay" />
   <node name="republish_right" type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/right/image_raw_throttle raw out:=/stereo_camera/right/image_raw_throttle_relay" />
@@ -36,7 +36,7 @@
         <remap from="right/camera_info" to="right/camera_info_throttle"/>
         <param name="disparity_range" value="128"/>
      </node>
-      
+
      <node if="$(arg stereo_sync)" pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync">
      <remap from="left/image_rect"   to="left/image_rect_color"/>
      <remap from="right/image_rect"   to="right/image_rect"/>
@@ -44,10 +44,10 @@
      <remap from="right/camera_info"   to="right/camera_info_throttle"/>
    </node>
   </group>
-              
+
-   <group ns="rtabmap">   
+   <group ns="rtabmap">
-   
+
-      <!-- Stereo Odometry -->   
+      <!-- Stereo Odometry -->
      <node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
         <remap from="left/image_rect"       to="/stereo_camera/left/image_rect"/>
         <remap from="right/image_rect"      to="/stereo_camera/right/image_rect"/>
@@ -66,12 +66,12 @@
         <param name="Odom/GuessMotion" type="string" value="true"/>
         <param name="Vis/MinInliers"     type="string" value="10"/>
         <param unless="$(arg local_bundle)" name="OdomF2M/BundleAdjustment" type="string" value="0"/>
-         <param name="OdomF2M/MaxSize"    type="string" value="1000"/> 
+         <param name="OdomF2M/MaxSize"    type="string" value="1000"/>
         <param name="GFTT/MinDistance"   type="string" value="10"/>
-         <param name="GFTT/QualityLevel"  type="string" value="0.00001"/> 
+         <param name="GFTT/QualityLevel"  type="string" value="0.00001"/>
         <param name="GFTT/QualityLevel"  type="string" value="0.00001"/>
      </node>
-   
+
      <!-- Visual SLAM: args: "delete_db_on_start" and "udebug" -->
      <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
         <param name="frame_id"         type="string" value="base_footprint"/>
@@ -95,12 +95,12 @@
         <param name="Grid/DepthDecimation"              type="string" value="4"/>
         <param name="Grid/FlatObstacleDetected"         type="string" value="true"/>
         <param name="Kp/MaxDepth"                       type="string" value="0"/>
-         <param name="Kp/DetectorStrategy"               type="string" value="6"/>  
+         <param name="Kp/DetectorStrategy"               type="string" value="6"/>
         <param name="Vis/EstimationType"                type="string" value="1"/>   <!-- 0=3D->3D, 1=3D->2D (PnP) -->
         <param name="Vis/MaxDepth"                      type="string" value="0"/>
-         <param name="RGBD/CreateOccupancyGrid"          type="string" value="true"/>       
+         <param name="RGBD/CreateOccupancyGrid"          type="string" value="true"/>
      </node>
-      
+
      <!-- Visualisation RTAB-Map -->
      <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
         <param unless="$(arg stereo_sync)" name="subscribe_stereo"    type="bool" value="true"/>
@@ -108,7 +108,7 @@
         <param name="subscribe_rgbd"      type="bool" value="$(arg stereo_sync)"/>
         <param name="queue_size"          type="int" value="10"/>
         <param name="frame_id"            type="string" value="base_footprint"/>
-         
+
         <remap from="left/image_rect"  to="/stereo_camera/left/image_rect_color"/>
         <remap from="right/image_rect"  to="/stereo_camera/right/image_rect"/>
         <remap from="left/camera_info"  to="/stereo_camera/left/camera_info_throttle"/>
@@ -118,10 +118,10 @@
         <remap from="odom"              to="/stereo_odometry"/>
         <remap from="mapData"           to="mapData"/>
      </node>
-         
+
   </group>
-  
+
-   <!-- Visualisation RVIZ --> 
+   <!-- Visualisation RVIZ -->
   <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_stereo_outdoor.rviz"/>
 </launch>
@@ -2,7 +2,7 @@
 <launch>
    <!-- 6DOF stereo demo: download a bag example from http://projects.csail.mit.edu/stata/downloads.php.
    You will need to remove the transform /combined_odometry from the /tf messages:
-    $ rosbag filter 2011-01-20-07-18-45.bag out.bag 'topic != "/tf" or topic == "/tf" and m.transforms[0].header.frame_id != "/odom_combined"' 
+    $ rosbag filter 2011-01-20-07-18-45.bag out.bag 'topic != "/tf" or topic == "/tf" and m.transforms[0].header.frame_id != "/odom_combined"'
    Run the example:
    $ roslaunch rtabmap demo_stereo.launch
    $ rosbag play -.-clock out.bag        (replace -.- by double-dashes)
@@ -25,37 +25,37 @@
 		<param name="odom_frame_id" value="/odom"/>
 		<param name="ref_frame_change_method" value="1"/>
 	</node>
-	
+
  <group ns="rtabmap">
-  
+
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="subscribe_depth" type="bool" value="true"/>
      <param name="subscribe_laserScan" type="bool" value="true"/>
-	
+
      <remap from="rgb/image" to="/wide_stereo/left/image_rect"/>
      <remap from="rgb/camera_info" to="/wide_stereo/left/camera_info"/>
-      
+
      <remap from="depth/image" to="/wide_stereo/depth"/>
-      
+
      <remap from="odom" to="/stereo_odometer/odometry"/>
      <remap from="scan" to="/base_scan"/>
-      
+
      <param name="frame_id" type="string" value="/base_footprint"/>
      <param name="queue_size" type="int" value="30"/>
-      
+
      <param name="Rtabmap/TimeThr" type="string" value="700"/>
      <param name="Rtabmap/DetectionRate" type="string" value="1"/>
      <param name="SURF/HessianThreshold" type="string" value="600"/>
      <param name="LccBow/MaxDepth" type="string" value="0"/>
-      <param name="RGBD/LocalLoopDetectionSpace" type="string" value="false"/> 
+      <param name="RGBD/LocalLoopDetectionSpace" type="string" value="false"/>
 	  <param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/>
      <param name="LccBow/MinInliers" type="string" value="10"/>
      <param name="LccBow/InlierDistance" type="string" value="0.05"/>
      <param name="Kp/DetectorStrategy" type="string" value="0"/>   <!-- use SURF -->
 	  <param name="Kp/NNStrategy" type="string" value="1"/>         <!-- kdTree -->
-           
+
      <!-- Uncomment to force 3dof loop closure constraint using                   -->
      <!-- the 2d scans (set ScanMatchingSize=1 to correct odometry with laser)    -->
      <!--
@@ -66,20 +66,20 @@
 	  -->
    </node>
-  
+
    <!-- Visualisation (client side) -->
    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
      <param name="subscribe_depth" type="bool" value="true"/>
      <param name="subscribe_laserScan" type="bool" value="true"/>
      <param name="queue_size" type="int" value="30"/>
-    
+
      <remap from="rgb/image" to="/wide_stereo/left/image_rect"/>
      <remap from="rgb/camera_info" to="/wide_stereo/left/camera_info"/>
      <remap from="depth/image" to="/wide_stereo/depth"/>
-      
+
      <remap from="scan" to="/base_scan"/>
      <remap from="odom" to="/stereo_odometer/odometry"/>
    </node>
  </group>
-	
+
 </launch>
@@ -37,7 +37,7 @@
      <remap from="depth/image"       to="/camera/depth/image_raw"/>
      <remap from="rgb/camera_info"   to="/camera/rgb/camera_info"/>
    </node>
-  
+
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
      <param name="database_path"       type="string" value="$(arg database_path)"/>
      <param name="frame_id"            type="string" value="base_footprint"/>
@@ -48,10 +48,10 @@
      <param name="subscribe_scan"      type="bool"   value="true"/>
      <param name="approx_sync"         type="bool"   value="true"/>
-      <!-- use actionlib to send goals to move_base --> 
+      <!-- use actionlib to send goals to move_base -->
      <param name="use_action_for_goal" type="bool" value="true"/>
      <remap from="move_base"            to="/move_base"/>
-	
+
      <!-- inputs -->
      <remap from="scan"            to="/scan"/>
      <remap from="odom"            to="/odom"/>
@@ -59,7 +59,7 @@
      <!-- output -->
      <remap from="grid_map" to="/map"/>
-	
+
      <!-- RTAB-Map's parameters -->
      <param name="Reg/Strategy"                 type="string" value="1"/>
      <param name="Reg/Force3DoF"                type="string" value="true"/>
@@ -89,7 +89,7 @@
  </include>
  <!-- rviz -->
-  <group if="$(arg open_rviz)"> 
+  <group if="$(arg open_rviz)">
    <node pkg="rviz" type="rviz" name="rviz" required="true"
          args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/>
  </group>
@@ -1,32 +1,32 @@
 <?xml version="1.0"?>
 <launch>
-  <!-- 
+  <!--
       Bringup Turtlebot:
       $ roslaunch turtlebot_bringup minimal.launch
-       
+
       Mapping:
       $ roslaunch rtabmap_ros demo_turtlebot_mapping.launch
-       
+
       Visualization:
       $ roslaunch rtabmap_ros demo_turtlebot_rviz.launch
-       
+
-       This launch file is a one to one replacement of the gmapping_demo.launch in the 
+       This launch file is a one to one replacement of the gmapping_demo.launch in the
       "SLAM Map Building with TurtleBot" tutorial:
       http://wiki.ros.org/turtlebot_navigation/Tutorials/indigo/Build%20a%20map%20with%20SLAM
-       
+
       For localization-only after a mapping session, add argument "localization:=true" to
-       demo_turtlebot_mapping.launch line above. Move the robot around until it can relocalize in 
+       demo_turtlebot_mapping.launch line above. Move the robot around until it can relocalize in
-       the previous map, then the 2D map should re-appear again. You can then follow the same steps 
+       the previous map, then the 2D map should re-appear again. You can then follow the same steps
       from 3.3.2 of the "Autonomous Navigation of a Known Map with TurtleBot" tutorial:
       http://wiki.ros.org/turtlebot_navigation/Tutorials/Autonomously%20navigate%20in%20a%20known%20map
- 
+
       For turtlebot in simulation (Gazebo):
         $ roslaunch turtlebot_gazebo turtlebot_world.launch
         $ roslaunch rtabmap_ros demo_turtlebot_mapping.launch simulation:=true
         $ roslaunch rtabmap_ros demo_turtlebot_rviz.launch
  -->
-  
+
  <arg name="database_path"     default="rtabmap.db"/>
  <arg name="rgbd_odometry"     default="false"/>
  <arg name="rtabmapviz"        default="false"/>
@@ -41,20 +41,20 @@
  <arg     if="$(arg simulation)" name="depth_topic" default="/camera/depth/image_raw"/>
  <arg unless="$(arg simulation)" name="depth_topic" default="/camera/depth_registered/image_raw"/>
  <arg name="camera_info_topic" default="/camera/rgb/camera_info"/>
-  
+
-  <arg name="wait_for_transform"  default="0.2"/> 
+  <arg name="wait_for_transform"  default="0.2"/>
-  <!-- 
+  <!--
-      robot_state_publisher's publishing frequency in "turtlebot_bringup/launch/includes/robot.launch.xml" 
+      robot_state_publisher's publishing frequency in "turtlebot_bringup/launch/includes/robot.launch.xml"
      can be increase from 5 to 10 Hz to avoid some TF warnings.
  -->
-  
+
  <!-- Navigation stuff (move_base) -->
  <include unless="$(arg simulation)" file="$(find turtlebot_bringup)/launch/3dsensor.launch">
     <arg     if="$(arg sw_registered)" name="depth_registration" value="false"/>
     <arg unless="$(arg sw_registered)" name="depth_registration" value="true"/>
  </include>
  <include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"/>
-  
+
  <!-- Mapping -->
  <group ns="rtabmap">
@@ -66,10 +66,10 @@
 	  <param name="subscribe_scan"      type="bool"   value="true"/>
 	  <param name="map_negative_poses_ignored" type="bool" value="true"/>
-          <!-- When sending goals on /rtabmap/goal topic, use actionlib to communicate with move_base --> 
+          <!-- When sending goals on /rtabmap/goal topic, use actionlib to communicate with move_base -->
 	  <param name="use_action_for_goal" type="bool" value="true"/>
          <remap from="move_base"            to="/move_base"/>
-	
+
 	  <!-- inputs -->
 	  <remap from="scan"            to="/scan"/>
 	  <remap from="rgb/image"       to="$(arg rgb_topic)"/>
@@ -80,10 +80,10 @@
          <param unless="$(arg rgbd_odometry)" name="odom_frame_id" value="odom"/>
          <param unless="$(arg rgbd_odometry)" name="odom_tf_linear_variance" value="0.001"/>
          <param unless="$(arg rgbd_odometry)" name="odom_tf_angular_variance" value="0.001"/>
-  	  
+
  	  <!-- output -->
  	  <remap from="grid_map" to="/map"/>
-	
+
 	  <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
 	  <param name="RGBD/ProximityBySpace"        type="string" value="true"/>   <!-- Local loop closure detection (using estimated position) with locations in WM -->
 	  <param name="RGBD/OptimizeFromGraphEnd"    type="string" value="false"/>  <!-- Set to false to generate map correction between /map and /odom -->
@@ -94,43 +94,43 @@
 	  <param name="Vis/InlierDistance"           type="string" value="0.1"/>    <!-- 3D visual words correspondence distance -->
 	  <param name="RGBD/AngularUpdate"           type="string" value="0.1"/>    <!-- Update map only if the robot is moving -->
 	  <param name="RGBD/LinearUpdate"            type="string" value="0.1"/>    <!-- Update map only if the robot is moving -->
-	  <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/> 
+	  <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/>
 	  <param name="Rtabmap/TimeThr"              type="string" value="0"/>
 	  <param name="Mem/RehearsalSimilarity"      type="string" value="0.30"/>
 	  <param name="Reg/Force3DoF"                type="string" value="true"/>
 	  <param name="GridGlobal/MinSize"           type="string" value="20"/>
-	  
+
 	  <!-- 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)"/> 
+	  <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
    </node>
-   
+
    <!-- Odometry : ONLY for testing without the actual robot! /odom TF should not be already published. -->
    <node if="$(arg rgbd_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
      <param name="frame_id"                    type="string" value="base_footprint"/>
      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
      <param name="Reg/Force3DoF"               type="string" value="true"/>
      <param name="Vis/InlierDistance"          type="string" value="0.05"/>
-      
+
      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
    </node>
-    
+
    <!-- visualization with rtabmapviz -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
  	  <param name="subscribe_depth"             type="bool" value="true"/>
      <param name="subscribe_scan"              type="bool" value="true"/>
      <param name="frame_id"                    type="string" value="base_footprint"/>
      <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_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="scan"            to="/scan"/>
    </node>
-    
+
  </group>
 </launch>
@@ -1,51 +1,51 @@
 <?xml version="1.0"?>
 <launch>
-  <!-- 
+  <!--
       Bringup Turtlebot:
       $ roslaunch turtlebot_bringup minimal.launch
-       
+
       Mapping:
       $ roslaunch rtabmap_ros demo_turtlebot_tango.launch
-       
+
       Visualization:
       $ roslaunch rtabmap_ros demo_turtlebot_rviz.launch
-       
+
-       This launch file is a one to one replacement of the gmapping_demo.launch in the 
+       This launch file is a one to one replacement of the gmapping_demo.launch in the
       "SLAM Map Building with TurtleBot" tutorial:
       http://wiki.ros.org/turtlebot_navigation/Tutorials/indigo/Build%20a%20map%20with%20SLAM
-       
+
       For localization-only after a mapping session, add argument "localization:=true" to
-       demo_turtlebot_mapping.launch line above. Move the robot around until it can relocalize in 
+       demo_turtlebot_mapping.launch line above. Move the robot around until it can relocalize in
-       the previous map, then the 2D map should re-appear again. You can then follow the same steps 
+       the previous map, then the 2D map should re-appear again. You can then follow the same steps
       from 3.3.2 of the "Autonomous Navigation of a Known Map with TurtleBot" tutorial:
       http://wiki.ros.org/turtlebot_navigation/Tutorials/Autonomously%20navigate%20in%20a%20known%20map
  -->
-  
+
  <arg name="database_path"     default="rtabmap.db"/>
  <arg name="tango_odometry"    default="true"/>
  <arg name="localization"      default="false"/>
  <arg name="rtabmapviz"        default="false"/>
  <arg     if="$(arg localization)" name="args"  default=""/>
  <arg unless="$(arg localization)" name="args"  default="--delete_db_on_start"/>
-  
+
-  <arg name="wait_for_transform"  default="0.2"/> 
+  <arg name="wait_for_transform"  default="0.2"/>
-  <!-- 
+  <!--
-      robot_state_publisher's publishing frequency in "turtlebot_bringup/launch/includes/robot.launch.xml" 
+      robot_state_publisher's publishing frequency in "turtlebot_bringup/launch/includes/robot.launch.xml"
      can be increase from 5 to 10 Hz to avoid some TF warnings.
  -->
  <!-- Adjust these transforms on where Tango is fixed on the robot (should be landscape to get laser scans right). When Tango odometry is used, we need to offset /start_of_service to actual camera pose, then set /base_footprint relative to /device  1.5707963267948966 -->
-  <node if="$(arg tango_odometry)" pkg="tf" type="static_transform_publisher" name="device_base_link" args="-0.3 0 0 0 1.5707963267948966 0 device base_footprint 100" /> 
+  <node if="$(arg tango_odometry)" pkg="tf" type="static_transform_publisher" name="device_base_link" args="-0.3 0 0 0 1.5707963267948966 0 device base_footprint 100" />
-  <node if="$(arg tango_odometry)" pkg="tf" type="static_transform_publisher" name="odom_start_of_service_link" args="0 0 0.3 0 0 0 odom start_of_service 100" /> 
+  <node if="$(arg tango_odometry)" pkg="tf" type="static_transform_publisher" name="odom_start_of_service_link" args="0 0 0.3 0 0 0 odom start_of_service 100" />
-  <node unless="$(arg tango_odometry)" pkg="tf" type="static_transform_publisher" name="base_device_link" args="0 0 0.3 0 0 0 device base_link 100" /> 
+  <node unless="$(arg tango_odometry)" pkg="tf" type="static_transform_publisher" name="base_device_link" args="0 0 0.3 0 0 0 device base_link 100" />
  <!-- Generate registered depth image -->
  <node name="pointcloud_to_depthimage" pkg="rtabmap_ros" type="pointcloud_to_depthimage">
    <remap from="cloud"            to="/tango/point_cloud"/>
    <remap from="image"            to="/tango/registered_depth"/>
    <remap from="camera_info"      to="/tango/camera/color_1/camera_info"/>
-   
+
    <param name="fixed_frame_id"   type="string"   value="odom"/>
    <param name="decimation"       type="int"      value="8"/>
    <param name="fill_holes_size"  type="int"      value="5"/>
@@ -54,7 +54,7 @@
  <!-- Navigation stuff (move_base) -->
  <node name="scan_relay" type="relay" pkg="topic_tools" args="/tango/laser_scan /scan"/>
  <include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"/>
-  
+
  <!-- relay to avoid subscribing multiple times to rgb image -->
  <node name="republish_rgb" type="republish" pkg="image_transport" args="compressed in:=/tango/camera/color_1/image_raw raw out:=/tango/camera/color_1/image_raw/relay" />
@@ -69,16 +69,16 @@
 	  <param name="subscribe_depth"     type="bool"   value="true"/>
 	  <param name="subscribe_scan"      type="bool"   value="true"/>
 	  <param name="approx_sync"         type="bool"   value="true"/>
-	
+
 	  <!-- inputs -->
 	  <remap from="scan"            to="/scan"/>
 	  <remap from="rgb/image"       to="/tango/camera/color_1/image_raw/relay"/>
  	  <remap from="depth/image"     to="/tango/registered_depth"/>
  	  <remap from="rgb/camera_info" to="/tango/camera/color_1/camera_info"/>
-  	  
+
  	  <!-- output -->
  	  <remap from="grid_map" to="/map"/>
-	
+
 	  <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
 	  <param name="RGBD/ProximityBySpace"        type="string" value="true"/>   <!-- Local loop closure detection (using estimated position) with locations in WM -->
 	  <param name="RGBD/OptimizeFromGraphEnd"    type="string" value="false"/>  <!-- Set to false to generate map correction between /map and /odom -->
@@ -89,33 +89,33 @@
 	  <param name="Vis/InlierDistance"           type="string" value="0.1"/>    <!-- 3D visual words correspondence distance -->
 	  <param name="RGBD/AngularUpdate"           type="string" value="0.1"/>    <!-- Update map only if the robot is moving -->
 	  <param name="RGBD/LinearUpdate"            type="string" value="0.1"/>    <!-- Update map only if the robot is moving -->
-	  <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/> 
+	  <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/>
 	  <param name="Rtabmap/TimeThr"              type="string" value="700"/>
 	  <param name="Mem/RehearsalSimilarity"      type="string" value="0.30"/>
 	  <param name="Optimizer/Slam2D"             type="string" value="false"/> <!-- cannot do 2D slam as Tango is 6DoF -->
 	  <param name="Reg/Force3DoF"                type="string" value="false"/> <!-- cannot do 2D slam as Tango is 6DoF -->
 	  <param name="Mem/ImagePreDecimation"       type="string" value="2"/>
-	  <param name="Mem/ImagePostDecimation"      type="string" value="2"/>   
+	  <param name="Mem/ImagePostDecimation"      type="string" value="2"/>
-	 
+
 	  <!-- 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)"/> 
+	  <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
    </node>
-       
+
    <!-- visualization with rtabmapviz -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
      <param name="subscribe_depth"             type="bool" value="true"/>
      <param name="subscribe_scan"              type="bool" value="true"/>
      <param name="frame_id"                    type="string" value="base_footprint"/>
      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
-    
+
      <remap from="rgb/image"       to="/tango/camera/color_1/image_raw/relay"/>
      <remap from="depth/image"     to="/tango/registered_depth"/>
      <remap from="rgb/camera_info" to="/tango/camera/color_1/camera_info"/>
      <remap from="scan"            to="/scan"/>
    </node>
-    
+
  </group>
 </launch>
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <launch>
-   
+
  <!-- Multi-cameras demo with 2 Kinects -->
-   
+
  <!-- Cameras -->
  <include file="$(find freenect_launch)/launch/freenect.launch">
    <arg name="depth_registration" value="True" />
@@ -21,21 +21,21 @@
      args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera1_link 100" />
  <node pkg="tf" type="static_transform_publisher" name="base_to_camera2_tf"
      args="-0.1325 -0.1975 0.0 -1.570796327 0.0 0.0 /base_link /camera2_link 100" />
-   
+
   <!-- Choose visualization -->
   <arg name="rviz"       default="false" />
-   <arg name="rtabmapviz" default="true" /> 
+   <arg name="rtabmapviz" default="true" />
-   
+
-   <!-- ODOMETRY MAIN ARGUMENTS: 
+   <!-- ODOMETRY MAIN ARGUMENTS:
        -"strategy"        : Strategy: 0=Frame-to-Map 1=Frame-to-Frame
        -"feature"         : Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-        -"nn"              : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE 
+        -"nn"              : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
-                             Set to 1 for float descriptor like SIFT/SURF                  
+                             Set to 1 for float descriptor like SIFT/SURF
-                             Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK  
+                             Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-        -"max_depth"       : Maximum features depth (m)  
+        -"max_depth"       : Maximum features depth (m)
-        -"min_inliers"     : Minimum visual correspondences to accept a transformation (m)  
+        -"min_inliers"     : Minimum visual correspondences to accept a transformation (m)
-        -"inlier_distance" : RANSAC maximum inliers distance (m)  
+        -"inlier_distance" : RANSAC maximum inliers distance (m)
-        -"local_map"       : Local map size: number of unique features to keep track 
+        -"local_map"       : Local map size: number of unique features to keep track
        -"odom_info_data"  : Fill odometry info messages with inliers/outliers data.
    -->
   <arg name="strategy"        default="0" />
@@ -47,7 +47,7 @@
   <arg name="local_map"       default="1000" />
   <arg name="odom_info_data"  default="true" />
   <arg name="wait_for_transform"  default="true" />
-   
+
   <!-- sync rgb/depth images per camera -->
   <group ns="camera1">
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera1_nodelet_manager">
@@ -63,31 +63,31 @@
      <remap from="rgb/camera_info"   to="rgb/camera_info"/>
    </node>
   </group>
-        
+
  <group ns="rtabmap">
-  
+
    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
-	  
+
 	  <param name="subscribe_rgbd"           type="bool"   value="true"/>
 	  <param name="frame_id"                 type="string" value="base_link"/>
 	  <param name="rgbd_cameras"             type="int"    value="2"/>
 	  <param name="wait_for_transform"       type="bool"   value="$(arg wait_for_transform)"/>
-	  <param name="Odom/Strategy"            type="string" value="$(arg strategy)"/> 
+	  <param name="Odom/Strategy"            type="string" value="$(arg strategy)"/>
 	  <param name="OdomF2M/BundleAdjustment" type="string" value="0"/> <!-- should be 0 for multi-cameras -->
 	  <param name="Vis/EstimationType"      type="string" value="0"/> <!-- should be 0 for multi-cameras -->
-	  <param name="Vis/FeatureType"         type="string" value="$(arg feature)"/> 
+	  <param name="Vis/FeatureType"         type="string" value="$(arg feature)"/>
-	  <param name="Vis/CorGuessWinSize"     type="string" value="0"/> 
+	  <param name="Vis/CorGuessWinSize"     type="string" value="0"/>
 	  <param name="Vis/CorNNType"           type="string" value="$(arg nn)"/>
-	  <param name="Vis/MaxDepth"            type="string" value="$(arg max_depth)"/>  
+	  <param name="Vis/MaxDepth"            type="string" value="$(arg max_depth)"/>
-	  <param name="Vis/MinInliers"          type="string" value="$(arg min_inliers)"/> 
+	  <param name="Vis/MinInliers"          type="string" value="$(arg min_inliers)"/>
-	  <param name="Vis/InlierDistance"      type="string" value="$(arg inlier_distance)"/>       
+	  <param name="Vis/InlierDistance"      type="string" value="$(arg inlier_distance)"/>
-      <param name="OdomF2M/MaxSize" type="string" value="$(arg local_map)"/> 
+      <param name="OdomF2M/MaxSize" type="string" value="$(arg local_map)"/>
-      <param name="Odom/FillInfoData"        type="string" value="$(arg odom_info_data)"/>   
+      <param name="Odom/FillInfoData"        type="string" value="$(arg odom_info_data)"/>
    </node>
-  
+
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
@@ -99,7 +99,7 @@
 	  <param name="wait_for_transform" type="bool"   value="$(arg wait_for_transform)"/>
 	  <param name="map_negative_poses_ignored" type="bool"   value="false"/>        <!-- refresh grid map even if we are not moving-->
 	  <param name="map_negative_scan_empty_ray_tracing" type="bool" value="false"/> <!-- don't fill empty space between the generated scans-->
-	
+
      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
@@ -108,7 +108,7 @@
      <param name="Vis/MinInliers"     type="string" value="10"/>
      <param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
    </node>
-  
+
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
      <param name="subscribe_depth"  type="bool"   value="false"/>
@@ -117,13 +117,13 @@
      <param name="frame_id"            type="string" value="base_link"/>
      <param name="rgbd_cameras"       type="int"    value="2"/>
      <param name="wait_for_transform"  type="bool"   value="$(arg wait_for_transform)"/>
-    
+
      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
    </node>
-  
+
  </group>
-  
+
  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
@@ -31,7 +31,7 @@
 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 #
-# Requirements: 
+# Requirements:
 # sudo apt-get install python-argparse
 """
@@ -48,31 +48,31 @@ import numpy
 def read_file_list(filename):
    """
-    Reads a trajectory from a text file. 
+    Reads a trajectory from a text file.
-    
+
    File format:
    The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched)
-    and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp. 
+    and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp.
-    
+
    Input:
    filename -- File name
-    
+
    Output:
    dict -- dictionary of (stamp,data) tuples
-    
+
    """
    file = open(filename)
    data = file.read()
-    lines = data.replace(","," ").replace("\t"," ").split("\n") 
+    lines = data.replace(","," ").replace("\t"," ").split("\n")
    list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
    list = [(float(l[0]),l[1:]) for l in list if len(l)>1]
    return dict(list)
 def associate(first_list, second_list,offset,max_difference):
    """
-    Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim 
+    Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim
    to find the closest match for every input tuple.
-    
+
    Input:
    first_list -- first dictionary of (stamp,data) tuples
    second_list -- second dictionary of (stamp,data) tuples
@@ -81,13 +81,13 @@ def associate(first_list, second_list,offset,max_difference):
    Output:
    matches -- list of matched tuples ((stamp1,data1),(stamp2,data2))
-    
+
    """
    first_keys = first_list.keys()
    second_keys = second_list.keys()
-    potential_matches = [(abs(a - (b + offset)), a, b) 
+    potential_matches = [(abs(a - (b + offset)), a, b)
-                         for a in first_keys 
+                         for a in first_keys
-                         for b in second_keys 
+                         for b in second_keys
                         if abs(a - (b + offset)) < max_difference]
    potential_matches.sort()
    matches = []
@@ -96,15 +96,15 @@ def associate(first_list, second_list,offset,max_difference):
            first_keys.remove(a)
            second_keys.remove(b)
            matches.append((a, b))
-    
+
    matches.sort()
    return matches
 if __name__ == '__main__':
-    
+
    # parse command line
    parser = argparse.ArgumentParser(description='''
-    This script takes two data files with timestamps and associates them   
+    This script takes two data files with timestamps and associates them
    ''')
    parser.add_argument('first_file', help='first text file (format: timestamp data)')
    parser.add_argument('second_file', help='second text file (format: timestamp data)')
@@ -116,7 +116,7 @@ if __name__ == '__main__':
    first_list = read_file_list(args.first_file)
    second_list = read_file_list(args.second_file)
-    matches = associate(first_list, second_list,float(args.offset),float(args.max_difference))    
+    matches = associate(first_list, second_list,float(args.offset),float(args.max_difference))
    if args.first_only:
        for a,b in matches:
@@ -124,5 +124,5 @@ if __name__ == '__main__':
    else:
        for a,b in matches:
            print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b])))
-            
+
-        
+
@@ -23,7 +23,7 @@
      type="cartographer_node" args="
          -configuration_directory
              $(find cartographer_ros)/configuration_files
-          -configuration_basename pr2.lua" 
+          -configuration_basename pr2.lua"
      output="screen">
    <remap from="scan" to="/base_scan_t_filtered" /> <!-- /base_scan_t /base_scan_t_filtered /camera_scan -->
    <remap from="odom" to="/odom_combined" />
@@ -1,4 +1,4 @@
-#!/usr/bin/env python  
+#!/usr/bin/env python
 import roslib
 import rospy
 import os
@@ -1,4 +1,4 @@
-#!/usr/bin/env python  
+#!/usr/bin/env python
 import roslib
 import rospy
 import os
@@ -15,7 +15,7 @@ def callback(data):
    global listener
    global rmse
    global lastTime
-  
+
    if rospy.get_time() - lastTime < 1:
        return
    lastTime = rospy.get_time()
@@ -9,17 +9,17 @@ index 67820e7..aec1839 100644
 +    maxDist: 1
     knn: 5
     epsilon: 3.16
- 
+
 outlierFilters:
   - TrimmedDistOutlierFilter:
 -     ratio: 0.85
 +     ratio: 0.95
   - SurfaceNormalOutlierFilter:
      maxAngle: 0.42
- 
+
@@ -25,7 +25,11 @@ transformationCheckers:
       maxTranslationNorm: 5.00
- 
+
 inspector:
 -# VTKFileInspector
 +# VTKFileInspector:
@@ -28,7 +28,7 @@ index 67820e7..aec1839 100644
 +#   dumpReading : 1
 +#   dumpReference : 1
  NullInspector
- 
+
 logger:
 diff --git a/libpointmatcher_ros/src/point_cloud.cpp b/libpointmatcher_ros/src/point_cloud.cpp
 index b77651d..8eb1f6c 100644
@@ -31,7 +31,7 @@
 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 #
-# Requirements: 
+# Requirements:
 # sudo apt-get install python-argparse
 """
@@ -46,21 +46,21 @@ import associate
 def align(model,data):
    """Align two trajectories using the method of Horn (closed-form).
-    
+
    Input:
    model -- first trajectory (3xn)
    data -- second trajectory (3xn)
-    
+
    Output:
    rot -- rotation matrix (3x3)
    trans -- translation vector (3x1)
    trans_error -- translational error per point (1xn)
-    
+
    """
    numpy.set_printoptions(precision=3,suppress=True)
    model_zerocentered = model - model.mean(1)
    data_zerocentered = data - data.mean(1)
-    
+
    W = numpy.zeros( (3,3) )
    for column in range(model.shape[1]):
        W += numpy.outer(model_zerocentered[:,column],data_zerocentered[:,column])
@@ -70,18 +70,18 @@ def align(model,data):
        S[2,2] = -1
    rot = U*S*Vh
    trans = data.mean(1) - rot * model.mean(1)
-    
+
    model_aligned = rot * model + trans
    alignment_error = model_aligned - data
-    
+
    trans_error = numpy.sqrt(numpy.sum(numpy.multiply(alignment_error,alignment_error),0)).A[0]
-        
+
    return rot,trans,trans_error
 def plot_traj(ax,stamps,traj,style,color,label):
    """
-    Plot a trajectory using matplotlib. 
+    Plot a trajectory using matplotlib.
-    
+
    Input:
    ax -- the plot
    stamps -- time stamps (1xn)
@@ -89,7 +89,7 @@ def plot_traj(ax,stamps,traj,style,color,label):
    style -- line style
    color -- line color
    label -- plot legend
-    
+
    """
    stamps.sort()
    interval = numpy.median([s-t for s,t in zip(stamps[1:],stamps[:-1])])
@@ -108,12 +108,12 @@ def plot_traj(ax,stamps,traj,style,color,label):
        last= stamps[i]
    if len(x)>0:
        ax.plot(x,y,style,color=color,label=label)
-            
+
 if __name__=="__main__":
    # parse command line
    parser = argparse.ArgumentParser(description='''
-    This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory. 
+    This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory.
    ''')
    parser.add_argument('first_file', help='ground truth trajectory (format: timestamp tx ty tz qx qy qz qw)')
    parser.add_argument('second_file', help='estimated trajectory (format: timestamp tx ty tz qx qy qz qw)')
@@ -129,7 +129,7 @@ if __name__=="__main__":
    first_list = associate.read_file_list(args.first_file)
    second_list = associate.read_file_list(args.second_file)
-    matches = associate.associate(first_list, second_list,float(args.offset),float(args.max_difference))    
+    matches = associate.associate(first_list, second_list,float(args.offset),float(args.max_difference))
    if len(matches)<2:
        sys.exit("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?")
@@ -137,18 +137,18 @@ if __name__=="__main__":
    first_xyz = numpy.matrix([[float(value) for value in first_list[a][0:3]] for a,b in matches]).transpose()
    second_xyz = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for a,b in matches]).transpose()
    rot,trans,trans_error = align(second_xyz,first_xyz)
-    
+
    second_xyz_aligned = rot * second_xyz + trans
-    
+
    first_stamps = first_list.keys()
    first_stamps.sort()
    first_xyz_full = numpy.matrix([[float(value) for value in first_list[b][0:3]] for b in first_stamps]).transpose()
-    
+
    second_stamps = second_list.keys()
    second_stamps.sort()
    second_xyz_full = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for b in second_stamps]).transpose()
    second_xyz_full_aligned = rot * second_xyz_full + trans
-    
+
    if args.verbose:
        print "compared_pose_pairs %d pairs"%(len(trans_error))
@@ -160,12 +160,12 @@ if __name__=="__main__":
        print "absolute_translational_error.max %f m"%numpy.max(trans_error)
    else:
        print "%f"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
-        
+
    if args.save_associations:
        file = open(args.save_associations,"w")
        file.write("\n".join(["%f %f %f %f %f %f %f %f"%(a,x1,y1,z1,b,x2,y2,z2) for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A)]))
        file.close()
-        
+
    if args.save:
        file = open(args.save,"w")
        file.write("\n".join(["%f "%stamp+" ".join(["%f"%d for d in line]) for stamp,line in zip(second_stamps,second_xyz_full_aligned.transpose().A)]))
@@ -186,10 +186,10 @@ if __name__=="__main__":
        #for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A):
        #    ax.plot([x1,x2],[y1,y2],'-',color="red",label=label)
        #    label=""
-            
+
        ax.legend()
-            
+
        ax.set_xlabel('x [m]')
        ax.set_ylabel('y [m]')
        plt.savefig(args.plot,dpi=300, format='pdf')
-        
+
@@ -1,4 +1,4 @@
-#!/usr/bin/env python  
+#!/usr/bin/env python
 import roslib
 import rospy
 import os
@@ -14,7 +14,7 @@ if __name__ == '__main__':
    offset_y = rospy.get_param('~offset_y', 0.0)
    offset_theta = rospy.get_param('~offset_theta', 0.0)
    gtFile = rospy.get_param('~file', 'groundtruth.txt')
-    gtFile = os.path.expanduser(gtFile) 
+    gtFile = os.path.expanduser(gtFile)
    br = tf.TransformBroadcaster()
    init_x = 0
@@ -34,7 +34,7 @@ if __name__ == '__main__':
                init_x = x
                init_y = y
                init = True
-            
+
            x -= init_x
            y -= init_y
@@ -15,7 +15,7 @@ odom_frame_id:="odom_combined" odom_tf_angular_variance:=0.0001 odom_tf_linear_v
 //neighbor link refine
 --RGBD/NeighborLinkRefining true --RGBD/OptimizeMaxError 1.5
 // odom frame to frame
--Odom/Strategy 1 
+--Odom/Strategy 1
 $ rosparam load scan_filter.yaml scan_to_scan_filter_chain
 $ rosrun laser_filters scan_to_scan_filter_chain scan:=/base_scan_t scan_filtered:=/base_scan_t_filtered
@@ -23,7 +23,7 @@ $ rosrun laser_filters scan_to_scan_filter_chain scan:=/base_scan_t scan_filtere
 $ ./republish_scan.py _offset:=82.2
 //stereo
-$ ./republish_camera_info.py camera_info_in:=/wide_stereo/right/camera_info camera_info_out:=/wide_stereo/right/camera_info_scaled 
+$ ./republish_camera_info.py camera_info_in:=/wide_stereo/right/camera_info camera_info_out:=/wide_stereo/right/camera_info_scaled
 $ export ROS_NAMESPACE=wide_stereo
 $ rosrun stereo_image_proc stereo_image_proc left/image_raw:=left/image_raw right/image_raw:=right/image_raw left/camera_info:=left/camera_info right/camera_info:=right/camera_info_scaled
@@ -66,7 +66,7 @@ $ rosbag play --clock --pause ./stata-mit/2012-01-25-12-14-25.bag
 //2012-01-25-12-33-29
 $ rosbag play --clock --pause ./stata-mit/2012-01-25-12-33-29.bag
 $ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-33-29_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275
-// for short-lidar 
+// for short-lidar
 $ rosparam load scan_filter.yaml scan_to_scan_filter_chain
 $ rosrun laser_filters scan_to_scan_filter_chain scan:=/base_scan_t scan_filtered:=/base_scan_t_filtered
 // for fake lidar kinect
@@ -1,8 +1,8 @@
 // rgbd_odometry
-$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/PublishRAMUsage true --Rtabmap/StartNewMapOnLoopClosure true --Reg/Force3DoF false --RGBD/ProximityPathMaxNeighbors 0 --Mem/STMSize 15 --Mem/BinDataKept false --Kp/FlannRebalancingFactor 1.0 --RGBD/LinearUpdate 0 --RGBD/ProximityBySpace true --RGBD/OptimizeMaxError 0.5 --FAST/Threshold 7" odom_args:="--Odom/Strategy 0 --Vis/CorType 0 --Odom/KeyFrameThr 0.3 --OdomF2M/MaxSize 2000 --OdomORBSLAM2/VocPath /home/mathieu/workspace/ORB_SLAM2/Vocabulary/ORBvoc.txt --OdomORBSLAM2/Fps 15" rgbd_sync:=true depth_scale:=1.043 frame_id:=base_footprint ground_truth_frame_id:=world ground_truth_base_frame_id:=scan_gt use_sim_time:=true  odom_topic:=odom rgb_topic:=/camera/rgb/image_raw_throttle depth_topic:=/camera/depth/image_raw_throttle camera_info_topic:=/camera/rgb/camera_info_throttle approx_sync:=false database_path:=/media/mathieu/5B60E7B25BDFCB79/bags/rtabmap.db odom_guess_frame_id:=odom_combined 
+$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/PublishRAMUsage true --Rtabmap/StartNewMapOnLoopClosure true --Reg/Force3DoF false --RGBD/ProximityPathMaxNeighbors 0 --Mem/STMSize 15 --Mem/BinDataKept false --Kp/FlannRebalancingFactor 1.0 --RGBD/LinearUpdate 0 --RGBD/ProximityBySpace true --RGBD/OptimizeMaxError 0.5 --FAST/Threshold 7" odom_args:="--Odom/Strategy 0 --Vis/CorType 0 --Odom/KeyFrameThr 0.3 --OdomF2M/MaxSize 2000 --OdomORBSLAM2/VocPath /home/mathieu/workspace/ORB_SLAM2/Vocabulary/ORBvoc.txt --OdomORBSLAM2/Fps 15" rgbd_sync:=true depth_scale:=1.043 frame_id:=base_footprint ground_truth_frame_id:=world ground_truth_base_frame_id:=scan_gt use_sim_time:=true  odom_topic:=odom rgb_topic:=/camera/rgb/image_raw_throttle depth_topic:=/camera/depth/image_raw_throttle camera_info_topic:=/camera/rgb/camera_info_throttle approx_sync:=false database_path:=/media/mathieu/5B60E7B25BDFCB79/bags/rtabmap.db odom_guess_frame_id:=odom_combined
 // use wheel odom
 odom_frame_id:=odom_combined odom_tf_angular_variance:=0.0001 odom_tf_linear_variance:=0.0001 visual_odometry:=false
-// flow 
+// flow
 --Vis/CorType 1 --Odom/KeyFrameThr 0.6 --Vis/BundleAdjustment 0
 // robot_localization
 odom_topic:=/odometry/filtered visual_odometry:=false approx_sync:=true
@@ -1,11 +1,11 @@
 // stereo_odometry
-$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/PublishRAMUsage true --Rtabmap/StartNewMapOnLoopClosure true --Reg/Force3DoF false --RGBD/ProximityPathMaxNeighbors 0 --Mem/STMSize 15 --Mem/BinDataKept false --Kp/FlannRebalancingFactor 1.0 --RGBD/LinearUpdate 0 --RGBD/ProximityBySpace true --Odom/KeyFrameThr 0.3 --Odom/Strategy 0 --OdomF2M/MaxSize 2000 --RGBD/OptimizeMaxError 0.5 --OdomORBSLAM2/VocPath /home/mathieu/workspace/ORB_SLAM2/Vocabulary/ORBvoc.txt --OdomORBSLAM2/Fps 15 --FAST/Threshold 7" odom_args:="--Vis/CorType 0" frame_id:=base_footprint ground_truth_frame_id:=world ground_truth_base_frame_id:=scan_gt use_sim_time:=true stereo:=true stereo_namespace:=/wide_stereo left_camera_info_topic:=/wide_stereo/left/camera_info_throttle right_camera_info_topic:=/wide_stereo/right/camera_info_scaled odom_topic:=odom  approx_sync:=false database_path:=/media/mathieu/5B60E7B25BDFCB79/bags/rtabmap.db odom_guess_frame_id:=odom_combined 
+$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/PublishRAMUsage true --Rtabmap/StartNewMapOnLoopClosure true --Reg/Force3DoF false --RGBD/ProximityPathMaxNeighbors 0 --Mem/STMSize 15 --Mem/BinDataKept false --Kp/FlannRebalancingFactor 1.0 --RGBD/LinearUpdate 0 --RGBD/ProximityBySpace true --Odom/KeyFrameThr 0.3 --Odom/Strategy 0 --OdomF2M/MaxSize 2000 --RGBD/OptimizeMaxError 0.5 --OdomORBSLAM2/VocPath /home/mathieu/workspace/ORB_SLAM2/Vocabulary/ORBvoc.txt --OdomORBSLAM2/Fps 15 --FAST/Threshold 7" odom_args:="--Vis/CorType 0" frame_id:=base_footprint ground_truth_frame_id:=world ground_truth_base_frame_id:=scan_gt use_sim_time:=true stereo:=true stereo_namespace:=/wide_stereo left_camera_info_topic:=/wide_stereo/left/camera_info_throttle right_camera_info_topic:=/wide_stereo/right/camera_info_scaled odom_topic:=odom  approx_sync:=false database_path:=/media/mathieu/5B60E7B25BDFCB79/bags/rtabmap.db odom_guess_frame_id:=odom_combined
 // with wheel odom
 odom_frame_id:=odom_combined odom_tf_angular_variance:=0.0001 odom_tf_linear_variance:=0.0001 visual_odometry:=false
-// flow 
+// flow
 --Vis/CorType 1 --Odom/KeyFrameThr 0.6
-$ ./republish_camera_info.py camera_info_in:=/wide_stereo/right/camera_info_throttle camera_info_out:=/wide_stereo/right/camera_info_scaled 
+$ ./republish_camera_info.py camera_info_in:=/wide_stereo/right/camera_info_throttle camera_info_out:=/wide_stereo/right/camera_info_scaled
 $ export ROS_NAMESPACE=wide_stereo
 $ rosrun stereo_image_proc stereo_image_proc left/image_raw:=left/image_raw_throttle right/image_raw:=right/image_raw_throttle left/camera_info:=left/camera_info_throttle right/camera_info:=right/camera_info_scaled
@@ -7,7 +7,7 @@ index bd9977c..b4f9382 100644
 #include "ros/console.h"
 #include "nav_msgs/MapMetaData.h"
 +#include <nav_msgs/Path.h>
- 
+
 #include "gmapping/sensor/sensor_range/rangesensor.h"
 #include "gmapping/sensor/sensor_odometry/odometrysensor.h"
@@ -258,6 +259,7 @@ void SlamGMapping::startLiveSlam()
@@ -24,12 +24,12 @@ index bd9977c..b4f9382 100644
   sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
 +  pathPub_ = node_.advertise<nav_msgs::Path>("map_path", 1, true);
   ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
-   
+
   rosbag::Bag bag;
@@ -410,6 +413,18 @@ SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
     return false;
   }
- 
+
 +  try
 +    {
 +	  	tf_.lookupTransform(laser_frame_, base_frame_, scan.header.stamp, scan_to_base_);
@@ -47,7 +47,7 @@ index bd9977c..b4f9382 100644
   v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
@@ -617,6 +632,8 @@ SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
     ROS_DEBUG("scan processed");
- 
+
     GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
 +    GMapping::GridSlamProcessor::TNode * node = gsp_->getParticles()[gsp_->getBestParticleIndex()].node;
 +
@@ -56,7 +56,7 @@ index bd9977c..b4f9382 100644
     ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
@@ -699,6 +716,23 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
                                 delta_);
- 
+
   ROS_DEBUG("Trajectory tree:");
 +  nav_msgs::Path path;
 +  int count = 0;
@@ -95,15 +95,15 @@ index bd9977c..b4f9382 100644
@@ -764,8 +805,11 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
   map_.map.header.stamp = ros::Time::now();
   map_.map.header.frame_id = tf_.resolve( map_frame_ );
- 
+
 +  path.header = map_.map.header;
 +
   sst_.publish(map_.map);
   sstm_.publish(map_.map.info);
 +  pathPub_.publish(path);
 }
- 
+
- bool 
+ bool
 diff --git a/gmapping/src/slam_gmapping.h b/gmapping/src/slam_gmapping.h
 index ae622b9..8d84645 100644
 --- a/gmapping/src/slam_gmapping.h
@@ -119,7 +119,7 @@ index ae622b9..8d84645 100644
@@ -92,6 +93,8 @@ class SlamGMapping
     std::string map_frame_;
     std::string odom_frame_;
- 
+
 +    tf::StampedTransform scan_to_base_;
 +
     void updateMap(const sensor_msgs::LaserScan& scan);
@@ -8,19 +8,19 @@ index 712a9ca..0c0d885 100644
     void publishLoop(double transform_publish_period);
 -    void publishGraphVisualization();
 +    void publishGraphVisualization(const ros::Time & stamp);
- 
+
     // ROS handles
     ros::NodeHandle node_;
@@ -435,16 +435,22 @@ SlamKarto::getOdomPose(karto::Pose2& karto_pose, const ros::Time& t)
 }
- 
+
 void
 -SlamKarto::publishGraphVisualization()
 +SlamKarto::publishGraphVisualization(const ros::Time & stamp)
 {
   std::vector<float> graph;
   solver_->getGraph(graph);
- 
+
 +  std::vector<karto::LocalizedRangeScan*> scans = mapper_->GetAllProcessedScans();
 +
 +  if(scans.empty())
@@ -28,7 +28,7 @@ index 712a9ca..0c0d885 100644
 +	  return;
 +  }
   visualization_msgs::MarkerArray marray;
- 
+
   visualization_msgs::Marker m;
   m.header.frame_id = "map";
 -  m.header.stamp = ros::Time::now();
@@ -37,7 +37,7 @@ index 712a9ca..0c0d885 100644
   m.ns = "karto";
   m.type = visualization_msgs::Marker::SPHERE;
@@ -462,7 +468,7 @@ SlamKarto::publishGraphVisualization()
- 
+
   visualization_msgs::Marker edge;
   edge.header.frame_id = "map";
 -  edge.header.stamp = ros::Time::now();
@@ -46,10 +46,10 @@ index 712a9ca..0c0d885 100644
   edge.ns = "karto";
   edge.id = 0;
@@ -477,14 +483,14 @@ SlamKarto::publishGraphVisualization()
- 
+
   m.action = visualization_msgs::Marker::ADD;
   uint id = 0;
-  for (uint i=0; i<graph.size()/2; i++) 
+-  for (uint i=0; i<graph.size()/2; i++)
 +  for (uint i=0; i<scans.size(); i++)
   {
     m.id = id;
@@ -65,7 +65,7 @@ index 712a9ca..0c0d885 100644
     {
       edge.points.clear();
@@ -500,15 +506,15 @@ SlamKarto::publishGraphVisualization()
- 
+
       marray.markers.push_back(visualization_msgs::Marker(edge));
       id++;
 -    }
@@ -74,40 +74,40 @@ index 712a9ca..0c0d885 100644
 -
 +/*
   m.action = visualization_msgs::Marker::DELETE;
-   for (; id < marker_count_; id++) 
+   for (; id < marker_count_; id++)
   {
     m.id = id;
     marray.markers.push_back(visualization_msgs::Marker(m));
 -  }
 +  }*/
- 
+
   marker_count_ = marray.markers.size();
- 
+
@@ -537,12 +543,14 @@ SlamKarto::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
   karto::Pose2 odom_pose;
   if(addScan(laser, scan, odom_pose))
   {
-    ROS_DEBUG("added scan at pose: %.3f %.3f %.3f", 
+-    ROS_DEBUG("added scan at pose: %.3f %.3f %.3f",
-+    ROS_INFO("added scan at pose: %.3f %.3f %.3f", 
+    ROS_INFO("added scan at pose: %.3f %.3f %.3f",
               odom_pose.GetX(),
               odom_pose.GetY(),
               odom_pose.GetHeading());
- 
+
 -    publishGraphVisualization();
 +    publishGraphVisualization(scan->header.stamp);
 +
 +    ROS_INFO("published markers");
- 
+
-     if(!got_map_ || 
+     if(!got_map_ ||
        (scan->header.stamp - last_map_update) > map_update_interval_)
 diff --git a/src/spa_solver.cpp b/src/spa_solver.cpp
 index 5d9a962..6a65211 100644
 --- a/src/spa_solver.cpp
 +++ b/src/spa_solver.cpp
@@ -46,9 +46,9 @@ void SpaSolver::Compute()
- 
+
   typedef std::vector<sba::Node2d, Eigen::aligned_allocator<sba::Node2d> > NodeVector;
- 
+
 -  ROS_INFO("Calling doSPA for loop closure");
 +  //ROS_INFO("Calling doSPA for loop closure");
   m_Spa.doSPA(40);
@@ -1,4 +1,4 @@
-#!/usr/bin/env python  
+#!/usr/bin/env python
 import roslib
 import rospy
 import os
@@ -16,11 +16,11 @@ def callback(data):
    global lastSize
    global slamPosesInd
    global rmse
-  
+
    point_markers = []
    for m in data.markers:
        if m.type==2:
-            point_markers.append(m) 
+            point_markers.append(m)
    if len(point_markers) > 0 and lastSize != len(point_markers):
        t = rospy.Time(point_markers[0].header.stamp.secs, point_markers[0].header.stamp.nsecs)
@@ -1,4 +1,4 @@
-#!/usr/bin/env python  
+#!/usr/bin/env python
 import roslib
 import rospy
 import os
@@ -8,7 +8,7 @@
 		    <remap from="right/image"       to="right/image_raw"/>
 		    <remap from="left/camera_info"  to="left/camera_info"/>
 		    <remap from="right/camera_info" to="right/camera_info"/>
-		    
+
 		    <param name="queue_size" type="int" value="10"/>
 		    <param name="rate" type="double" value="15"/>
 		</node>
@@ -17,14 +17,14 @@
 	<group ns="camera">
 	    <node pkg="nodelet" type="nodelet" name="data_throttle" args="standalone rtabmap_ros/data_throttle">
 	      <param name="rate" type="double" value="15.0"/>
-	   
+
 	      <remap from="rgb/image_in"       to="rgb/image_raw"/>
 	      <remap from="depth/image_in"     to="depth/image_raw"/>
 	      <remap from="rgb/camera_info_in" to="rgb/camera_info"/>
-	    
+
 	      <remap from="rgb/image_out"       to="rgb/image_raw_throttle"/>
 	      <remap from="depth/image_out"     to="depth/image_raw_throttle"/>
 	      <remap from="rgb/camera_info_out" to="rgb/camera_info_throttle"/>
-	    </node>  
+	    </node>
-	</group> 
+	</group>
 </launch>
@@ -2,76 +2,76 @@
 <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 
+           $ roslaunch freenect_launch freenect.launch depth_registration:=true
           $ roslaunch openni2_launch openni2.launch depth_registration:=true -->
- 
+
  <!-- Choose visualization -->
  <arg name="rviz"                    default="false" />
-  <arg name="rtabmapviz"              default="true" /> 
+  <arg name="rtabmapviz"              default="true" />
-  
+
  <!-- Localization-only mode -->
  <arg name="localization"            default="false"/>
-  
+
  <!-- Corresponding config files -->
  <arg name="rtabmapviz_cfg"          default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(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="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="subscribe_scan"          default="false"/>         <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
  <arg name="scan_topic"              default="/scan"/>
-  
+
  <arg name="subscribe_scan_cloud"    default="false"/>         <!-- Assuming 3D scan if set -->
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
-  
+
  <arg name="visual_odometry"         default="true"/>          <!-- Generate visual odometry -->
  <arg name="odom_topic"              default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
  <arg name="odom_frame_id"           default=""/>              <!-- If set, TF is used to get odometry instead of the topic -->
-   
+
  <arg name="namespace"               default="rtabmap"/>
  <arg name="wait_for_transform"      default="0.2"/>
-  
+
  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
-    <arg name="rtabmapviz"              value="$(arg rtabmapviz)" /> 
+    <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)" />
-  
+
    <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="rtabmap_args"            value="$(arg rtabmap_args)"/>
-    <arg name="launch_prefix"           value="$(arg launch_prefix)"/>          
+    <arg name="launch_prefix"           value="$(arg launch_prefix)"/>
    <arg name="approx_sync"             value="$(arg approx_sync)"/>
    <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="compressed"              value="$(arg compressed)"/>
-   
+
    <arg name="subscribe_scan"          value="$(arg subscribe_scan)"/>
    <arg name="scan_topic"              value="$(arg scan_topic)"/>
-  
+
    <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="visual_odometry"         value="$(arg visual_odometry)"/>
-    <arg name="odom_topic"              value="$(arg odom_topic)"/>    
+    <arg name="odom_topic"              value="$(arg odom_topic)"/>
-    <arg name="odom_frame_id"           value="$(arg odom_frame_id)"/>         
+    <arg name="odom_frame_id"           value="$(arg odom_frame_id)"/>
    <arg name="odom_args"               value="$(arg rtabmap_args)"/>
  </include>
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <launch>
-   
+
   <!-- Kinect 2
        Install Kinect2 : Follow ALL directives at https://github.com/code-iai/iai_kinect2
                          Make sure it is calibrated!
@@ -12,30 +12,30 @@
   <!-- Which image resolution to process in rtabmap: sd, qhd, hd -->
   <arg name="resolution" default="qhd" />
-   
+
   <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
   <arg name="frame_id" default="kinect2_base_link"/>
-   
+
   <!-- Rotate the camera -->
   <arg name="pi/2" value="1.5707963267948966"/>
   <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
   <node pkg="tf" type="static_transform_publisher" name="kinect2_base_link"
-        args="$(arg optical_rotate) kinect2_base_link kinect2_link 100" /> 
+        args="$(arg optical_rotate) kinect2_base_link kinect2_link 100" />
-  
+
   <!-- Choose visualization -->
   <arg name="rviz" default="false" />
-   <arg name="rtabmapviz" default="true" /> 
+   <arg name="rtabmapviz" default="true" />
-   
+
   <!-- Corresponding config files -->
   <arg name="rtabmapviz_cfg"          default="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" />
   <arg name="rviz_cfg"                default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
-  
+
   <!-- slightly increase default parameters for larger images (qhd=720p) -->
-   <arg name="gftt_block_size" default="5" /> 
+   <arg name="gftt_block_size" default="5" />
-   <arg name="gftt_min_distance" default="5" /> 
+   <arg name="gftt_min_distance" default="5" />
-        
+
  <group ns="rtabmap">
-  
+
    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen">
      <remap from="rgb/image" to="/kinect2/$(arg resolution)/image_color_rect"/>
@@ -44,41 +44,41 @@
      <param name="frame_id" type="string" value="$(arg frame_id)"/>
      <param name="approx_sync" type="bool" value="false"/>
-	  
+
 	  <param name="GFTT/BlockSize" type="string" value="$(arg gftt_block_size)"/>
      <param name="GFTT/MinDistance" type="string" value="$(arg gftt_min_distance)"/>
    </node>
-  
+
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="subscribe_depth" type="bool" value="true"/>
 	  <param name="frame_id" type="string" value="$(arg frame_id)"/>
-	
+
      <remap from="rgb/image" to="/kinect2/$(arg resolution)/image_color_rect"/>
      <remap from="depth/image" to="/kinect2/$(arg resolution)/image_depth_rect"/>
      <remap from="rgb/camera_info" to="/kinect2/$(arg resolution)/camera_info"/>
      <param name="approx_sync" type="bool" value="false"/>
-      
+
      <param name="GFTT/BlockSize" type="string" value="$(arg gftt_block_size)"/>
      <param name="GFTT/MinDistance" type="string" value="$(arg gftt_min_distance)"/>
    </node>
-  
+
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="$(arg rtabmapviz_cfg)" output="screen">
      <param name="subscribe_depth" type="bool" value="true"/>
      <param name="subscribe_odom_info" type="bool" value="true"/>
      <param name="approx_sync" type="bool" value="false"/>
      <param name="frame_id" type="string" value="$(arg frame_id)"/>
-    
+
      <remap from="rgb/image" to="/kinect2/$(arg resolution)/image_color_rect"/>
      <remap from="depth/image" to="/kinect2/$(arg resolution)/image_depth_rect"/>
      <remap from="rgb/camera_info" to="/kinect2/$(arg resolution)/camera_info"/>
    </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) -->
@@ -91,7 +91,7 @@
    <remap from="odom_in"             to="rtabmap/odom"/>
    <param name="approx_sync" type="bool" value="false"/>
-    
+
    <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"/>
@@ -6,38 +6,38 @@
  <!-- For stereo:=false
        Your RGB-D sensor should be already started with "depth_registration:=true".
        Examples:
-           $ roslaunch freenect_launch freenect.launch depth_registration:=true 
+           $ roslaunch freenect_launch freenect.launch depth_registration:=true
           $ roslaunch openni2_launch openni2.launch depth_registration:=true -->
-           
+
  <!-- For stereo:=true
-        Your camera should be calibrated and publishing rectified left and right 
+        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:
           $ roslaunch rtabmap_ros bumblebee.launch -->
-     
+
-  <!-- Choose between depth and stereo, set both to false to do only scan -->      
+  <!-- Choose between depth and stereo, set both to false to do only scan -->
  <arg name="stereo"                    default="false"/>
  <arg     if="$(arg stereo)" name="depth"  default="false"/>
  <arg unless="$(arg stereo)" name="depth"  default="true"/>
  <arg name="subscribe_rgb"  default="$(arg depth)"/>
- 
+
  <!-- Choose visualization -->
-  <arg name="rtabmapviz"              default="true" /> 
+  <arg name="rtabmapviz"              default="true" />
  <arg name="rviz"                    default="false" />
-  
+
  <!-- Localization-only mode -->
  <arg name="localization"            default="false"/>
  <arg name="initial_pose"            default=""/>             <!-- Format: "x y z roll pitch yaw" or "x y z qx qy qz qw". Default: see "RGBD/StartAtOrigin" doc -->
-  
+
  <!-- sim time for convenience, if playing a rosbag -->
  <arg name="use_sim_time"            default="false"/>
  <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
-  
+
  <!-- Corresponding config files -->
  <arg name="cfg"                     default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(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="odom_frame_id"           default=""/>                <!-- If set, TF is used to get odometry instead of the topic -->
  <arg name="odom_frame_id_init"      default=""/>                <!-- If set, TF map->odom is published even if no odometry topic has been received yet. The frame id should match the one in the topic. -->
@@ -59,22 +59,22 @@
  <!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
  <arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
-  <arg unless="$(arg stereo)" name="approx_sync"  default="$(arg depth)"/>     
+  <arg unless="$(arg stereo)" name="approx_sync"  default="$(arg depth)"/>
  <arg name="approx_sync_max_interval"  default="0"/> <!-- (sec) 0 means infinite interval duration (used with approx_sync=true) -->
-   
+
  <!-- RGB-D related topics -->
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />
-  
+
  <!-- stereo related topics -->
  <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" />
-  
+
  <!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet -->
  <arg name="rgbd_sync"               default="false"/>         <!-- pre-sync rgb_topic, depth_topic, camera_info_topic -->
  <arg name="approx_rgbd_sync"        default="true"/>          <!-- false=exact synchronization -->
@@ -83,15 +83,15 @@
  <arg name="depth_scale"             default="1.0" />         <!-- Deprecated, use rgbd_depth_scale instead -->
  <arg name="rgbd_depth_scale"        default="$(arg depth_scale)" />
  <arg name="rgbd_decimation"         default="1" />
-  
+
  <arg name="compressed"              default="false"/>         <!-- If you want to subscribe to compressed image topics -->
  <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
  <arg name="depth_image_transport"   default="compressedDepth"/>  <!-- Depth compatible types: compressedDepth (see "rosrun image_transport list_transports") -->
-  
+
  <arg name="gen_cloud"               default="false"/> <!-- only works with depth image and if not subscribing to scan_cloud topic-->
  <arg name="gen_cloud_decimation"    default="4"/>
  <arg name="gen_cloud_voxel"         default="0.05"/>
-   
+
  <arg name="subscribe_scan"          default="false"/>
  <arg name="scan_topic"              default="/scan"/>
  <arg name="subscribe_scan_cloud"    default="$(arg gen_cloud)"/>
@@ -103,13 +103,13 @@
  <arg name="scan_cloud_max_points"   default="0"/>
  <arg name="scan_cloud_filtered"     default="$(arg scan_deskewing)"/> <!-- use filtered cloud from icp_odometry for mapping -->
  <arg name="gen_scan"                default="false"/> <!-- only works with depth image and if not subscribing to scan topic-->
-  
+
  <arg name="gen_depth"                  default="false" /> <!-- Generate depth image from scan_cloud -->
  <arg name="gen_depth_decimation"       default="1" />
  <arg name="gen_depth_fill_holes_size"  default="0" />
  <arg name="gen_depth_fill_iterations"  default="1" />
  <arg name="gen_depth_fill_holes_error" default="0.1" />
-   
+
  <arg name="visual_odometry"          default="true"/>          <!-- Launch rtabmap visual odometry node -->
  <arg name="icp_odometry"             default="false"/>         <!-- Launch rtabmap icp odometry node -->
  <arg name="odom_topic"               default="odom"/>          <!-- Odometry topic name -->
@@ -127,7 +127,7 @@
  <arg name="imu_topic"                default="/imu/data"/>          <!-- only used with VIO approaches -->
  <arg name="wait_imu_to_init"         default="false"/>
  <arg name="use_odom_features"        default="false"/>
-  
+
  <arg name="scan_cloud_assembling"              default="false"/>
  <arg name="scan_cloud_assembling_time"         default="1"/>      <!-- max_clouds and time should not be set at the same time -->
  <arg name="scan_cloud_assembling_max_clouds"   default="0"/>      <!-- max_clouds and time should not be set at the same time -->
@@ -137,18 +137,18 @@
  <arg name="scan_cloud_assembling_range_max"    default="0.0"/>    <!-- 0=disabled -->
  <arg name="scan_cloud_assembling_noise_radius"   default="0.0"/>    <!-- 0=disabled -->
  <arg name="scan_cloud_assembling_noise_min_neighbors"   default="5"/>
-  
+
  <arg name="subscribe_user_data"      default="false"/>             <!-- user data synchronized subscription -->
  <arg name="user_data_topic"          default="/user_data"/>
  <arg name="user_data_async_topic"    default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->
-  
+
  <arg name="gps_topic"                default="/gps/fix" />         <!-- gps async subscription -->
-  
+
  <arg name="tag_topic"                default="/tag_detections" />  <!-- apriltags async subscription -->
  <arg name="tag_linear_variance"      default="0.0001" />
  <arg name="tag_angular_variance"     default="9999" />             <!-- >=9999 means ignore rotation in optimization, when rotation estimation of the tag is not reliable -->
  <arg name="fiducial_topic"           default="/fiducial_transforms" />  <!-- aruco_detect async subscription, use tag_linear_variance and tag_angular_variance to set covriance -->
-  
+
  <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
  <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
  <arg unless="$(arg compressed)" name="rgb_topic_relay"           default="$(arg rgb_topic)"/>
@@ -163,7 +163,7 @@
  <!-- Nodes -->
  <group ns="$(arg namespace)">
-     
+
    <!-- relays -->
    <group if="$(arg depth)">
      <group unless="$(arg subscribe_rgbd)">
@@ -206,7 +206,7 @@
        </node>
      </group>
    </group>
-    
+
    <group unless="$(arg rgbd_sync)">
       <group if="$(arg subscribe_rgbd)">
         <node name="republish_rgbd_image"  type="rgbd_relay" pkg="rtabmap_ros" clear_params="$(arg clear_params)">
@@ -217,7 +217,7 @@
         </node>
      </group>
    </group>
-    
+
    <node if="$(arg gen_cloud)" pkg="nodelet" type="nodelet" name="gen_cloud_from_depth" args="standalone rtabmap_ros/point_cloud_xyz" clear_params="$(arg clear_params)" output="$(arg output)">
      <remap from="depth/image"       to="$(arg depth_topic_relay)"/>
      <remap from="depth/camera_info" to="$(arg camera_info_topic)"/>
@@ -232,7 +232,7 @@
    <!-- Visual odometry -->
    <group unless="$(arg icp_odometry)">
      <group if="$(arg visual_odometry)">
-      
+
        <!-- RGB-D Odometry -->
        <node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
          <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
@@ -241,7 +241,7 @@
          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
          <remap from="odom"            to="$(arg odom_topic)"/>
          <remap from="imu"             to="$(arg imu_topic)"/>
-      
+
          <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
          <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
          <param name="publish_tf"                  type="bool"   value="$(arg publish_tf_odom)"/>
@@ -271,7 +271,7 @@
          <remap from="rgbd_image"             to="$(arg rgbd_topic_relay)"/>
          <remap from="odom"                   to="$(arg odom_topic)"/>
          <remap from="imu"                    to="$(arg imu_topic)"/>
-      
+
          <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
          <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
          <param name="publish_tf"                  type="bool"   value="$(arg publish_tf_odom)"/>
@@ -293,14 +293,14 @@
        </node>
      </group>
    </group>
-    
+
    <!-- ICP Odometry -->
    <node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>
-      
+
      <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
      <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
      <param name="publish_tf"                  type="bool"   value="$(arg publish_tf_odom)"/>
@@ -319,7 +319,7 @@
      <param name="deskewing"                   type="bool"   value="$(arg scan_deskewing)"/>
      <param name="deskewing_slerp"             type="bool"   value="$(arg scan_deskewing_slerp)"/>
    </node>
-    
+
    <node if="$(eval not icp_odometry and scan_deskewing and subscribe_scan_cloud)" pkg="rtabmap_ros" type="lidar_deskewing" name="lidar_deskewing" clear_params="$(arg clear_params)" output="$(arg output)">
      <param name="wait_for_transform" value="$(arg wait_for_transform)"/>
      <param     if="$(arg visual_odometry)" name="fixed_frame_id" value="$(arg vo_frame_id)"/>
@@ -328,7 +328,7 @@
      <remap from="input_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="$(arg scan_cloud_topic)/deskewed" to="odom_filtered_input_scan"/>
    </node>
-    
+
    <node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" clear_params="$(arg clear_params)" output="$(arg output)">
      <remap     if="$(arg scan_cloud_filtered)" from="cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(arg scan_cloud_filtered)" from="cloud" to="$(arg scan_cloud_topic)"/>
@@ -344,7 +344,7 @@
      <param name="noise_min_neighbors" type="int"    value="$(arg scan_cloud_assembling_noise_min_neighbors)"/>
      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
    </node>
-  
+
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
@@ -378,25 +378,25 @@
      <param name="queue_size"           type="int" value="$(arg queue_size)"/>
      <param if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" name="scan_cloud_max_points" type="int" value="$(arg scan_cloud_max_points)"/>
      <param name="landmark_linear_variance"   type="double" value="$(arg tag_linear_variance)"/>
-      <param name="landmark_angular_variance"  type="double" value="$(arg tag_angular_variance)"/> 
+      <param name="landmark_angular_variance"  type="double" value="$(arg tag_angular_variance)"/>
      <param name="gen_depth"                  type="bool"   value="$(arg gen_depth)" />
      <param name="gen_depth_decimation"       type="int"    value="$(arg gen_depth_decimation)" />
      <param name="gen_depth_fill_holes_size"  type="int"    value="$(arg gen_depth_fill_holes_size)" />
      <param name="gen_depth_fill_iterations"  type="int"    value="$(arg gen_depth_fill_iterations)" />
      <param name="gen_depth_fill_holes_error" type="double" value="$(arg gen_depth_fill_holes_error)" />
-      
+
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
-      
+
      <remap     if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
      <remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
-	
+
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <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 if="$(eval scan_cloud_assembling)" from="scan_cloud" to="assembled_cloud"/>
      <remap if="$(eval scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="odom_filtered_input_scan"/>
@@ -409,13 +409,13 @@
      <remap from="fiducial_transforms"    to="$(arg fiducial_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>
-      
+
      <!-- 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)"/>
    </node>
-  
+
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" clear_params="$(arg clear_params)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
      <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
@@ -434,28 +434,28 @@
      <param name="wait_for_transform_duration" type="double"   value="$(arg wait_for_transform)"/>
      <param name="queue_size"           type="int"    value="$(arg queue_size)"/>
      <param name="approx_sync"          type="bool"   value="$(eval approx_sync and not use_odom_features)"/>
-    
+
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
-      
+
      <remap     if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
      <remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
-      
+
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
-     
+
      <remap unless="$(arg icp_odometry)" from="scan" to="$(arg scan_topic)"/>
      <remap     if="$(eval icp_odometry or scan_cloud_filtered)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(eval icp_odometry or scan_cloud_filtered)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="scan_descriptor"        to="$(arg scan_descriptor_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
    </node>
-  
+
  </group>
-  
+
  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" clear_params="$(arg clear_params)" output="$(arg output)">
@@ -3,62 +3,62 @@
 <launch>
   <!-- Backward compatibility launch file, use "rtabmap.launch rgbd:=false stereo:=true" instead -->
-   <!-- Your camera should be calibrated and publishing rectified left and right 
+   <!-- 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:
           $ roslaunch rtabmap_ros bumblebee.launch -->
- 
+
  <!-- Choose visualization -->
-  <arg name="rtabmapviz"              default="true" /> 
+  <arg name="rtabmapviz"              default="true" />
  <arg name="rviz"                    default="false" />
-  
+
  <!-- Localization-only mode -->
  <arg name="localization"            default="false"/>
-  
+
  <!-- Corresponding config files -->
  <arg name="rtabmapviz_cfg"          default="$(find rtabmap_ros)/launch/config/rgbd_gui.ini" />
  <arg name="rviz_cfg"                default="$(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="database_path"           default="~/.ros/rtabmap.db"/>
  <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="compressed"              default="false"/>
-   
+
  <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"/>
-   
+
  <arg name="subscribe_scan_cloud"    default="false"/>         <!-- Assuming 3D scan if set -->
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
-  
+
  <arg name="visual_odometry"         default="true"/>          <!-- Generate visual odometry -->
  <arg name="odom_topic"              default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
  <arg name="odom_frame_id"           default=""/>              <!-- If set, TF is used to get odometry instead of the topic -->
-  
+
  <arg name="namespace"               default="rtabmap"/>
  <arg name="wait_for_transform"      default="0.2"/>
-  
+
  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
    <arg name="stereo"                  value="true"/>
-    <arg name="rtabmapviz"              value="$(arg rtabmapviz)" /> 
+    <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)" />
-  
+
    <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="rtabmap_args"            value="$(arg rtabmap_args)"/>
-    <arg name="launch_prefix"           value="$(arg launch_prefix)"/>          
+    <arg name="launch_prefix"           value="$(arg launch_prefix)"/>
    <arg name="approx_sync"             value="$(arg approx_sync)"/>
    <arg name="stereo_namespace"        value="$(arg stereo_namespace)"/>
@@ -66,18 +66,18 @@
    <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)" />
-  
+
-    <arg name="compressed"              value="$(arg compressed)"/>                                                                                
+    <arg name="compressed"              value="$(arg compressed)"/>
-   
+
    <arg name="subscribe_scan"          value="$(arg subscribe_scan)"/>
    <arg name="scan_topic"              value="$(arg scan_topic)"/>
-  
+
    <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="visual_odometry"         value="$(arg visual_odometry)"/>
-    <arg name="odom_topic"              value="$(arg odom_topic)"/>       
+    <arg name="odom_topic"              value="$(arg odom_topic)"/>
-    <arg name="odom_frame_id"           value="$(arg odom_frame_id)"/>   
+    <arg name="odom_frame_id"           value="$(arg odom_frame_id)"/>
    <arg name="odom_args"               value="$(arg rtabmap_args)"/>
  </include>
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <launch>
-    
+
    <node pkg="camera1394stereo" type="camera1394stereo_node" name="camera1394stereo_node" output="screen" >
 	    <param name="video_mode" value="format7_mode3" />
 	    <param name="format7_color_coding" value="raw16" />
@@ -10,13 +10,13 @@
 	    <param name="camera_info_url_left" value="" />
 	    <param name="camera_info_url_right" value="" />
  	</node>
-  	
+
 	<arg name="gen_depth" default="false"/>
  	<arg name="pi/2" value="1.5707963267948966" />
    	<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
  	<node pkg="tf" type="static_transform_publisher" name="camera_base_link"
-        args="$(arg optical_rotate) base_link stereo_camera 100" />  
+        args="$(arg optical_rotate) base_link stereo_camera 100" />
-  	
+
  	<!-- Run the ROS package stereo_image_proc (throttle to 10 Hz to avoid rectifying all images) -->
 	<group ns="/stereo_camera" >
 	    	<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="standalone rtabmap_ros/stereo_throttle">
@@ -24,11 +24,11 @@
 		    <remap from="right/image"       to="right/image_raw"/>
 		    <remap from="left/camera_info"  to="left/camera_info"/>
 		    <remap from="right/camera_info" to="right/camera_info"/>
-		    
+
 		    <param name="queue_size" type="int" value="10"/>
 		    <param name="rate" type="double" value="10"/>
 		</node>
-		
+
 		<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
 			<remap from="left/image_raw"    to="left/image_raw_throttle"/>
 			<remap from="left/camera_info"  to="left/camera_info_throttle"/>
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <launch>
-<!-- 
+<!--
 Examples:
   F2M (default VO):
      $ roslaunch rtabmap_ros euroc_datasets.launch
@@ -73,10 +73,10 @@ Examples:
 <!-- TF frames -->
 <node pkg="tf" type="static_transform_publisher" name="imu_base_link" args="0 0 0 3.1415926 -1.570796 0 base_link imu4 5"/>
 <node pkg="tf" type="static_transform_publisher" name="cam0_imu_link" args="-0.021640 -0.064677 0.009811 1.555925 0.025777 0.003757 imu4 cam0 50"/>
-<node pkg="tf" type="static_transform_publisher" name="cam1_imu_link" args="-0.019844 0.045369 0.007862 1.558237 0.025393 0.017907 imu4 cam1 50"/> 
+<node pkg="tf" type="static_transform_publisher" name="cam1_imu_link" args="-0.019844 0.045369 0.007862 1.558237 0.025393 0.017907 imu4 cam1 50"/>
 <!-- For MH sequences, /leica/position doesn't give the orientation, so minimal ground truth error could be as high as 12 cm -->
-<node     if="$(arg MH_seq)" pkg="tf" type="static_transform_publisher" name="leica_base_link" args="0.120209 -0.0184772 -0.0748903 0 0 0 leica base_link_gt 100"/> 
+<node     if="$(arg MH_seq)" pkg="tf" type="static_transform_publisher" name="leica_base_link" args="0.120209 -0.0184772 -0.0748903 0 0 0 leica base_link_gt 100"/>
 <node unless="$(arg MH_seq)" pkg="tf" type="static_transform_publisher" name="vicon_base_link" args="0.12395 -0.02781 -0.06901 0 0 0 vicon/firefly_sbx/firefly_sbx base_link_gt 100"/>
 <node     if="$(arg MH_seq)" pkg="rtabmap_ros" type="point_to_tf.py" name="point_to_tf">
@@ -96,7 +96,7 @@ Examples:
   <param name="use_mag" value="false"/>
   <param name="world_frame" value="enu"/>
   <param name="publish_tf" value="false"/>
-</node> 
+</node>
 <!-- RTAB-Map -->
 <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <launch>
-   
+
   <!-- Example to run rgbd datasets:
   $ wget http://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.bag
   $ rosbag decompress rgbd_dataset_freiburg3_long_office_household.bag
@@ -11,28 +11,28 @@
   $ roslaunch rtabmap_ros rgbdslam_datasets.launch
   $ rosbag play -.-clock rgbd_dataset_freiburg3_long_office_household.bag
   -->
-   
+
   <param name="use_sim_time" type="bool" value="True"/>
-     
+
   <!-- Choose visualization -->
   <arg name="rviz" default="true" />
-   <arg name="rtabmapviz" default="false" /> 
+   <arg name="rtabmapviz" default="false" />
- 
+
  <!-- TF FRAMES -->
-  <node pkg="tf" type="static_transform_publisher" name="world_to_map" 
+  <node pkg="tf" type="static_transform_publisher" name="world_to_map"
    args="0.0 0.0 0.0 0.0 0.0 0.0 /world /map 100" />
-    
+
-      
+
  <group ns="rtabmap">
-  
+
    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
      <remap from="rgb/image"       to="/camera/rgb/image_color"/>
      <remap from="depth/image"     to="/camera/depth/image"/>
      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
-	  
+
      <param name="Odom/Strategy" type="string" value="0"/>      <!-- 0=Frame-to-Map, 1=Frame-to-KeyFrame -->
-      <param name="Odom/ResetCountdown" type="string" value="15"/> 
+      <param name="Odom/ResetCountdown" type="string" value="15"/>
      <param name="Odom/GuessSmoothingDelay" type="string" value="0"/>
      <param name="frame_id" type="string" value="kinect"/>
@@ -41,12 +41,12 @@
      <param name="ground_truth_frame_id" type="string" value="world"/>
      <param name="ground_truth_base_frame_id" type="string" value="kinect_gt"/>
    </node>
-    
+
    <!-- Visual SLAM -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="subscribe_depth" type="bool" value="true"/>
-     
+
      <param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="true"/>
      <param name="RGBD/CreateOccupancyGrid" type="string" value="false"/>
      <param name="Rtabmap/CreateIntermediateNodes" type="string" value="true"/>
@@ -54,38 +54,38 @@
      <param name="RGBD/AngularUpdate" type="string" value="0"/>
      <param name="frame_id" type="string" value="kinect"/>
-      <param name="ground_truth_frame_id" type="string" value="world"/> 
+      <param name="ground_truth_frame_id" type="string" value="world"/>
      <param name="ground_truth_base_frame_id" type="string" value="kinect_gt"/>
-	
+
      <remap from="rgb/image" to="/camera/rgb/image_color"/>
      <remap from="depth/image" to="/camera/depth/image"/>
      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
      <param name="queue_size" type="int" value="10"/>
    </node>
-    
+
    <!-- Visualisation  -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
      <param name="subscribe_depth" type="bool" value="true"/>
      <param name="subscribe_odom_info" type="bool" value="true"/>
      <param name="queue_size" type="int" value="30"/>
-      
+
      <param name="frame_id" type="string" value="kinect"/>
-    
+
      <remap from="rgb/image" to="/camera/rgb/image_color"/>
      <remap from="depth/image" to="/camera/depth/image"/>
      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
    </node>
-        
+
  </group>
-  
+
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbdslam_datasets.rviz"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
    <remap from="rgb/image"       to="/camera/rgb/image_color"/>
    <remap from="depth/image"     to="/camera/depth/image"/>
    <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
    <remap from="cloud"           to="voxel_cloud" />
-    
+
    <param name="queue_size" type="int" value="10"/>
    <param name="decimation" type="double" value="4"/>
  </node>
@@ -1,10 +1,10 @@
 <?xml version="1.0"?>
-<launch>  
+<launch>
-  <!-- This launch assumes that you have already 
+  <!-- This launch assumes that you have already
       started you preferred RGB-D sensor and your IMU.
       TF between frame_id and the sensors should already be set too. -->
-       
+
  <arg name="frame_id"                default="base_link" />
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
@@ -16,8 +16,8 @@
  <!-- Localization-only mode -->
  <arg name="localization"      default="false"/>
  <arg     if="$(arg localization)" name="rtabmap_args"  default=""/>
-  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>  
+  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>
-  
+
  <group ns="rtabmap">
    <!-- Visual Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen" args="$(arg rtabmap_args)">
@@ -33,26 +33,26 @@
      <param name="Odom/FillInfoData"      type="string" value="true"/>
      <param name="Odom/ResetCountdown"    type="string" value="1"/>
-      <param name="Vis/FeatureType"        type="string" value="6"/> 
+      <param name="Vis/FeatureType"        type="string" value="6"/>
      <param name="OdomF2M/MaxSize"        type="string" value="1000"/>
    </node>
    <!-- SLAM -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
      <param name="frame_id"        type="string" value="$(arg frame_id)"/>
- 
+
      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="odom"            to="/odometry/filtered"/>
-      
+
      <param name="Kp/DetectorStrategy"    type="string" value="6"/> <!-- use same features as odom -->
      <!-- localization mode -->
-      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/> 
+      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
-      
+
    </node>
  </group>
@@ -70,7 +70,7 @@
      <param name="transform_time_offset" value="0.0"/>
      <param name="odom0" value="/vo"/>
-      <param name="imu0" value="$(arg imu_topic)"/> 
+      <param name="imu0" value="$(arg imu_topic)"/>
      <!-- The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
      <rosparam param="odom0_config">[true, true, true,
@@ -90,8 +90,8 @@
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
-                                     true,  true,  true] </rosparam>  
+                                     true,  true,  true] </rosparam>
-      
+
      <param name="odom0_differential" value="false"/>
      <param name="imu0_differential" value="false"/>
@@ -104,7 +104,7 @@
      <!-- ======== ADVANCED PARAMETERS ======== -->
      <param name="odom0_queue_size" value="5"/>
-      <param name="imu0_queue_size" value="50"/> 
+      <param name="imu0_queue_size" value="50"/>
      <!-- The values are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz,
           vroll, vpitch, vyaw, ax, ay, az. -->
@@ -9,7 +9,7 @@
    <arg name="depth_registration" value="true" />
    <arg name="publish_tf" value="false" />
  </include>
-  
+
  <!-- IMU Sensor: -->
  <node pkg="imu_brick" type="imu_brick_node" name="imu_brick">
      <param name="frame_id"                       value="imu_link"/>
@@ -20,20 +20,20 @@
      <param name="cov_acceleration" type="double" value="0.1"/>
      <param name="remove_gravitational_acceleration" type="bool" value="true"/>
   </node>
-      
+
  <!-- IMU frame: just over the RGB camera -->
  <node pkg="tf" type="static_transform_publisher" name="rgb_to_imu_tf"
      args="-0.032 0.0 0.032 0.0 0.0 0.0 /camera_rgb_frame /imu_link 100" />
-      
+
  <arg name="pi/2" value="1.5707963267948966" />
   	<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
  <node pkg="tf" type="static_transform_publisher" name="optical_rotation"
-        args="$(arg optical_rotate) /camera_rgb_frame /camera_rgb_optical_frame 100" />  
+        args="$(arg optical_rotate) /camera_rgb_frame /camera_rgb_optical_frame 100" />
  <include file="$(find rtabmap_ros)/launch/tests/sensor_fusion.launch">
      <arg name="frame_id" value="camera_rgb_frame"/>
      <arg name="localization" value="$(arg localization)"/>
      <arg name="imu_remove_gravitational_acceleration" value="false"/>
  </include>
-  
+
 </launch>
@@ -47,7 +47,7 @@ tag_bundles:
       name: 'tag_bundle1',
       layout:
         [
-           # for a pixel = ~8mm 
+           # for a pixel = ~8mm
           {id: 1,   size: 0.0620, x: 0.0000, y:  0.0000, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
           {id: 2,   size: 0.0620, x: 0.0770, y:  0.0000, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
           {id: 25,  size: 0.0620, x: 0.0000, y: -0.0770, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
@@ -16,7 +16,7 @@
  <!-- Set parameters -->
  <rosparam command="load" file="$(find rtabmap_ros)/launch/tests/tag_settings.yaml" ns="apriltag_ros_continuous_node" />
  <rosparam command="load" file="$(find rtabmap_ros)/launch/tests/tags.yaml" ns="apriltag_ros_continuous_node" />
-  
+
  <node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="apriltag_ros_continuous_node" clear_params="true" output="screen">
    <remap from="image_rect" to="$(arg rgb_topic)" />
    <remap from="camera_info" to="$(arg camera_info_topic)" />
@@ -2,7 +2,7 @@
 <launch>
 <!-- Example usage of RTAB-Map with VINS-Fusion support for realsense D435i.
-     Make sure to disable the IR emitter or put a tape on the IR emitter to 
+     Make sure to disable the IR emitter or put a tape on the IR emitter to
     avoid VINS tracking the fixed IR points (that would cause large drifts) -->
 <arg name="rtabmapviz" default="true"/>
@@ -13,7 +13,7 @@
 <include file="$(find realsense2_camera)/launch/rs_camera.launch">
   <arg name="align_depth"   value="$(arg depth_mode)"/>
-   <arg name="unite_imu_method" value="$(arg unite_imu_method)"/> 
+   <arg name="unite_imu_method" value="$(arg unite_imu_method)"/>
   <arg name="enable_gyro"   value="true"/>
   <arg name="enable_accel"  value="true"/>
   <arg name="enable_infra1" value="true"/>
@@ -3,17 +3,17 @@
 <launch>
  <!-- We test here ICP odometry using a guess from visual odometry -->
-  
+
  <arg name="rgbd"    default="false"/>
  <arg name="pm"      default="false"/>
  <arg name="nodelet" default="false"/>
-   
+
  <include file="$(find freenect_launch)/launch/freenect.launch" >
    <arg name="depth_registration" value="true"/>
    <arg name="data_skip" value="3"/>
  </include>
-  
+
-  <group ns="camera"> 
+  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="points_xyz" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
      <remap from="depth/image"       to="depth_registered/image_raw"/>
      <remap from="depth/camera_info" to="depth_registered/camera_info"/>
@@ -21,7 +21,7 @@
      <param name="voxel_size" type="double" value="0.05"/>
      <param name="decimation" type="int" value="8"/>
-      
+
      <param name="Odom/AlignWithGround"  type="string" value="true"/>
    </node>
@@ -31,11 +31,11 @@
        <remap from="depth/image"       to="depth_registered/image_raw"/>
        <remap from="rgb/camera_info"   to="rgb/camera_info"/>
        <remap from="rgb/image"         to="rgb/image_rect_mono"/>
-	  
+
-        <param name="frame_id"        type="string" value="camera_link"/>   
+        <param name="frame_id"        type="string" value="camera_link"/>
        <param name="scan_normal_k"   type="int" value="10"/>
-        <param name="subscribe_scan_cloud" type="bool" value="true"/>   
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
-     
+
        <param name="Icp/PointToPlane"  type="string" value="true"/>
        <param name="Icp/VoxelSize"     type="string" value="0"/>
        <param name="Icp/PM"            type="string" value="$(arg pm)"/>
@@ -43,10 +43,10 @@
      </node>
      <node unless="$(arg rgbd)" pkg="nodelet" type="nodelet" name="icp_odometry" args="load rtabmap_ros/icp_odometry camera_nodelet_manager" output="screen">
        <remap from="scan_cloud"      to="/voxel_cloud"/>
-	  
+
-        <param name="frame_id"        type="string" value="camera_link"/>   
+        <param name="frame_id"        type="string" value="camera_link"/>
-        <param name="scan_normal_k"   type="int" value="10"/>   
+        <param name="scan_normal_k"   type="int" value="10"/>
-       
+
        <param name="Icp/PointToPlane"  type="string" value="true"/>
        <param name="Icp/VoxelSize"     type="string" value="0"/>
        <param name="Icp/PM"            type="string" value="$(arg pm)"/>
@@ -55,18 +55,18 @@
        <param name="Odom/ResetCountdown"  type="string" value="1"/>
      </node>
    </group>
-    
+
    <group unless="$(arg nodelet)">
      <node if="$(arg rgbd)" pkg="rtabmap_ros" type="rgbdicp_odometry" name="rgbdicp_odometry" output="screen">
        <remap from="scan_cloud"        to="/voxel_cloud"/>
        <remap from="depth/image"       to="depth_registered/image_raw"/>
        <remap from="rgb/camera_info"   to="rgb/camera_info"/>
        <remap from="rgb/image"         to="rgb/image_rect_mono"/>
-	  
+
-        <param name="frame_id"        type="string" value="camera_link"/>   
+        <param name="frame_id"        type="string" value="camera_link"/>
        <param name="scan_normal_k"   type="int" value="10"/>
-        <param name="subscribe_scan_cloud" type="bool" value="true"/>   
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
-     
+
        <param name="Icp/PointToPlane"  type="string" value="true"/>
        <param name="Icp/VoxelSize"     type="string" value="0"/>
        <param name="Icp/PM"            type="string" value="$(arg pm)"/>
@@ -74,10 +74,10 @@
      </node>
      <node unless="$(arg rgbd)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
        <remap from="scan_cloud"      to="/voxel_cloud"/>
-	  
+
-        <param name="frame_id"        type="string" value="camera_link"/>   
+        <param name="frame_id"        type="string" value="camera_link"/>
-        <param name="scan_normal_k"   type="int" value="10"/>   
+        <param name="scan_normal_k"   type="int" value="10"/>
-     
+
        <param name="Icp/PointToPlane"  type="string" value="true"/>
        <param name="Icp/VoxelSize"     type="string" value="0"/>
        <param name="Icp/PM"            type="string" value="$(arg pm)"/>
@@ -88,12 +88,12 @@
    </group>
  </group>
-  
+
-  <!-- We just use odometry without rtabmap node, so set a static /map->/odom 
+  <!-- We just use odometry without rtabmap node, so set a static /map->/odom
       transform so that rviz config below works out-of-the-box -->
  <node pkg="tf" type="static_transform_publisher" name="map_odom"
-        args="0 0 0 0 0 0 map odom 100" />  
+        args="0 0 0 0 0 0 map odom 100" />
-  
+
  <!-- Visualization RVIZ -->
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
 </launch>
@@ -1,16 +1,16 @@
 <?xml version="1.0"?>
 <launch>
-  
+
    <arg name="rtabmapviz"    default="true"/>
-   
+
    <include file="$(find azure_kinect_ros_driver)/launch/driver.launch">
       <arg name="point_cloud" value="false"/>
       <arg name="rgb_point_cloud" value="false"/>
       <arg name="fps" value="15"/>
    </include>
-   
+
    <group ns="rtabmap">
-    
+
      <node pkg="nodelet" type="nodelet" name="points_xyz" args="standalone rtabmap_ros/point_cloud_xyz" output="screen">
        <remap from="depth/image"       to="/depth_to_rgb/image_raw"/>
        <remap from="depth/camera_info" to="/depth_to_rgb/camera_info"/>
@@ -22,38 +22,38 @@
        <param name="normal_k"    type="int"    value="10"/>
        <param name="approx_sync" type="bool"   value="false"/>
      </node>
-    
+
      <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
        <remap from="scan_cloud" to="voxel_cloud"/>
-        <param name="frame_id" type="string" value="camera_base"/>  
+        <param name="frame_id" type="string" value="camera_base"/>
        <!-- ICP parameters -->
        <param name="Icp/PointToPlane"        type="string" value="true"/>
-        <param name="Icp/PM"                  type="string" value="true"/> 
+        <param name="Icp/PM"                  type="string" value="true"/>
-        <param name="Icp/PMOutlierRatio"      type="string" value="0.7"/>  
+        <param name="Icp/PMOutlierRatio"      type="string" value="0.7"/>
-        <!-- Odom parameters -->       
+        <!-- Odom parameters -->
        <param name="OdomF2M/ScanSubtractRadius" type="string" value="0.05"/>
-        <param name="OdomF2M/ScanMaxSize"        type="string" value="5000"/>      
+        <param name="OdomF2M/ScanMaxSize"        type="string" value="5000"/>
      </node>
-      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">	  
+      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
-        <param name="frame_id"             type="string" value="camera_base"/>  
+        <param name="frame_id"             type="string" value="camera_base"/>
        <param name="subscribe_depth"      type="bool" value="false"/>
        <param name="subscribe_rgb"        type="bool" value="false"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <param name="approx_sync"          type="bool" value="false"/>
-        
+
        <remap from="scan_cloud" to="voxel_cloud"/>
-     
+
-        <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/> 
+        <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/>
-        <param name="RGBD/ProximityOdomGuess"        type="string" value="true"/> 
+        <param name="RGBD/ProximityOdomGuess"        type="string" value="true"/>
-          
+
        <!-- ICP parameters -->
        <param name="Icp/PointToPlane"        type="string" value="true"/>
-        <param name="Icp/PM"                  type="string" value="true"/> 
+        <param name="Icp/PM"                  type="string" value="true"/>
        <param name="Icp/PMOutlierRatio"      type="string" value="0.7"/>
-        <param name="Icp/MaxTranslation"      type="string" value="0.5"/>         
+        <param name="Icp/MaxTranslation"      type="string" value="0.5"/>
      </node>
      <node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
@@ -40,7 +40,7 @@
    <arg name="rgb_topic"               value="$(arg rgb_topic)" />
    <arg name="depth_topic"             value="$(arg depth_topic)" />
    <arg name="camera_info_topic"       value="$(arg camera_info_topic)" />
-                                                                                
+
    <arg name="subscribe_scan_cloud"    value="true"/>
    <arg name="scan_cloud_topic"        value="/assembled_cloud2"/>
@@ -4,12 +4,12 @@
        $ roslaunch rtabmap_ros demo_robot_mapping.launch rviz:=true rtabmapviz:=false
        $ roslaunch rtabmap_ros test_map_assembler.launch
        $ rosbag play -clock demo_mapping.bag
-  -->        
+  -->
- 
+
  <group ns="rtabmap">
     <node pkg="rtabmap_ros" type="map_assembler" name="map_assembler">
        <remap from="mapData" to="mapData"/>
        <param name="regenerate_local_grids" value="true"/>
     </node>
-  </group>  
+  </group>
 </launch>
@@ -2,24 +2,24 @@
 <launch>
   <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
     <arg name="rtabmap_args" value="
-                                     --delete_db_on_start 
+                                     --delete_db_on_start
-                                     --RGBD/OptimizeMaxError 0 
+                                     --RGBD/OptimizeMaxError 0
                                     --Optimizer/Iterations 0
                                     --RGBD/ProximityBySpace false"/>
     <arg name="rtabmapviz" value="false"/>
  </include>
-  
+
  <group ns="rtabmap">
     <node pkg="rtabmap_ros" type="map_optimizer" name="map_optimizer"/>
-     
+
     <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
        <remap from="mapData" to="mapData_optimized"/>
        <param name="frame_id" value="camera_link"/>
        <param name="subscribe_depth" value="false"/>
     </node>
-     
+
     <node pkg="rtabmap_ros" type="map_assembler" name="map_assembler">
        <remap from="mapData" to="mapData_optimized"/>
     </node>
-  </group>  
+  </group>
 </launch>
@@ -10,16 +10,16 @@
 		<remap from="disparity/image"       to="disparity"/>
 		<remap from="disparity/camera_info" to="right/camera_info_throttle"/>
 		<remap from="cloud"                 to="cloudXYZ"/>
-		
+
 		<param name="voxel_size" type="double" value="0.05"/>
 		<param name="decimation" type="int" value="4"/>
 		<param name="max_depth" type="double" value="4"/>
 	</node>
-	
+
 	<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection obstacles_manager">
 	    <remap from="cloud" to="cloudXYZ"/>
-		<param name="frame_id" type="string" value="base_footprint"/>		
+		<param name="frame_id" type="string" value="base_footprint"/>
 		<param name="wait_for_transform" type="bool" value="true"/>
 		<param name="min_cluster_size" type="int" value="20"/>
 		<param name="max_obstacles_height" type="double" value="0.0"/>
@@ -1,15 +1,15 @@
 <?xml version="1.0"?>
 <launch>
-  
+
-    <!-- 
+    <!--
    Hand-held 3D lidar mapping example using only a Ouster OS-1 (no camera).
    Prerequisities: rtabmap should be built with libpointmatcher
    Example:
     $ roslaunch rtabmap_ros test_ouster.launch os1_hostname:=os1-XXXXXXXXXXXX.local os1_udp_dest:=192.168.1.XXX
     $ rosrun rviz rviz -f map
     $ Show TF and /rtabmap/cloud_map topics
-    ISSUE: You may have to reset odometry after receiving the first cloud if the map looks tilted. The problem seems 
+    ISSUE: You may have to reset odometry after receiving the first cloud if the map looks tilted. The problem seems
           coming from the first cloud sent by os1_cloud_node, which may be poorly synchronized with IMU data.
    -->
@@ -23,7 +23,7 @@
    <arg name="rtabmapviz"    default="true"/>
    <arg name="scan_20_hz"    default="true"/>
    <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
-    
+
    <!-- Ouster -->
    <remap unless="$(arg use_sim_time)" from="/os1_cloud_node/imu" to="/os1_cloud_node/imu/data_raw"/>
    <include unless="$(arg use_sim_time)" file="$(find ouster_ros)/os1.launch">
@@ -41,17 +41,17 @@
      <param name="use_mag" value="false"/>
      <param name="world_frame" value="enu"/>
      <param name="publish_tf" value="false"/>
-    </node> 
+    </node>
    <node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
      <remap from="imu/data" to="/os1_cloud_node/imu/data"/>
      <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
      <param name="base_frame_id" value="$(arg frame_id)"/>
-    </node> 
+    </node>
    <group ns="rtabmap">
      <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
        <remap from="scan_cloud" to="/os1_cloud_node/points"/>
-        <param name="frame_id"        type="string" value="$(arg frame_id)"/>  
+        <param name="frame_id"        type="string" value="$(arg frame_id)"/>
        <param name="odom_frame_id"   type="string" value="odom"/>
        <param     if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
        <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
@@ -59,7 +59,7 @@
        <remap from="imu" to="/os1_cloud_node/imu/data"/>
        <param name="guess_frame_id"   type="string" value="$(arg frame_id)_stabilized"/>
        <param name="wait_imu_to_init" type="bool" value="true"/>
-     
+
        <!-- ICP parameters -->
        <param name="Icp/PointToPlane"        type="string" value="true"/>
        <param name="Icp/Iterations"          type="string" value="10"/>
@@ -70,29 +70,29 @@
        <param name="Icp/PointToPlaneRadius"  type="string" value="0"/>
        <param name="Icp/MaxTranslation"      type="string" value="2"/>
        <param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
-        <param name="Icp/PM"                  type="string" value="true"/> 
+        <param name="Icp/PM"                  type="string" value="true"/>
        <param name="Icp/PMOutlierRatio"      type="string" value="0.1"/>
-        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>  
+        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
-        <!-- Odom parameters -->       
+        <!-- Odom parameters -->
        <param name="Odom/ScanKeyFrameThr"       type="string" value="0.95"/>
        <param name="Odom/Strategy"              type="string" value="0"/>
        <param name="OdomF2M/ScanSubtractRadius" type="string" value="0.2"/>
-        <param name="OdomF2M/ScanMaxSize"        type="string" value="15000"/>      
+        <param name="OdomF2M/ScanMaxSize"        type="string" value="15000"/>
      </node>
-      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">	  
+      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
-        <param name="frame_id"             type="string" value="$(arg frame_id)"/>  
+        <param name="frame_id"             type="string" value="$(arg frame_id)"/>
        <param name="subscribe_depth"      type="bool" value="false"/>
        <param name="subscribe_rgb"        type="bool" value="false"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <param name="approx_sync"          type="bool" value="false"/>
-        
+
        <remap from="scan_cloud" to="/os1_cloud_node/points"/>
        <remap from="imu" to="/os1_cloud_node/imu/data"/>
-     
+
        <!-- RTAB-Map's parameters -->
-        <param name="Rtabmap/DetectionRate"          type="string" value="1"/>  
+        <param name="Rtabmap/DetectionRate"          type="string" value="1"/>
        <param name="RGBD/NeighborLinkRefining"      type="string" value="false"/>
        <param name="RGBD/ProximityBySpace"          type="string" value="true"/>
        <param name="RGBD/ProximityMaxGraphDepth"    type="string" value="0"/>
@@ -104,8 +104,8 @@
        <!-- param name="Mem/LaserScanVoxelSize"     type="string" value="0.1"/ -->
        <!-- param name="Mem/LaserScanNormalK"       type="string" value="10"/ -->
        <!-- param name="Mem/LaserScanRadius"        type="string" value="0"/ -->
-        
+
-        <param name="Reg/Strategy"                   type="string" value="1"/> 
+        <param name="Reg/Strategy"                   type="string" value="1"/>
        <param name="Grid/CellSize"                  type="string" value="0.1"/>
        <param name="Grid/RangeMax"                  type="string" value="20"/>
        <param name="Grid/ClusterRadius"             type="string" value="1"/>
@@ -121,7 +121,7 @@
        <param name="Icp/Epsilon"                    type="string" value="0.001"/>
        <param name="Icp/MaxTranslation"             type="string" value="3"/>
        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="1"/>
-        <param name="Icp/PM"                         type="string" value="true"/> 
+        <param name="Icp/PM"                         type="string" value="true"/>
        <param name="Icp/PMOutlierRatio"             type="string" value="0.7"/>
        <param name="Icp/CorrespondenceRatio"        type="string" value="0.4"/>
      </node>
@@ -1,36 +1,36 @@
 <?xml version="1.0"?>
 <launch>
-  
+
-    <!-- 
+    <!--
    Hand-held 3D lidar mapping example using only a Ouster GEN2 (no camera).
    Prerequisities: rtabmap should be built with libpointmatcher
-    
+
    Example:
-    
+
      $ roslaunch rtabmap_ros test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX
      $ rosrun rviz rviz -f map
      RVIZ: Show TF and /rtabmap/cloud_map topics
-    
+
-    ISSUE: You may have to reset odometry after receiving the first cloud if the map looks tilted. The problem seems 
+    ISSUE: You may have to reset odometry after receiving the first cloud if the map looks tilted. The problem seems
           coming from the first cloud sent by os_cloud_node, which may be poorly synchronized with IMU data.
-           
+
    PTP mode (synchronize timestamp with host computer time)
-    
+
      * Install:
-      
+
        $ sudo apt install linuxptp httpie
        $ printf "[global]\ntx_timestamp_timeout 10\n" >> ~/os.conf
-      
+
      * Running:
-     
+
        (replace "XXXXXXXXXXXX" by your ouster serial, as well as XXX by its IP address)
        (replace "eth0" by the network interface used to communicate with ouster)
-       
+
        $ http PUT http://os-XXXXXXXXXXXX.local/api/v1/time/ptp/profile <<< '"default-relaxed"'
        $ sudo ptp4l -i eth0 -m -f ~/os.conf -S
        $ roslaunch rtabmap_ros test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX ptp:=true
-       
+
    -->
    <arg name="use_sim_time"  default="false"/>
@@ -44,14 +44,14 @@
    <arg name="deskewing"     default="true"/>
    <arg name="slerp"         default="false"/>
    <arg name="scan_20_hz"    default="true"/>
-    <arg name="voxel_size"    default="0.15"/>           <!-- indoor: 0.1 to 0.3, outdoor: 0.3 to 0.5 --> 
+    <arg name="voxel_size"    default="0.15"/>           <!-- indoor: 0.1 to 0.3, outdoor: 0.3 to 0.5 -->
    <arg name="assemble"      default="false"/>
    <arg name="ptp"           default="false"/>          <!-- See comments in header to start before launching the launch -->
    <arg name="imu_topic"     default="/os_cloud_node/imu"/>
    <arg name="scan_topic"    default="/os_cloud_node/points"/>
-    
+
    <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
-    
+
    <!-- Ouster -->
    <include unless="$(arg use_sim_time)" file="$(find ouster_ros)/ouster.launch">
      <arg name="sensor_hostname" value="$(arg sensor_hostname)"/>
@@ -76,8 +76,8 @@
      <remap from="imu/data" to="$(arg imu_topic)/filtered"/>
      <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
      <param name="base_frame_id" value="$(arg frame_id)"/>
-    </node> 
+    </node>
-    
+
    <!-- Lidar Deskewing -->
    <node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_ros/lidar_deskewing" output="screen">
      <param name="wait_for_transform" value="0.01"/>
@@ -93,14 +93,14 @@
      <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
        <remap from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
        <remap from="imu"        to="$(arg imu_topic)/filtered"/>
-        <param name="frame_id"        type="string" value="$(arg frame_id)"/>  
+        <param name="frame_id"        type="string" value="$(arg frame_id)"/>
        <param name="odom_frame_id"   type="string" value="odom"/>
        <param     if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
        <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
        <param name="guess_frame_id"   type="string" value="$(arg frame_id)_stabilized"/>
        <param name="wait_imu_to_init" type="bool" value="true"/>
-     
+
        <!-- ICP parameters -->
        <param name="Icp/PointToPlane"        type="string" value="true"/>
        <param name="Icp/Iterations"          type="string" value="10"/>
@@ -111,31 +111,31 @@
        <param name="Icp/PointToPlaneRadius"  type="string" value="0"/>
        <param name="Icp/MaxTranslation"      type="string" value="2"/>
        <param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
-        <param name="Icp/PM"                  type="string" value="true"/> 
+        <param name="Icp/PM"                  type="string" value="true"/>
        <param name="Icp/PMOutlierRatio"      type="string" value="0.1"/>
        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
-        <!-- Odom parameters -->       
+        <!-- Odom parameters -->
        <param name="Odom/ScanKeyFrameThr"       type="string" value="0.95"/>
        <param name="Odom/Strategy"              type="string" value="0"/>
        <param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg voxel_size)"/>
-        <param name="OdomF2M/ScanMaxSize"        type="string" value="15000"/>      
+        <param name="OdomF2M/ScanMaxSize"        type="string" value="15000"/>
      </node>
-      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">	  
+      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
-        <param name="frame_id"             type="string" value="$(arg frame_id)"/>  
+        <param name="frame_id"             type="string" value="$(arg frame_id)"/>
        <param name="subscribe_depth"      type="bool" value="false"/>
        <param name="subscribe_rgb"        type="bool" value="false"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <param name="approx_sync"          type="bool" value="false"/>
-        
+
        <remap     if="$(arg assemble)" from="scan_cloud" to="assembled_cloud"/>
        <remap unless="$(arg assemble)" from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
        <remap from="imu"        to="$(arg imu_topic)/filtered"/>
-     
+
        <!-- RTAB-Map's parameters -->
        <param     if="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="0"/> <!-- already set 1 Hz in point_cloud_assembler -->
-        <param unless="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="1"/>  
+        <param unless="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="1"/>
        <param name="RGBD/NeighborLinkRefining"      type="string" value="false"/>
        <param name="RGBD/ProximityBySpace"          type="string" value="true"/>
        <param name="RGBD/ProximityMaxGraphDepth"    type="string" value="0"/>
@@ -148,10 +148,10 @@
        <!-- param name="Mem/LaserScanVoxelSize"     type="string" value="0.1"/ -->
        <!-- param name="Mem/LaserScanNormalK"       type="string" value="10"/ -->
        <!-- param name="Mem/LaserScanRadius"        type="string" value="0"/ -->
-        
+
-        <param name="Reg/Strategy"                   type="string" value="1"/> 
+        <param name="Reg/Strategy"                   type="string" value="1"/>
        <param name="Optimizer/GravitySigma"         type="string" value="0.5"/>
-        <param name="Optimizer/Strategy"             type="string" value="1"/> 
+        <param name="Optimizer/Strategy"             type="string" value="1"/>
        <param name="Grid/CellSize"                  type="string" value="0.1"/>
        <param name="Grid/RangeMax"                  type="string" value="20"/>
        <param name="Grid/ClusterRadius"             type="string" value="1"/>
@@ -166,11 +166,11 @@
        <param name="Icp/Epsilon"                    type="string" value="0.001"/>
        <param name="Icp/MaxTranslation"             type="string" value="3"/>
        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="1"/>
-        <param name="Icp/PM"                         type="string" value="true"/> 
+        <param name="Icp/PM"                         type="string" value="true"/>
        <param name="Icp/PMOutlierRatio"             type="string" value="0.7"/>
        <param name="Icp/CorrespondenceRatio"        type="string" value="0.2"/>
      </node>
-      
+
      <node if="$(arg assemble)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
        <remap from="cloud" to="$(arg scan_topic_deskewed)"/>
        <remap from="odom"            to="odom"/>
@@ -1,21 +1,21 @@
 <?xml version="1.0"?>
 <launch>
-   
+
  <include file="$(find freenect_launch)/launch/freenect.launch" >
    <arg name="depth_registration" value="true"/>
    <arg name="data_skip" value="3"/>
  </include>
-  
+
  <group ns="camera">
  <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
    <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
    <remap from="depth/image_in"     to="depth_registered/image_raw"/>
    <remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
-    
+
    <param name="decimation" type="int" value="2"/>
  </node>
-  
+
  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb camera_nodelet_manager">
    <remap from="rgb/image"       to="rgb/image_out"/>
    <remap from="depth/image"     to="depth/image_out"/>
@@ -27,7 +27,7 @@
    <param name="noise_filter_radius" type="double" value="0.05"/>
    <param name="normal_k" type="int" value="6"/>
  </node>
-  
+
  <node pkg="nodelet" type="nodelet" name="points_xyz" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
    <remap from="depth/image"     to="depth/image_out"/>
    <remap from="depth/camera_info" to="rgb/camera_info_out"/>
@@ -43,10 +43,10 @@
  <!-- stereo test with stereo_outdoorA.bag -->
 <!--
   <param name="use_sim_time" value="true"/>
-  
+
   <node name="republish_left" type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/left/image_raw_throttle raw out:=/stereo_camera/left/image_raw_throttle_relay" />
   <node name="republish_right" type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/right/image_raw_throttle raw out:=/stereo_camera/right/image_raw_throttle_relay" />
-   
+
   <group ns="/stereo_camera" >
      <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
         <remap from="left/image_raw"    to="left/image_raw_throttle_relay"/>
@@ -56,18 +56,18 @@
         <param name="disparity_range" value="128"/>
      </node>
   </group>
-  
+
  <node pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="stereo_throttle" args="load rtabmap_ros/stereo_throttle standalone_nodelet">
    <remap from="left/image"         to="/stereo_camera/left/image_rect_color"/>
    <remap from="right/image"        to="/stereo_camera/right/image_rect"/>
    <remap from="left/camera_info"   to="/stereo_camera/left/camera_info_throttle"/>
    <remap from="right/camera_info"  to="/stereo_camera/right/camera_info_throttle"/>
-    
+
    <param name="decimation" type="int" value="2"/>
    <param name="approx_sync" type="bool" value="false"/>
  </node>
-  
+
  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
    <remap from="left/image"         to="/stereo_camera/left/image_rect_color_throttle"/>
    <remap from="right/image"        to="/stereo_camera/right/image_rect_throttle"/>
@@ -1,45 +1,45 @@
 <?xml version="1.0"?>
 <launch>
-   
+
   <!-- Example with rgbd datasets:
   $ wget http://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.bag
   $ rosbag decompress rgbd_dataset_freiburg3_long_office_household.bag
   $ chmod +x test_prior_rename_kinect_bag_tf.py
   $ ./test_prior_rename_kinect_bag_tf.py
-   We simulate an external "global_pose" by republishing ground truth TF (VICON) with some 
+   We simulate an external "global_pose" by republishing ground truth TF (VICON) with some
-   covariance, normally you should not have to use test_prior_tf_to_pose.py as the node 
+   covariance, normally you should not have to use test_prior_tf_to_pose.py as the node
   publishing the global pose would give it directly as a pose with correct covariance.
-   
+
-   Rename all child_frame_id "/kinect" to "/kinect_gt" Tf in the bag 
+   Rename all child_frame_id "/kinect" to "/kinect_gt" Tf in the bag
   using test_prior_rename_kinect_bag_tf.py in this directory!
-   
+
-   $ roslaunch rtabmap_ros test_prior.launch 
+   $ roslaunch rtabmap_ros test_prior.launch
   $ chmod +x test_prior_tf_to_pose.py
   $ ./test_prior_tf_to_pose.py
   $ rosbag play -.-clock rgbd_dataset_freiburg3_long_office_household_tf_renamed.bag
   -->
-   
+
   <param name="use_sim_time" type="bool" value="True"/>
-     
+
   <!-- Choose visualization -->
   <arg name="rviz" default="true" />
-   <arg name="rtabmapviz" default="false" /> 
+   <arg name="rtabmapviz" default="false" />
-   
+
   <!-- TF FRAMES -->
-  <node pkg="tf" type="static_transform_publisher" name="world_to_map" 
+  <node pkg="tf" type="static_transform_publisher" name="world_to_map"
    args="0.0 0.0 0.0 0.0 0.0 0.0 /world /map 100" />
-    
+
  <group ns="rtabmap">
-  
+
    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
      <remap from="rgb/image"       to="/camera/rgb/image_color"/>
      <remap from="depth/image"     to="/camera/depth/image"/>
      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
      <remap from="odom" to="vis_odom"/>
-	  
+
      <param name="odom_frame_id" type="string" value="vis_odom"/>
      <param name="frame_id" type="string" value="kinect"/>
    </node>
@@ -48,11 +48,11 @@
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="subscribe_depth" type="bool" value="true"/>
      <param name="frame_id" type="string" value="kinect"/>
-      
+
      <param name="RGBD/CreateOccupancyGrid" type="string" value="false"/>
      <param name="Optimizer/PriorsIgnored" type="string" value="false"/>
      <param name="Optimizer/Strategy" type="string" value="1"/>
-	
+
      <remap from="rgb/image" to="/camera/rgb/image_color"/>
      <remap from="depth/image" to="/camera/depth/image"/>
      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
@@ -61,24 +61,24 @@
      <param name="ground_truth_frame_id" type="string" value="world"/>
      <param name="ground_truth_base_frame_id" type="string" value="kinect_gt"/>
    </node>
-    
+
    <!-- Visualisation  -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
      <param name="subscribe_depth" type="bool" value="true"/>
      <param name="subscribe_laserScan" type="bool" value="false"/>
      <param name="subscribe_odom_info" type="bool" value="true"/>
      <param name="queue_size" type="int" value="30"/>
-      
+
      <param name="frame_id" type="string" value="kinect"/>
-    
+
      <remap from="rgb/image" to="/camera/rgb/image_color"/>
      <remap from="depth/image" to="/camera/depth/image"/>
      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
      <remap from="odom" to="vis_odom"/>
    </node>
-        
+
  </group>
-  
+
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbdslam_datasets.rviz"/>
 </launch>
@@ -1,4 +1,4 @@
-#!/usr/bin/env python 
+#!/usr/bin/env python
 import rosbag
 from tf.msg import tfMessage
 with rosbag.Bag('rgbd_dataset_freiburg3_long_office_household_tf_renamed.bag', 'w') as outbag:
@@ -1,4 +1,4 @@
-#!/usr/bin/env python  
+#!/usr/bin/env python
 import rospy
 import tf
 import numpy
@@ -14,9 +14,9 @@ if __name__ == '__main__':
    cov = rospy.get_param('~cov', 1)
    rateParam = rospy.get_param('~rate', 30) # 10hz
    pub = rospy.Publisher('global_pose', PoseWithCovarianceStamped, queue_size=1)
-   
+
    print 'start loop!'
-    rate = rospy.Rate(rateParam) 
+    rate = rospy.Rate(rateParam)
    while not rospy.is_shutdown():
        poseOut = PoseWithCovarianceStamped()
        try:
@@ -27,7 +27,7 @@ if __name__ == '__main__':
            print str(e)
            rate.sleep()
            continue
-        
+
        poseOut.header.stamp.nsecs = now.nsecs
        poseOut.header.stamp.secs = now.secs
        poseOut.header.frame_id = outputFrame
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
-<launch>  
+<launch>
  <!-- Kinect: -->
  <include file="$(find freenect_launch)/launch/freenect.launch">
@@ -21,17 +21,17 @@
   <!-- Nodes -->
  <group ns="rtabmap">
-  
+
    <!-- RGB-D Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
      <param name="subscribe_rgbd"       type="bool"   value="true"/>
      <remap     if="$(arg compressed)"    from="rgbd_image"   to="/camera/rgbd_image/compressed"/>
      <remap unless="$(arg compressed)"    from="rgbd_image"   to="/camera/rgbd_image"/>
-        
+
 	  <param name="Odom/AlignWithGround"  type="string" value="true"/>
      <param name="frame_id"         type="string" value="camera_link"/>
    </node>
-    
+
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="subscribe_depth"       type="bool"   value="false"/>
      <param name="subscribe_rgbd"       type="bool"   value="true"/>
@@ -40,7 +40,7 @@
      <remap     if="$(arg compressed)"    from="rgbd_image"   to="/camera/rgbd_image/compressed"/>
      <remap unless="$(arg compressed)"    from="rgbd_image"   to="/camera/rgbd_image"/>
    </node>
-  
+
    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
      <param name="subscribe_depth"      type="bool"   value="false"/>
      <param name="subscribe_rgbd"       type="bool"   value="true"/>
@@ -50,7 +50,7 @@
      <remap unless="$(arg compressed)"    from="rgbd_image"   to="/camera/rgbd_image"/>
      <param name="subscribe_odom_info"  type="bool"   value="true"/>
    </node>
-  
+
  </group>
 </launch>
@@ -6,32 +6,32 @@
  <include file="$(find freenect_launch)/launch/freenect.launch">
    <arg name="depth_registration" value="true" />
  </include>
-  
+
-  <arg name="frame_id"                default="camera_link"/> 
+  <arg name="frame_id"                default="camera_link"/>
-  <arg name="rtabmap_args"            default="--delete_db_on_start"/>   <!-- delete_db_on_start, udebug -->  
+  <arg name="rtabmap_args"            default="--delete_db_on_start"/>   <!-- delete_db_on_start, udebug -->
  <arg name="odom_args"               default="$(arg rtabmap_args)"/>
-   
+
  <!-- RGB-D related topics -->
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
- 
+
-  <group ns="camera"> 
+  <group ns="camera">
-  
+
    <!-- Use RGBD synchronization -->
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager">
      <remap from="rgb/image"         to="$(arg rgb_topic)"/>
      <remap from="depth/image"       to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
    </node>
-  
+
    <!-- RGB-D Odometry -->
      <node pkg="nodelet" type="nodelet" name="rgbd_odometry" args="load rtabmap_ros/rgbd_odometry camera_nodelet_manager $(arg odom_args)">
        <param name="frame_id"        type="string" value="$(arg frame_id)"/>
        <param name="subscribe_depth" type="bool"   value="false"/>
        <param name="subscribe_rgbd"  type="bool"   value="true"/>
      </node>
-  
+
    <!-- RTAB-Map -->
    <node pkg="nodelet" type="nodelet" name="rtabmap" args="load rtabmap_ros/rtabmap camera_nodelet_manager $(arg rtabmap_args)">
      <param name="frame_id"        type="string" value="$(arg frame_id)"/>
@@ -39,7 +39,7 @@
      <param name="subscribe_rgbd"  type="bool"   value="true"/>
      <param name="approx_sync"     type="bool"   value="false"/>
    </node>
-  
+
    <!-- Visualisation -->
    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
      <param name="frame_id" type="string" value="$(arg frame_id)"/>
@@ -47,7 +47,7 @@
      <param name="subscribe_odom_info"  type="bool"   value="true"/>
      <param name="approx_sync"     type="bool"   value="false"/>
    </node>
-    
+
  </group>
 </launch>
@@ -1,16 +1,16 @@
 <?xml version="1.0"?>
 <launch>
-   
+
  <!-- Testing 2 Kinects localizing in the same map at the same time -->
-  <!-- Prerequisities: the default ~/.ros/rtabmap.db should be 
+  <!-- Prerequisities: the default ~/.ros/rtabmap.db should be
                       already created with one of the Kinect using mapping tutorial:
-                       http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping 
+                       http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping
              $ roslaunch freenect_launch freenect.launch depth_registration:=true
              $ roslaunch rtabmap_ros rtabmap.launch args:="-d"
  -->
-  
+
-   
+
  <!-- Cameras -->
  <include file="$(find freenect_launch)/launch/freenect.launch">
    <arg name="depth_registration" value="True" />
@@ -6,16 +6,16 @@
  <include file="$(find openni2_launch)/launch/openni2.launch">
    <arg name="depth_registration" value="true" />
  </include>
-  
+
  <arg name="depth"  default="/camera/depth_registered/image_raw" />
  <arg name="model"  default="$(find rtabmap_ros)/launch/calibration/distortion_model_PS1080.bin" /> <!-- XTION Live Pro -->
- 
+
-  <group ns="camera"> 
+  <group ns="camera">
    <!-- Undistort depth image -->
    <node pkg="nodelet" type="nodelet" name="undistort" args="load rtabmap_ros/undistort_depth camera_nodelet_manager">
      <remap from="depth" to="$(arg depth)"/>
      <param name="model" value="$(arg model)"/>
-    </node>   
+    </node>
  </group>
 </launch>
@@ -6,25 +6,25 @@
  <include file="$(find freenect_launch)/launch/freenect.launch">
    <arg name="depth_registration" value="true" />
  </include>
-  
+
-  <arg name="frame_id"                default="camera_link"/> 
+  <arg name="frame_id"                default="camera_link"/>
-  <arg name="rtabmap_args"            default="--delete_db_on_start"/>   <!-- delete_db_on_start, udebug -->  
+  <arg name="rtabmap_args"            default="--delete_db_on_start"/>   <!-- delete_db_on_start, udebug -->
  <arg name="odom_args"               default="$(arg rtabmap_args)"/>
-   
+
  <!-- RGB-D related topics -->
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
- 
+
-  <group ns="camera"> 
+  <group ns="camera">
-  
+
    <!-- Use RGBD synchronization -->
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager">
      <remap from="rgb/image"         to="$(arg rgb_topic)"/>
      <remap from="depth/image"       to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
    </node>
-  
+
    <!-- RGB-D Odometry -->
      <node pkg="nodelet" type="nodelet" name="rgbd_odometry" args="load rtabmap_ros/rgbd_odometry camera_nodelet_manager $(arg odom_args)">
        <param name="frame_id"        type="string" value="$(arg frame_id)"/>
@@ -32,7 +32,7 @@
        <param name="subscribe_rgbd"  type="bool"   value="true"/>
        <param name="keep_color"      type="bool"   value="true"/>
      </node>
-  
+
    <!-- RTAB-Map -->
    <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" args="$(arg rtabmap_args)" output="screen">
      <param name="frame_id"        type="string" value="$(arg frame_id)"/>
@@ -40,7 +40,7 @@
      <param name="approx_sync"     type="bool"   value="false"/>
      <remap from="rgbd_image" to="odom_rgbd_image"/>
    </node>
-  
+
    <!-- Visualisation -->
    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
      <param name="frame_id" type="string" value="$(arg frame_id)"/>
@@ -48,7 +48,7 @@
      <param name="subscribe_odom_info"  type="bool"   value="true"/>
      <param name="approx_sync"     type="bool"   value="false"/>
    </node>
-    
+
  </group>
 </launch>
@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <!-- -->
 <launch>
-  
+
-    <!-- 
+    <!--
    Hand-held 3D lidar mapping example using only a Velodyne PUCK (no camera).
    Prerequisities: rtabmap should be built with libpointmatcher
    Example:
@@ -29,26 +29,26 @@
    <arg name="resolution"         default="0.05"/> <!-- set 0.05-0.3 for indoor, set 0.3-0.5 for outdoor (0.4 for kitti) -->
    <arg name="iterations"         default="10"/>
-    
+
    <!-- Grid parameters -->
    <arg name="ground_is_obstacle" default="true"/>
    <arg name="grid_max_range"     default="20"/>
-    
+
    <!-- For F2M Odometry -->
    <arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car, kitti) -->
    <arg name="local_map_size"    default="15000"/>
    <arg name="key_frame_thr"     default="0.6"/>
-    
+
    <!-- For FLOAM Odometry -->
    <arg name="floam"    default="false"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html -->
    <arg name="floam_sensor" default="0"/> <!-- 0=16 rings (VLP16), 1=32 rings, 2=64 rings (kitti dataset) -->
-    
+
    <include unless="$(arg use_sim_time)" file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
       <arg     if="$(arg scan_20_hz)" name="rpm" value="1200"/>
       <arg unless="$(arg scan_20_hz)" name="rpm" value="600"/>
       <arg name="organize_cloud" value="$(arg organize_cloud)"/>
    </include>
-    
+
    <!-- IMU orientation estimation and publish tf -->
    <node if="$(arg use_imu)" pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
      <remap from="imu/data_raw" to="$(arg imu_topic)"/>
@@ -63,12 +63,12 @@
      <remap from="imu/data" to="$(arg imu_topic)/filtered"/>
      <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
      <param name="base_frame_id" value="$(arg frame_id)"/>
-    </node> 
+    </node>
    <group ns="rtabmap">
      <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
        <remap from="scan_cloud" to="$(arg scan_topic)"/>
-        <param name="frame_id"        type="string" value="$(arg frame_id)"/>  
+        <param name="frame_id"        type="string" value="$(arg frame_id)"/>
        <param name="odom_frame_id"   type="string" value="odom"/>
        <param name="deskewing"       type="bool"   value="$(arg deskewing)"/>
        <param name="deskewing_slerp" type="bool"   value="$(arg slerp)"/>
@@ -80,7 +80,7 @@
        <remap if="$(arg use_imu)" from="imu" to="$(arg imu_topic)/filtered"/>
        <param if="$(arg use_imu)" name="guess_frame_id"   type="string" value="$(arg frame_id)_stabilized"/>
        <param if="$(arg use_imu)" name="wait_imu_to_init" type="bool" value="true"/>
-     
+
        <!-- ICP parameters -->
        <param name="Icp/PointToPlane"        type="string" value="true"/>
        <param name="Icp/Iterations"          type="string" value="$(arg iterations)"/>
@@ -93,37 +93,37 @@
        <param name="Icp/PointToPlaneRadius"  type="string" value="0"/>
        <param name="Icp/MaxTranslation"      type="string" value="2"/>
        <param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
-        <param name="Icp/PM"                  type="string" value="true"/> 
+        <param name="Icp/PM"                  type="string" value="true"/>
        <param name="Icp/PMOutlierRatio"      type="string" value="0.7"/>
-        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>  
+        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
        <param if="$(arg ground_normals_up)" name="Icp/PointToPlaneGroundNormalsUp"  type="string" value="0.8"/>
-        <!-- Odom parameters -->       
+        <!-- Odom parameters -->
        <param name="Odom/ScanKeyFrameThr"       type="string" value="$(arg key_frame_thr)"/>
        <param     if="$(arg floam)" name="Odom/Strategy" type="string" value="11"/>
        <param unless="$(arg floam)" name="Odom/Strategy" type="string" value="0"/>
        <param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg resolution)"/>
        <param name="OdomF2M/ScanMaxSize"        type="string" value="$(arg local_map_size)"/>
-        <param name="OdomLOAM/Sensor"            type="string" value="$(arg floam_sensor)"/> 
+        <param name="OdomLOAM/Sensor"            type="string" value="$(arg floam_sensor)"/>
        <param name="OdomLOAM/Resolution"        type="string" value="$(arg resolution)"/>
        <param if="$(eval not deskewing and scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
        <param if="$(eval not deskewing and not scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
        <param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
      </node>
-      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">	  
+      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
-        <param name="frame_id"             type="string" value="$(arg frame_id)"/>  
+        <param name="frame_id"             type="string" value="$(arg frame_id)"/>
        <param name="subscribe_depth"      type="bool" value="false"/>
        <param name="subscribe_rgb"        type="bool" value="false"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <param name="approx_sync"          type="bool" value="false"/>
        <param name="wait_for_transform_duration" type="double" value="0.2"/>
-        
+
        <remap from="scan_cloud" to="assembled_cloud"/>
        <remap from="imu" to="$(arg imu_topic)/filtered"/>
-     
+
        <!-- RTAB-Map's parameters -->
-        <param name="Rtabmap/DetectionRate"          type="string" value="1"/>  
+        <param name="Rtabmap/DetectionRate"          type="string" value="1"/>
        <param name="RGBD/NeighborLinkRefining"      type="string" value="false"/>
        <param name="RGBD/ProximityBySpace"          type="string" value="true"/>
        <param name="RGBD/ProximityMaxGraphDepth"    type="string" value="0"/>
@@ -133,8 +133,8 @@
        <param name="Mem/NotLinkedNodesKept"         type="string" value="false"/>
        <param name="Mem/STMSize"                    type="string" value="30"/>
        <param name="Mem/LaserScanNormalK"           type="string" value="20"/>
-        
+
-        <param name="Reg/Strategy"                   type="string" value="1"/> 
+        <param name="Reg/Strategy"                   type="string" value="1"/>
        <param name="Grid/CellSize"                  type="string" value="$(arg resolution)"/>
        <param name="Grid/RangeMax"                  type="string" value="$(arg grid_max_range)"/>
        <param name="Grid/ClusterRadius"             type="string" value="1"/>
@@ -150,7 +150,7 @@
        <param name="Icp/Epsilon"                    type="string" value="0.001"/>
        <param name="Icp/MaxTranslation"             type="string" value="3"/>
        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="$(eval resolution*10)"/>
-        <param name="Icp/PM"                         type="string" value="true"/> 
+        <param name="Icp/PM"                         type="string" value="true"/>
        <param name="Icp/PMOutlierRatio"             type="string" value="0.7"/>
        <param name="Icp/CorrespondenceRatio"        type="string" value="$(arg loop_ratio)"/>
      </node>
@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <!-- -->
 <launch>
-  
+
-    <!-- 
+    <!--
    Hand-held 3D lidar mapping example using a Velodyne PUCK, an external IMU and color camera (using D435i as example).
    Prerequisities: rtabmap should be built with libpointmatcher
    We use D435i imu only for lidar deskewing and icp_odometry guess in this example.
@@ -28,30 +28,30 @@
    <arg name="resolution"         default="0.05"/> <!-- set 0.05-0.3 for indoor, set 0.3-0.5 for outdoor -->
    <arg name="iterations"         default="10"/>
-    
+
    <!-- Grid parameters -->
    <arg name="ground_is_obstacle" default="true"/>
    <arg name="grid_max_range"     default="20"/>
-    
+
    <!-- For F2M Odometry -->
    <arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car) -->
    <arg name="local_map_size"    default="15000"/>
    <arg name="key_frame_thr"     default="0.6"/>
-    
+
    <!-- For FLOAM Odometry -->
    <arg name="floam"    default="false"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html -->
    <arg name="floam_sensor" default="0"/> <!-- 0=16 rings (VLP16), 1=32 rings, 2=64 rings -->
-    
+
    <!-- Static transform between velodyne and D435i: TODO: Adjust with real position/orientation!!! -->
    <node unless="$(arg use_sim_time)" pkg="tf" type="static_transform_publisher" name="velodyne_to_camera_tf" args="0.03 0.064 -0.055 0 -0.02 0 velodyne camera_link 100"/>
-    
+
    <!-- Velodyne sensor VLP16 -->
    <include unless="$(arg use_sim_time)" file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
       <arg     if="$(arg scan_20_hz)" name="rpm" value="1200"/>
       <arg unless="$(arg scan_20_hz)" name="rpm" value="600"/>
       <arg name="organize_cloud" value="true"/> <!-- should be organized for deskewing -->
    </include>
-    
+
    <!-- D435i -->
    <group unless="$(arg use_sim_time)">
      <include file="$(find realsense2_camera)/launch/rs_camera.launch">
@@ -60,7 +60,7 @@
        <arg name="enable_accel"     value="true"/>
      </include>
    </group>
-    
+
    <!-- IMU orientation estimation and publish tf accordingly to os_sensor frame -->
    <node pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
      <remap from="imu/data_raw" to="$(arg imu_topic)"/>
@@ -75,8 +75,8 @@
      <remap from="imu/data" to="$(arg imu_topic)/filtered"/>
      <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
      <param name="base_frame_id" value="$(arg frame_id)"/>
-    </node> 
+    </node>
-    
+
    <!-- Lidar Deskewing -->
    <node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_ros/lidar_deskewing" output="screen">
      <param name="wait_for_transform" value="0.01"/>
@@ -100,7 +100,7 @@
        <param name="wait_for_transform_duration" type="double" value="0.2"/>
        <param     if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
        <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
-     
+
        <!-- ICP parameters -->
        <param name="Icp/PointToPlane"        type="string" value="true"/>
        <param name="Icp/Iterations"          type="string" value="$(arg iterations)"/>
@@ -113,39 +113,39 @@
        <param name="Icp/PointToPlaneRadius"  type="string" value="0"/>
        <param name="Icp/MaxTranslation"      type="string" value="2"/>
        <param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
-        <param name="Icp/PM"                  type="string" value="true"/> 
+        <param name="Icp/PM"                  type="string" value="true"/>
        <param name="Icp/PMOutlierRatio"      type="string" value="0.7"/>
-        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>  
+        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
        <param if="$(arg ground_normals_up)" name="Icp/PointToPlaneGroundNormalsUp"  type="string" value="0.8"/>
-        <!-- Odom parameters -->       
+        <!-- Odom parameters -->
        <param name="Odom/ScanKeyFrameThr"       type="string" value="$(arg key_frame_thr)"/>
        <param     if="$(arg floam)" name="Odom/Strategy" type="string" value="11"/>
        <param unless="$(arg floam)" name="Odom/Strategy" type="string" value="0"/>
        <param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg resolution)"/>
        <param name="OdomF2M/ScanMaxSize"        type="string" value="$(arg local_map_size)"/>
-        <param name="OdomLOAM/Sensor"            type="string" value="$(arg floam_sensor)"/> 
+        <param name="OdomLOAM/Sensor"            type="string" value="$(arg floam_sensor)"/>
        <param name="OdomLOAM/Resolution"        type="string" value="$(arg resolution)"/>
        <param if="$(eval not deskewing and scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
        <param if="$(eval not deskewing and not scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
        <param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
      </node>
-      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">	  
+      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
-        <param name="frame_id"             type="string" value="$(arg frame_id)"/>  
+        <param name="frame_id"             type="string" value="$(arg frame_id)"/>
        <param name="subscribe_depth"      type="bool" value="false"/>
        <param name="subscribe_rgb"        type="bool" value="true"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <param name="approx_sync"          type="bool" value="true"/>
        <param name="wait_for_transform_duration" type="double" value="0.2"/>
-        
+
        <remap from="scan_cloud" to="assembled_cloud"/>
        <remap from="rgb/image" to="/camera/color/image_raw"/>
        <remap from="rgb/camera_info" to="/camera/color/camera_info"/>
        <remap from="imu"        to="$(arg imu_topic)/filtered"/>
-     
+
        <!-- RTAB-Map's parameters -->
-        <param name="Rtabmap/DetectionRate"          type="string" value="1"/>  
+        <param name="Rtabmap/DetectionRate"          type="string" value="1"/>
        <param name="RGBD/NeighborLinkRefining"      type="string" value="false"/>
        <param name="RGBD/ProximityBySpace"          type="string" value="true"/>
        <param name="RGBD/ProximityMaxGraphDepth"    type="string" value="0"/>
@@ -155,8 +155,8 @@
        <param name="Mem/NotLinkedNodesKept"         type="string" value="false"/>
        <param name="Mem/STMSize"                    type="string" value="30"/>
        <param name="Mem/LaserScanNormalK"           type="string" value="20"/>
-        
+
-        <param name="Reg/Strategy"                   type="string" value="1"/> 
+        <param name="Reg/Strategy"                   type="string" value="1"/>
        <param name="Grid/CellSize"                  type="string" value="$(arg resolution)"/>
        <param name="Grid/RangeMax"                  type="string" value="$(arg grid_max_range)"/>
        <param name="Grid/ClusterRadius"             type="string" value="1"/>
@@ -172,7 +172,7 @@
        <param name="Icp/Epsilon"                    type="string" value="0.001"/>
        <param name="Icp/MaxTranslation"             type="string" value="3"/>
        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="$(eval resolution*10)"/>
-        <param name="Icp/PM"                         type="string" value="true"/> 
+        <param name="Icp/PM"                         type="string" value="true"/>
        <param name="Icp/PMOutlierRatio"             type="string" value="0.7"/>
        <param name="Icp/CorrespondenceRatio"        type="string" value="$(arg loop_ratio)"/>
      </node>
@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <!-- -->
 <launch>
-  
+
-    <!-- 
+    <!--
    Hand-held 3D lidar mapping example using only a Velodyne PUCK and external odometry (using t265 as example).
    Prerequisities: rtabmap should be built with libpointmatcher
    We use T265 only for lidar deskewing and icp_odometry guess in this example.
@@ -28,30 +28,30 @@
    <arg name="resolution"         default="0.05"/> <!-- set 0.05-0.3 for indoor, set 0.3-0.5 for outdoor -->
    <arg name="iterations"         default="10"/>
-    
+
    <!-- Grid parameters -->
    <arg name="ground_is_obstacle" default="true"/>
    <arg name="grid_max_range"     default="20"/>
-    
+
    <!-- For F2M Odometry -->
    <arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car) -->
    <arg name="local_map_size"    default="15000"/>
    <arg name="key_frame_thr"     default="0.6"/>
-    
+
    <!-- For FLOAM Odometry -->
    <arg name="floam"    default="false"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html -->
    <arg name="floam_sensor" default="0"/> <!-- 0=16 rings (VLP16), 1=32 rings, 2=64 rings -->
-    
+
    <!-- Static transform between velodyne and T265: TODO: Adjust with real position/orientation!!! -->
    <node unless="$(arg use_sim_time)" pkg="tf" type="static_transform_publisher" name="T265_to_velodyne_tf" args="-0.01 0 0.055 0 0 0 t265_link velodyne 100"/>
-    
+
    <!-- Velodyne sensor VLP16 -->
    <include unless="$(arg use_sim_time)" file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
       <arg     if="$(arg scan_20_hz)" name="rpm" value="1200"/>
       <arg unless="$(arg scan_20_hz)" name="rpm" value="600"/>
       <arg name="organize_cloud" value="true"/> <!-- should be organized for deskewing -->
    </include>
-    
+
    <!-- T265 -->
    <group unless="$(arg use_sim_time)" ns="t265">
      <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
@@ -66,7 +66,7 @@
        <arg name="enable_pose"           value="true"/>
      </include>
    </group>
-    
+
    <!-- Lidar Deskewing -->
    <node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_ros/lidar_deskewing" output="screen">
      <param name="wait_for_transform" value="0.01"/>
@@ -88,7 +88,7 @@
        <param name="wait_for_transform_duration" type="double" value="0.2"/>
        <param     if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
        <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
-     
+
        <!-- ICP parameters -->
        <param name="Icp/PointToPlane"        type="string" value="true"/>
        <param name="Icp/Iterations"          type="string" value="$(arg iterations)"/>
@@ -101,36 +101,36 @@
        <param name="Icp/PointToPlaneRadius"  type="string" value="0"/>
        <param name="Icp/MaxTranslation"      type="string" value="2"/>
        <param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
-        <param name="Icp/PM"                  type="string" value="true"/> 
+        <param name="Icp/PM"                  type="string" value="true"/>
        <param name="Icp/PMOutlierRatio"      type="string" value="0.7"/>
-        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>  
+        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
        <param if="$(arg ground_normals_up)" name="Icp/PointToPlaneGroundNormalsUp"  type="string" value="0.8"/>
-        <!-- Odom parameters -->       
+        <!-- Odom parameters -->
        <param name="Odom/ScanKeyFrameThr"       type="string" value="$(arg key_frame_thr)"/>
        <param     if="$(arg floam)" name="Odom/Strategy" type="string" value="11"/>
        <param unless="$(arg floam)" name="Odom/Strategy" type="string" value="0"/>
        <param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg resolution)"/>
        <param name="OdomF2M/ScanMaxSize"        type="string" value="$(arg local_map_size)"/>
-        <param name="OdomLOAM/Sensor"            type="string" value="$(arg floam_sensor)"/> 
+        <param name="OdomLOAM/Sensor"            type="string" value="$(arg floam_sensor)"/>
        <param name="OdomLOAM/Resolution"        type="string" value="$(arg resolution)"/>
        <param if="$(eval not deskewing and scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
        <param if="$(eval not deskewing and not scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
        <param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
      </node>
-      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">	  
+      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
-        <param name="frame_id"             type="string" value="$(arg frame_id)"/>  
+        <param name="frame_id"             type="string" value="$(arg frame_id)"/>
        <param name="subscribe_depth"      type="bool" value="false"/>
        <param name="subscribe_rgb"        type="bool" value="false"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <param name="approx_sync"          type="bool" value="false"/>
        <param name="wait_for_transform_duration" type="double" value="0.2"/>
-        
+
        <remap from="scan_cloud" to="assembled_cloud"/>
-     
+
        <!-- RTAB-Map's parameters -->
-        <param name="Rtabmap/DetectionRate"          type="string" value="1"/>  
+        <param name="Rtabmap/DetectionRate"          type="string" value="1"/>
        <param name="RGBD/NeighborLinkRefining"      type="string" value="false"/>
        <param name="RGBD/ProximityBySpace"          type="string" value="true"/>
        <param name="RGBD/ProximityMaxGraphDepth"    type="string" value="0"/>
@@ -140,8 +140,8 @@
        <param name="Mem/NotLinkedNodesKept"         type="string" value="false"/>
        <param name="Mem/STMSize"                    type="string" value="30"/>
        <param name="Mem/LaserScanNormalK"           type="string" value="20"/>
-        
+
-        <param name="Reg/Strategy"                   type="string" value="1"/> 
+        <param name="Reg/Strategy"                   type="string" value="1"/>
        <param name="Grid/CellSize"                  type="string" value="$(arg resolution)"/>
        <param name="Grid/RangeMax"                  type="string" value="$(arg grid_max_range)"/>
        <param name="Grid/ClusterRadius"             type="string" value="1"/>
@@ -157,7 +157,7 @@
        <param name="Icp/Epsilon"                    type="string" value="0.001"/>
        <param name="Icp/MaxTranslation"             type="string" value="3"/>
        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="$(eval resolution*10)"/>
-        <param name="Icp/PM"                         type="string" value="true"/> 
+        <param name="Icp/PM"                         type="string" value="true"/>
        <param name="Icp/PMOutlierRatio"             type="string" value="0.7"/>
        <param name="Icp/CorrespondenceRatio"        type="string" value="$(arg loop_ratio)"/>
      </node>