Added "decimation" parameter to data_throttle and stereo_throttle nodelets. Added cloud processing parameters from point_cloud_xyz to point_cloud_xyzrgb nodelet. Added stereo inputs to point_cloud_xyzrgb.

launch/azimut3/az3_bag_record.launch

@@ -20,8 +20,8 @@
   
   <!-- Throttling messages -->
   <group ns="camera">
-    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap/data_throttle camera_nodelet_manager" output="screen">
-      <param name="max_rate" type="double" value="10.0"/>
+    <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"/>

launch/azimut3/az3_db_record.launch

@@ -11,8 +11,8 @@
   
   <!-- Throttling messages -->
   <group ns="camera">
-    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap/data_throttle camera_nodelet_manager" output="screen">
-      <param name="max_rate" type="double" value="10.0"/>
+    <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"/>

launch/azimut3/az3_mapping_client_nav.launch

@@ -4,21 +4,44 @@
   <!-- Choose visualization -->
   <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"/>
+  </node>
+  <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"/>
+    <remap from="rgb/camera_info" to="/camera/data_resized_camera_info"/>
+    <remap from="cloud"           to="/voxel_cloud" />
+  </node>
+
   <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"/>
-    
-  <!-- use a relay on this machine, same for images -->
-  <node if="$(arg rviz)" name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData /rtabmap/mapData_relay">
-    <param name="lazy" type="bool" value="true"/>
-  </node>
 </launch>

launch/azimut3/az3_mapping_client_nav.launch~

@@ -1,23 +0,0 @@
-
-<launch>
-
-  <!-- We have two nodes (grid_map_assembler and rviz) subscribing to /rtabmap/mapData, so use -->
-  <!-- a relay on this machine, same for images -->
-  <node name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData_optimized /rtabmap/mapData_relay"/>
-  <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" />
-    
-  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/azimut3/config/azimut3_nav.rviz"/>
-  
-  <!-- Below, construct point cloud of the latest throttled data, disabled for bandwidth efficiency -->
-  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
-    <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"/>
-    <remap from="cloud"           to="voxel_cloud" />
-    
-    <param name="queue_size" type="int" value="10"/>
-    <param name="voxel_size" type="double" value="0.01"/>
-  </node>
-</launch>

launch/azimut3/az3_mapping_robot.launch

@@ -11,7 +11,7 @@
   <!-- 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="max_rate" type="double" value="5.0"/>
+      <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"/>

launch/azimut3/az3_mapping_robot_kinect_odom.launch

@@ -11,7 +11,7 @@
   <!-- 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="max_rate" type="double" value="5.0"/>
+      <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"/>

launch/azimut3/az3_mapping_robot_kinect_scan.launch

@@ -11,7 +11,7 @@
   <!-- 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="max_rate" type="double" value="5.0"/>
+      <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"/>

launch/azimut3/az3_mapping_robot_nav.launch

@@ -8,7 +8,9 @@
   <include file="$(find az3_bringup)/az3_standalone.launch"/>
 
   <!-- OpenNI -->
-  <include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
+  <include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch">
+    <arg name="data_skip" value="5" />
+  </include>
 
   <!-- SLAM (robot side) -->
   <group ns="rtabmap">
@@ -108,23 +110,24 @@
   <!-- 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="max_rate" type="double" value="4.0"/>
+      <param name="rate" type="double" value="0"/> <!-- set by data_skip above -->
+      <param name="decimation" type="int" value="4"/>
    
       <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"/>
+      <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"/>
     </node>
 
     <!-- for the planner -->
     <node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
-      <remap from="depth/image"     to="/camera/data_throttled_image_depth"/>
-      <remap from="depth/camera_info" to="/camera/data_throttled_camera_info"/>
+      <remap from="depth/image"     to="data_resized_image_depth"/>
+      <remap from="depth/camera_info" to="data_resized_camera_info"/>
       <remap from="cloud"           to="/local_costmap_cloud" />
-      <param name="decimation" type="int" value="4"/>
+      <param name="decimation" type="int" value="1"/>
       <param name="max_depth" type="double" value="6.0"/>
       <param name="noise_filter_radius" type="double" value="0.05"/>
       <param name="noise_filter_min_neighbors" type="int" value="10"/>

launch/azimut3/az3_openni.launch

@@ -1,9 +1,12 @@
 
 <launch>
 
+  <arg name="data_skip" default="0" />
+
   <!-- Xtion -->
   <include file="$(find openni2_launch)/launch/openni2.launch">
     <arg name="depth_registration" value="True" />
+    <arg name="data_skip" value="$(arg data_skip)" />
     <arg name="rgb_camera_info_url"
         value="file://$(find rtabmap_ros)/launch/calibration/rgb_PS1080_PrimeSense.yaml" />
    <arg name="depth_camera_info_url"

launch/azimut3/az3_remote_mapping_robot.launch

