updated launch rviz launch files to use RTAB-Map MapCloud plugin

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1465 f169173b-cf89-36c8-b27e-44dbe73f0c83

launch/azimut3/az3_mapping_client_rviz.launch

@@ -2,14 +2,6 @@
 <launch>
 
   <group ns="rtabmap">
-    <!-- Visualisation (client side) -->
-    <!-- Map assembler for rviz -->
-    <node pkg="rtabmap" type="map_assembler" name="map_assembler"  output="screen">
-      <param name="cloud_decimation" type="int" value="4"/>
-      <param name="cloud_voxel_size" type="double" value="0.02"/>
-      <param name="scan_voxel_size" type="double" value="0.01"/>
-    </node>
-  
     <!-- Grid map assembler for rviz -->
     <node pkg="rtabmap" type="grid_map_assembler" name="grid_map_assembler" output="screen"/>
   </group>

launch/config/azimut3.rviz

@@ -26,7 +26,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: LaserScan
+    SyncSource: ""
 Visualization Manager:
   Class: ""
   Displays:
@@ -98,8 +98,6 @@ Visualization Manager:
           Value: true
         camera_rgb_optical_frame:
           Value: true
-        map:
-          Value: true
         odom:
           Value: true
         wheelLB_linkWheel_link:
@@ -124,62 +122,9 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        map:
+        {}
-          odom:
-            base_footprint:
-              base_link:
-                base_laser_link:
-                  {}
-                camera_link:
-                  camera_depth_frame:
-                    camera_depth_optical_frame:
-                      {}
-                  camera_rgb_frame:
-                    camera_rgb_optical_frame:
-                      {}
-                wheelLB_linkWheel_link:
-                  wheelLB_wheel_link:
-                    {}
-                wheelLF_linkWheel_link:
-                  wheelLF_wheel_link:
-                    {}
-                wheelRB_linkWheel_link:
-                  wheelRB_wheel_link:
-                    {}
-                wheelRF_linkWheel_link:
-                  wheelRF_wheel_link:
-                    {}
       Update Interval: 0
       Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 2.3372
-        Min Value: -0.700095
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: RGB8
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.01
-      Style: Points
-      Topic: /rtabmap/assembled_clouds
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -288,6 +233,36 @@ Visualization Manager:
       Update Interval: 0
       Value: true
       Visual Enabled: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rtabmap/MapCloud
+      Cloud decimation: 4
+      Cloud max depth (m): 4
+      Cloud voxel size (m): 0.02
+      Color: 255; 255; 255
+      Color Transformer: ""
+      Download map: false
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: MapCloud
+      Position Transformer: ""
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /rtabmap/mapData
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -341,5 +316,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1273
-  X: 115
+  X: 108
   Y: 202

launch/config/rgbd.rviz

@@ -6,7 +6,7 @@ Panels:
       Expanded:
         - /Global Options1
         - /TF1/Frames1
-      Splitter Ratio: 0.5
+      Splitter Ratio: 0.411058
     Tree Height: 518
   - Class: rviz/Selection
     Name: Selection
@@ -25,7 +25,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: MapCloud
 Visualization Manager:
   Class: ""
   Displays:
@@ -45,7 +45,7 @@ Visualization Manager:
         Z: 0
       Plane: XY
       Plane Cell Count: 10
-      Reference Frame: odom
+      Reference Frame: map
       Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
@@ -81,49 +81,80 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: false
+        base_footprint:
+          Value: true
+        base_laser_link:
+          Value: true
+        base_link:
+          Value: true
+        camera_depth_frame:
+          Value: true
+        camera_depth_optical_frame:
+          Value: true
+        camera_link:
+          Value: true
+        camera_rgb_frame:
+          Value: true
+        camera_rgb_optical_frame:
+          Value: true
+        map:
+          Value: true
+        odom:
+          Value: true
+        wheelLB_linkWheel_link:
+          Value: true
+        wheelLB_wheel_link:
+          Value: true
+        wheelLF_linkWheel_link:
+          Value: true
+        wheelLF_wheel_link:
+          Value: true
+        wheelRB_linkWheel_link:
+          Value: true
+        wheelRB_wheel_link:
+          Value: true
+        wheelRF_linkWheel_link:
+          Value: true
+        wheelRF_wheel_link:
+          Value: true
       Marker Scale: 1
       Name: TF
       Show Arrows: true
       Show Axes: true
       Show Names: true
       Tree:
-        {}
+        map:
+          odom:
+            base_footprint:
+              base_link:
+                base_laser_link:
+                  {}
+                camera_link:
+                  camera_depth_frame:
+                    camera_depth_optical_frame:
+                      {}
+                  camera_rgb_frame:
+                    camera_rgb_optical_frame:
+                      {}
+                wheelLB_linkWheel_link:
+                  wheelLB_wheel_link:
+                    {}
+                wheelLF_linkWheel_link:
+                  wheelLF_wheel_link:
+                    {}
+                wheelRB_linkWheel_link:
+                  wheelRB_wheel_link:
+                    {}
+                wheelRF_linkWheel_link:
+                  wheelRF_wheel_link:
+                    {}
       Update Interval: 0
       Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 2.3372
+        Max Value: 8.41375
-        Min Value: -0.700095
+        Min Value: 2.13701
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: RGB8
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.01
-      Style: Points
-      Topic: /assembled_clouds
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 9.61762
-        Min Value: -10.4251
         Value: true
       Axis: X
       Channel Name: intensity
@@ -143,8 +174,8 @@ Visualization Manager:
       Selectable: true
       Size (Pixels): 3
       Size (m): 0.01
-      Style: Flat Squares
+      Style: Points
-      Topic: /jn0/base_scan
+      Topic: /base_scan
       Use Fixed Frame: false
       Use rainbow: true
       Value: true
@@ -154,12 +185,42 @@ Visualization Manager:
       Draw Behind: false
       Enabled: true
       Name: Map
-      Topic: /grid_map
+      Topic: /rtabmap/grid_map
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rtabmap/MapCloud
+      Cloud decimation: 4
+      Cloud max depth (m): 4
+      Cloud voxel size (m): 0.02
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Download map: false
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: MapCloud
+      Position Transformer: XYZ
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /rtabmap/mapData
+      Use Fixed Frame: true
+      Use rainbow: true
       Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: odom
+    Fixed Frame: map
     Frame Rate: 30
   Name: root
   Tools:
@@ -176,17 +237,22 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 6.3839
+      Distance: 12.2587
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
       Focal Point:
-        X: 1.21633
+        X: 2.34873
-        Y: 0.709636
+        Y: 0.212305
-        Z: -0.00735356
+        Z: -2.50326
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 0.5598
+      Pitch: 0.939797
       Target Frame: base_link
       Value: Orbit (rviz)
-      Yaw: 3.58419
+      Yaw: 3.45419
     Saved: ~
 Window Geometry:
   Displays:
@@ -194,7 +260,7 @@ Window Geometry:
   Height: 691
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000013600000295fc0200000006fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000000000000295000000dd00fffffffb0000000a0049006d00610067006501000002b1000000c70000000000000000000000010000010f00000312fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000312000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003bd0000029500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001b200000295fc0200000006fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000000000000295000000dd00fffffffb0000000a0049006d00610067006501000002b1000000c70000000000000000000000010000010f00000312fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000312000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003410000029500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -204,5 +270,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1273
-  X: 212
+  X: 121
   Y: 201

launch/demo/demo_robot_mapping_rviz.launch

@@ -7,49 +7,44 @@
 
   <param name="use_sim_time" type="bool" value="True"/>
 
-  <!-- SLAM (robot side) -->
+  <group ns="rtabmap">
-  <!-- args: "delete_db_on_start" and "udebug" -->
+    <!-- SLAM (robot side) -->
-  <node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
+    <!-- args: "delete_db_on_start" and "udebug" -->
-	<param name="frame_id" type="string" value="base_footprint"/>
+    <node name="rtabmap" pkg="rtabmap" 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_depth" type="bool" value="true"/>
-	<param name="subscribe_laserScan" type="bool" value="true"/>
+	  <param name="subscribe_laserScan" type="bool" value="true"/>
 	
-	<remap from="odom" to="/az3/base_controller/odom"/>
+	  <remap from="odom" to="/az3/base_controller/odom"/>
-	<remap from="scan" to="/jn0/base_scan"/>
+	  <remap from="scan" to="/jn0/base_scan"/>
 
-	<remap from="rgb/image" to="data_throttled_image"/>
+	  <remap from="rgb/image" to="/data_throttled_image"/>
-  	<remap from="depth/image" to="data_throttled_image_depth"/>
+  	  <remap from="depth/image" to="/data_throttled_image_depth"/>
-  	<remap from="rgb/camera_info" to="data_throttled_camera_info"/>
+  	  <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
   	
-  	<param name="rgb/image_transport" type="string" value="compressed"/>
+      <param name="rgb/image_transport" type="string" value="compressed"/>
-    <param name="depth/image_transport" type="string" value="compressedDepth"/>
+      <param name="depth/image_transport" type="string" value="compressedDepth"/>
 	
-	<param name="queue_size" type="int" value="10"/>
+	  <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. -->
+	  <!-- 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/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 -->
+	  <param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>  <!-- Local loop closure detection (using estimated position) with locations in WM -->
-	<param name="RGBD/LocalLoopDetectionTime" type="string" value="true"/>   <!-- Local loop closure detection with locations in STM -->
+	  <param name="RGBD/LocalLoopDetectionTime" type="string" value="true"/>   <!-- Local loop closure detection with locations in STM -->
-	<param name="LccIcp/Type" type="string" value="2"/>                      <!-- 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
+	  <param name="LccIcp/Type" type="string" value="2"/>                      <!-- 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
-	<param name="LccBow/MaxDepth" type="string" value="0.0"/>                <!-- 3D visual words maximum depth 0=infinity -->
+	  <param name="LccBow/MaxDepth" type="string" value="0.0"/>                <!-- 3D visual words maximum depth 0=infinity -->
-	<param name="LccBow/InlierDistance" type="string" value="0.1"/>          <!-- 3D visual words correspondence distance -->
+	  <param name="LccBow/InlierDistance" type="string" value="0.1"/>          <!-- 3D visual words correspondence distance -->
-  </node>
+      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>    <!-- Optimize graph from initial node so /map -> /odom transform will be generated -->
+    </node>
+    
+    <!-- Grid map assembler for rviz -->
+    <node pkg="rtabmap" type="grid_map_assembler" name="grid_map_assembler" output="screen"/>
+  </group>
   
   <!-- Visualisation -->
       
-  <!-- Map assembler for rviz-->
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap)/launch/config/rgbd.rviz" output="screen"/>
-  <node pkg="rtabmap" type="map_assembler" name="map_assembler"  output="screen" args="">
-    <param name="cloud_decimation" type="int" value="8"/>
-    <param name="cloud_max_depth" type="double" value="4"/>
-    <param name="cloud_voxel_size" type="double" value="0.02"/>
-    <param name="scan_voxel_size" type="double" value="0.01"/>
-  </node>
-  
-  <!-- Grid map assembler for rviz -->
-  <node pkg="rtabmap" type="grid_map_assembler" name="grid_map_assembler" output="screen"/>
-    
-  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap)/launch/config/rgbd.rviz"/>
   
   <node pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
    <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap/point_cloud_xyzrgb standalone_nodelet">

launch/rgbd_mapping_rviz.launch

@@ -39,14 +39,6 @@
         
   </group>
       
-  <!-- Map assembler for rviz -->
-  <node pkg="rtabmap" type="map_assembler" name="map_assembler"  output="screen">
-    <remap from="infoEx"       to="rtabmap/infoEx"/>
-    <param name="cloud_decimation" type="int" value="4"/>
-    <param name="cloud_voxel_size" type="double" value="0.02"/>
-    <param name="scan_voxel_size" type="double" value="0.01"/>
-  </node>
-    
   <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap)/launch/config/rgbd.rviz"/>
     
   <!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->

src/CoreNode.cpp

@@ -48,7 +48,10 @@ int main(int argc, char** argv)
 					if(uSplit(iter->first, '/').front().compare("Bayes") == 0 ||
 					   uSplit(iter->first, '/').front().compare("VhEp") == 0 ||
 					   uSplit(iter->first, '/').front().compare("Odom") == 0 ||
-					   uSplit(iter->first, '/').front().compare("OdomBin") == 0 ||
+					   uSplit(iter->first, '/').front().compare("FAST") == 0 ||
+					   uSplit(iter->first, '/').front().compare("BRIEF") == 0 ||
+					   uSplit(iter->first, '/').front().compare("ORB") == 0 ||
+					   uSplit(iter->first, '/').front().compare("FREAK") == 0 ||
 					   uSplit(iter->first, '/').front().compare("OdomICP") == 0)
 					{
 						parameters.erase(iter++);

src/VisualOdometryNode.cpp

@@ -320,10 +320,12 @@ int main(int argc, char *argv[])
 				for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
 				{
 					if((uSplit(iter->first, '/').front().compare("Odom") == 0 ||
-						uSplit(iter->first, '/').front().compare("OdomBin") == 0 ||
-						uSplit(iter->first, '/').front().compare("Kp") == 0 ||
 						uSplit(iter->first, '/').front().compare("SURF") == 0 ||
-						uSplit(iter->first, '/').front().compare("SIFT") == 0)
+						uSplit(iter->first, '/').front().compare("SIFT") == 0 ||
+						uSplit(iter->first, '/').front().compare("ORB") == 0 ||
+						uSplit(iter->first, '/').front().compare("FAST") == 0 ||
+						uSplit(iter->first, '/').front().compare("FREAK") == 0 ||
+						uSplit(iter->first, '/').front().compare("BRIEF") == 0)
 					   &&
 					   uSplit(iter->first, '/').front().compare("OdomICP") != 0)
 					{