From 85268d5d42c64e57ec7d04e43726bf870fc94f7c Mon Sep 17 00:00:00 2001
From: Mathieu Labbe <matlabbe@gmail.com>
Date: Tue, 3 Feb 2015 14:23:32 -0500
Subject: [PATCH] updated azimut3 navigation launch with rtabmap's planner

---
 launch/azimut3/az3_mapping_client_nav.launch  |  2 +-
 launch/azimut3/az3_mapping_client_nav.launch~ | 23 +++++
 launch/azimut3/az3_mapping_robot_nav.launch   | 83 +++++++++----------
 launch/azimut3/config/azimut3_nav.rviz        | 42 +++++-----
 4 files changed, 86 insertions(+), 64 deletions(-)
 create mode 100644 launch/azimut3/az3_mapping_client_nav.launch~

diff --git a/launch/azimut3/az3_mapping_client_nav.launch b/launch/azimut3/az3_mapping_client_nav.launch
index 0e4c15ef..20b803e4 100644
--- a/launch/azimut3/az3_mapping_client_nav.launch
+++ b/launch/azimut3/az3_mapping_client_nav.launch
@@ -3,7 +3,7 @@
 
   <!-- 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="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData /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" />
diff --git a/launch/azimut3/az3_mapping_client_nav.launch~ b/launch/azimut3/az3_mapping_client_nav.launch~
new file mode 100644
index 00000000..0e4c15ef
--- /dev/null
+++ b/launch/azimut3/az3_mapping_client_nav.launch~
@@ -0,0 +1,23 @@
+
+<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>
diff --git a/launch/azimut3/az3_mapping_robot_nav.launch b/launch/azimut3/az3_mapping_robot_nav.launch
index 178fc716..daba5f0e 100644
--- a/launch/azimut3/az3_mapping_robot_nav.launch
+++ b/launch/azimut3/az3_mapping_robot_nav.launch
@@ -75,50 +75,52 @@
   <!-- 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="frame_id" type="string" value="base_footprint"/>
 	
-	  <param name="subscribe_depth" type="bool" value="true"/>
-	  <param name="subscribe_laserScan" 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="mapData" to="mapData"/>
+      <remap from="odom" to="/base_controller/odom"/>
+      <remap from="scan" to="/base_scan"/>
+      <remap from="mapData" to="mapData"/>
 
-	  <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="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="goal_pose" to="/planner_goal"/>	
+
+      <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. -->
-	  <param name="RGBD/ScanMatchingSize" type="string" value="1"/>           
-	  <param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>
+      <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
+      <param name="RGBD/PoseScanMatching" type="string" value="true"/>           
+      <param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>
   
-	  <param name="LccIcp/Type" type="string" value="2"/>                    
-	  <param name="LccIcp2/Iterations" type="string" value="100"/>
-	  <param name="LccIcp2/VoxelSize" type="string" value="0"/>
+      <param name="LccIcp/Type" type="string" value="2"/>                    
+      <param name="LccIcp2/Iterations" type="string" value="100"/>
+      <param name="LccIcp2/VoxelSize" type="string" value="0"/>
 	 
-	  <param name="RGBD/AngularUpdate" type="string" value="0.01"/>             <!-- Update map only if the robot is moving -->
-	  <param name="RGBD/LinearUpdate" type="string" value="0.01"/>              <!-- Update map only if the robot is moving -->
+      <param name="RGBD/AngularUpdate" type="string" value="0.01"/>             <!-- Update map only if the robot is moving -->
+      <param name="RGBD/LinearUpdate" type="string" value="0.01"/>              <!-- Update map only if the robot is moving -->
 	  
-	  <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
-	  <param name="Mem/RehearsedNodesKept" type="string" value="false"/>
+      <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
+      <param name="Mem/RehearsedNodesKept" type="string" value="false"/>
 
-	  <param name="Rtabmap/TimeThr" type="string" value="700"/>
+      <param name="Rtabmap/TimeThr" type="string" value="700"/>
       <param name="Rtabmap/DetectionRate" type="string" value="1"/>
 
-	  <param name="RGBD/ToroIterations" type="string" value="0"/>
-      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
+      <param name="RGBD/ToroIterations" type="string" value="100"/>
+      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
 	 
-	  <param name="Kp/TfIdfLikelihoodUsed" type="string" value="true"/>
-	  <param name="Kp/DetectorStrategy" type="string" value="0"/>
+      <param name="Kp/TfIdfLikelihoodUsed" type="string" value="true"/>
+      <param name="Kp/DetectorStrategy" type="string" value="0"/>
       <param name="Kp/WordsPerImage" type="string" value="200"/>
       <param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
       <param name="Kp/NNStrategy" type="string" value="1"/>
 	  