@@ -11,7 +11,7 @@
   <!-- 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="max_rate" type="double" value="5.0"/>
+      <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"/>

launch/azimut3/config/azimut3_nav.rviz

@@ -7,7 +7,7 @@ Panels:
         - /Global Options1
         - /TF1/Frames1
       Splitter Ratio: 0.601881
-    Tree Height: 371
+    Tree Height: 187
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Views
@@ -73,7 +73,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: AxisColor
       Decay Time: 0
-      Enabled: false
+      Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -86,10 +86,10 @@ Visualization Manager:
       Size (Pixels): 5
       Size (m): 0.01
       Style: Points
-      Topic: /base_scan
+      Topic: /base_scan_relay
       Use Fixed Frame: false
       Use rainbow: true
-      Value: false
+      Value: true
     - Alpha: 0.7
       Class: rviz/Map
       Color Scheme: map
@@ -131,8 +131,8 @@ Visualization Manager:
       Value: true
       Visual Enabled: true
     - Class: rviz/Image
-      Enabled: false
-      Image Topic: /camera/data_throttled_image
+      Enabled: true
+      Image Topic: /camera/data_resized_image_relay
       Max Value: 1
       Median window: 5
       Min Value: 0
@@ -140,7 +140,7 @@ Visualization Manager:
       Normalize Range: true
       Queue Size: 2
       Transport Hint: theora
-      Value: false
+      Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -158,7 +158,7 @@ Visualization Manager:
       Download graph: false
       Download map: false
       Enabled: true
-      Filter floor (m): 0.1
+      Filter floor (m): 0.07
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -180,26 +180,12 @@ Visualization Manager:
       Name: Info
       Topic: /rtabmap/info
       Value: true
-    - Alpha: 1
-      Axes Length: 1
-      Axes Radius: 0.1
-      Class: rviz/Pose
-      Color: 255; 25; 0
-      Enabled: true
-      Head Length: 0.3
-      Head Radius: 0.1
-      Name: Pose
-      Shaft Length: 1
-      Shaft Radius: 0.05
-      Shape: Arrow
-      Topic: /rtabmap/goal
-      Value: true
     - Alpha: 1
       Buffer Length: 1
       Class: rviz/Path
       Color: 255; 149; 57
       Enabled: true
-      Name: Path
+      Name: move_base global plan
       Topic: /planner/move_base/NavfnROS/plan
       Value: true
     - Alpha: 1
@@ -207,7 +193,7 @@ Visualization Manager:
       Class: rviz/Path
       Color: 2; 14; 255
       Enabled: true
-      Name: Path
+      Name: move_base local plan
       Topic: /planner/move_base/TrajectoryPlannerROS/local_plan
       Value: true
     - Alpha: 1
@@ -222,36 +208,36 @@ Visualization Manager:
       Color: 85; 255; 0
       Color Transformer: RGB8
       Decay Time: 0
-      Enabled: false
+      Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
       Min Color: 0; 0; 0
       Min Intensity: 0
-      Name: PointCloud2
+      Name: Voxel_cloud
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
       Size (Pixels): 3
       Size (m): 0.01
       Style: Points
-      Topic: /rtabmap/cloud_map
+      Topic: /voxel_cloud
       Use Fixed Frame: true
       Use rainbow: true
-      Value: false
+      Value: true
     - Alpha: 1
       Class: rtabmap_ros/MapGraph
       Color: 0; 0; 255
-      Enabled: false
+      Enabled: true
       Name: MapGraph
       Topic: /rtabmap/mapData_relay
-      Value: false
+      Value: true
     - Alpha: 1
       Buffer Length: 1
       Class: rviz/Path
       Color: 255; 0; 255
       Enabled: true
-      Name: Path
+      Name: Rtabmap global path
       Topic: /rtabmap/global_path
       Value: true
     - Alpha: 1
@@ -259,7 +245,7 @@ Visualization Manager:
       Class: rviz/Path
       Color: 85; 255; 255
       Enabled: true
-      Name: Path
+      Name: Rtabmap local path
       Topic: /rtabmap/local_path
       Value: true
     - Alpha: 1
@@ -270,12 +256,26 @@ Visualization Manager:
       Enabled: true
       Head Length: 0.3
       Head Radius: 0.1
-      Name: Pose
+      Name: Current goal
       Shaft Length: 1
       Shaft Radius: 0.05
       Shape: Arrow
       Topic: /rtabmap/current_goal
       Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.1
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.3
+      Head Radius: 0.1
+      Name: Global goal
+      Shaft Length: 1
+      Shaft Radius: 0.05
+      Shape: Arrow
+      Topic: /rtabmap/goal
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -296,22 +296,22 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 17.1662
+      Distance: 13.3786
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.0852849
-        Y: 0.0904403
-        Z: -0.189729
+        X: -1.4347
+        Y: 1.38269
+        Z: -0.0856586
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 1.2048
+      Pitch: 0.884797
       Target Frame: base_footprint
       Value: Orbit (rviz)
-      Yaw: 3.50041
+      Yaw: 3.81544
     Saved: ~
 Window Geometry:
   Displays:
@@ -321,7 +321,7 @@ Window Geometry:
   Hide Right Dock: false
   Image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000151000002dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001b4000000dd00fffffffb0000000a0049006d00610067006500000001290000012d0000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007301000001e2000001200000006400ffffff000000010000010f000001b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001b2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000151000002dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000000fc000000dd00fffffffb0000000a0049006d006100670065010000012a0000012d0000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000025d000000a50000006400ffffff000000010000010f000001b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001b2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -331,5 +331,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1228
-  X: 372
-  Y: 32
+  X: 406
+  Y: 154

launch/data_recorder.launch

@@ -22,7 +22,7 @@
 <!-- Throttling messages -->
   <node pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager"/>
   <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle standalone_nodelet" output="screen">
-    <param name="max_rate" type="double" value="10.0"/>
+    <param name="rate" type="double" value="10.0"/>
    
     <remap from="rgb/image_in"       to="/camera/rgb/image_rect_color"/>
     <remap from="depth/image_in"     to="/camera/depth_registered/image_raw"/>

launch/demo/demo_multi-session_mapping.launch

@@ -51,6 +51,7 @@
 	<param name="Bayes/FullPredictionUpdate" type="string" value="true"/>
 	<param name="Kp/DetectorStrategy" type="string" value="0"/>   <!-- use SURF -->
 	<param name="Kp/NNStrategy" type="string" value="1"/>         <!-- kdTree -->
+	<param name="Kp/WordsPerImage" type="string" value="400"/> 
   </node>
   
   <!-- Visualisation RTAB-Map -->

launch/tests/bumblebee.launch

@@ -18,7 +18,7 @@
   	<!-- Run the ROS package stereo_image_proc -->
 	<group ns="/stereo_camera" >
 		<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc"/>
-	    <node pkg="nodelet" type="nodelet" name="disparity2depth" args="standalone rtabmap/disparity_to_depth"/>
+	    <node pkg="nodelet" type="nodelet" name="disparity2depth" args="standalone rtabmap_ros/disparity_to_depth"/>
 	</group>
 	
 	

launch/tests/test_odometry.launch

@@ -19,7 +19,7 @@
    <node pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
    
   <!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
-  <node pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap/data_odom_sync standalone_nodelet">
+  <node pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
     <remap from="rgb/image_in"       to="camera/rgb/image_rect_color"/>
     <remap from="depth/image_in"     to="camera/depth_registered/image_raw"/>
     <remap from="rgb/camera_info_in" to="camera/depth_registered/camera_info"/>
@@ -32,7 +32,7 @@
     
     <param name="queue_size" type="int" value="30"/>
   </node>
-  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap/point_cloud_xyzrgb standalone_nodelet">
+  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
     <remap from="rgb/image"       to="data_odom_sync/image"/>
     <remap from="depth/image"     to="data_odom_sync/depth"/>
     <remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>

launch/tests/test_pointxyzrgb.launch

@@ -1,20 +1,69 @@
 
 <launch>
    
+  <include file="$(find freenect_launch)/launch/freenect.launch" >
+    <arg name="depth_registration" value="true"/>
+    <arg name="data_skip" value="3"/>
+  </include>
   
-  <!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
-  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap/point_cloud_xyzrgb">
-    <remap from="rgb/image"       to="camera/rgb/image_rect_color"/>
-    <remap from="depth/image"     to="camera/depth_registered/image_raw"/>
-    <remap from="rgb/camera_info" to="camera/depth_registered/camera_info"/>
-    <remap from="cloud"           to="/voxel_cloud" />
+  <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="queue_size" type="int" value="30"/>
+    <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"/>
+    <remap from="rgb/camera_info" to="rgb/camera_info_out"/>
+    <remap from="cloud"           to="/voxel_cloud" />
+
     <param name="voxel_size" type="double" value="0.02"/>
     <param name="decimation" type="int" value="2"/>
-
-    <param name="rgb/image_transport" type="string" value="compressed"/>
-    <param name="depth/image_transport" type="string" value="compressedDepth"/>
   </node>
+  </group>
+
+  <!-- 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"/>
+         <remap from="left/camera_info"  to="left/camera_info_throttle"/>
+         <remap from="right/image_raw"   to="right/image_raw_throttle_relay"/>
+         <remap from="right/camera_info" to="right/camera_info_throttle"/>
+         <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"/>
+    <remap from="left/camera_info"   to="/stereo_camera/left/camera_info_throttle_throttle"/>
+    <remap from="right/camera_info"  to="/stereo_camera/right/camera_info_throttle_throttle"/>
+    <remap from="cloud"              to="/voxel_cloud" />
+
+    <param name="decimation" type="int" value="2"/>
+    <param name="approx_sync" type="bool" value="false"/>
+  </node>
+-->
 
 </launch>