-	  <param name="Bayes/FullPredictionUpdate" type="string" value="false"/>
+      <param name="Bayes/FullPredictionUpdate" type="string" value="false"/>
 	  
-	  <param name="SURF/HessianThreshold" type="string" value="1000"/>
+      <param name="SURF/HessianThreshold" type="string" value="1000"/>
 	  
       <param name="LccBow/MaxDepth" type="string" value="4"/>
       <param name="LccBow/MinInliers" type="string" value="10"/>
@@ -126,21 +128,16 @@
       
       <param name="LccReextract/Activated" type="string" value="true"/>
       <param name="LccReextract/FeatureType" type="string" value="4"/>
-	  <param name="LccReextract/MaxWords" type="string" value="400"/>
-	  <param name="LccReextract/NNDR" type="string" value="0.8"/>
-	  <param name="LccReextract/NNType" type="string" value="3"/>  
-
-      <!-- Disable graph optimization because we use map_optimizer node below -->
-      <param name="RGBD/ToroIterations" type="string" value="0"/>
+      <param name="LccReextract/MaxWords" type="string" value="400"/>
+      <param name="LccReextract/NNDR" type="string" value="0.8"/>
+      <param name="LccReextract/NNType" type="string" value="3"/> 
     </node>
-    <!-- to always have the global map optimized -->
-    <node pkg="rtabmap_ros" type="map_optimizer" name="map_optimizer"/>
+  
+  
+    <!-- Grid map assembler for planner -->
+    <node pkg="rtabmap_ros" type="grid_map_assembler" name="grid_map_assembler" output="screen">
+       <remap from="grid_map" to="/map"/>
+    </node>
+
   </group>
-  
-  <!-- Grid map assembler for rviz -->
-  <node pkg="rtabmap_ros" type="grid_map_assembler" name="grid_map_assembler" output="screen">
-    <remap from="mapData" to="rtabmap/mapData_optimized"/>
-    <remap from="grid_map" to="map"/>
-  </node>
-  
 </launch>
diff --git a/launch/azimut3/config/azimut3_nav.rviz b/launch/azimut3/config/azimut3_nav.rviz
index acae6684..00bb2583 100644
--- a/launch/azimut3/config/azimut3_nav.rviz
+++ b/launch/azimut3/config/azimut3_nav.rviz
@@ -6,6 +6,8 @@ Panels:
       Expanded:
         - /Global Options1
         - /TF1/Frames1
+        - /Info1/Status1
+        - /MapGraph1
       Splitter Ratio: 0.601881
     Tree Height: 337
   - Class: rviz/Selection
@@ -20,7 +22,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: PointCloud2
+    SyncSource: Image
   - Class: rviz/Tool Properties
     Expanded:
       - /2D Pose Estimate1
@@ -125,8 +127,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: -0.904062
-        Min Value: -2.17531
+        Max Value: 5.19727
+        Min Value: -0.547517
         Value: true
       Axis: X
       Channel Name: intensity
@@ -326,13 +328,6 @@ Visualization Manager:
       Name: Path
       Topic: /planner/move_base/TrajectoryPlannerROS/local_plan
       Value: true
-    - Alpha: 1
-      class: rtabmap_ros/MapGraph
-      Color: 0; 0; 255
-      Enabled: true
-      Name: MapGraph
-      Topic: /rtabmap/mapData_relay
-      Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -345,7 +340,7 @@ Visualization Manager:
       Color: 85; 255; 0
       Color Transformer: RGB8
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -361,6 +356,13 @@ Visualization Manager:
       Topic: /voxel_cloud
       Use Fixed Frame: true
       Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Class: rtabmap_ros/MapGraph
+      Color: 255; 0; 0
+      Enabled: true
+      Name: MapGraph
+      Topic: /rtabmap/mapData_relay
       Value: true
   Enabled: true
   Global Options:
@@ -381,23 +383,23 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      class: rtabmap_ros/OrbitOriented
-      Distance: 25.5693
+      Class: rviz/Orbit
+      Distance: 14.3083
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 1.29372
-        Y: -0.0308032
-        Z: -0.622506
+        X: 1.91382
+        Y: -0.564774
+        Z: 0.661231
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 1.3098
-      Target Frame: base_footprint
-      Value: OrbitOriented (rtabmap)
-      Yaw: 3.11245
+      Pitch: 1.4054
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.1704
     Saved: ~
 Window Geometry:
   Displays: