From 1cba7053d047d504160fe37fe7f95410d4fbf79c Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Fri, 6 Nov 2015 14:52:37 -0500
Subject: [PATCH] Updated az3_mapping_robot_kinect_scan.launch demo

---
 .../az3_mapping_robot_kinect_scan.launch      | 169 ++++++++++++------
 .../config/costmap_common_params_2d.yaml      |   8 +-
 .../azimut3/config/local_costmap_params.yaml  |   4 +-
 3 files changed, 120 insertions(+), 61 deletions(-)

diff --git a/launch/azimut3/az3_mapping_robot_kinect_scan.launch b/launch/azimut3/az3_mapping_robot_kinect_scan.launch
index bb5406dd..546ea967 100644
--- a/launch/azimut3/az3_mapping_robot_kinect_scan.launch
+++ b/launch/azimut3/az3_mapping_robot_kinect_scan.launch
@@ -1,69 +1,128 @@
 
 <launch>
 
-  <!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
+  <arg name="rtabmap_args" default="" />
+  <arg name="localization_mode" default="false" />
+
+  <!-- AZIMUT 3 bringup: launch motors/odometry -->
   <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"/>
 
-  <!-- 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="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 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>   
-  </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="/kinect_scan"/>
+    <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_laserScan" type="bool"   value="true"/>
+      <param name="use_action_for_goal" type="bool"   value="true"/>
 
-	  <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/LocalLoopDetectionSpace" type="string" value="false"/>   <!-- Local loop closure detection (using estimated position) with locations in WM -->
-	  <param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/>    <!-- Local loop closure detection with locations in STM -->
-	  <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
-	  <param name="Kp/MaxDepth" type="string" value="4.0"/>
-	  <param name="LccIcp/Type" type="string" value="2"/>                       <!-- Loop closure transformation refining with ICP: 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
-	  <param name="LccIcp2/Iterations" type="string" value="100"/>
-	  <param name="LccIcp2/VoxelSize" type="string" value="0"/>
-	  <param name="LccIcp2/CorrespondenceRatio" type="string" value="0.5"/>
-	  <param name="LccBow/MinInliers" type="string" value="3"/>                 <!-- 3D visual words minimum inliers to accept loop closure -->
-	  <param name="LccBow/MaxDepth" type="string" value="4.0"/>                 <!-- 3D visual words maximum depth 0=infinity -->
-	  <param name="LccBow/InlierDistance" type="string" value="0.05"/>           <!-- 3D visual words correspondence distance -->
-	  <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="Rtabmap/TimeThr" type="string" value="700"/>
-	  <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>   
+      <remap from="scan"            to="/kinect_scan"/>
+      <remap from="odom"            to="/base_controller/odom"/>
+      <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="goal_out"        to="current_goal"/>	
+      <remap from="move_base"       to="/planner/move_base"/>
+      <remap from="grid_map"        to="/map"/>
+
+      <!-- RTAB-Map's parameters -->
+      <param unless="$(arg localization_mode)" name="Rtabmap/TimeThr"    type="string" value="400"/>
+      <param if="$(arg localization_mode)"     name="Memory/Incremental" type="string" value="false"/>
+      <param name="RGBD/PoseScanMatching"        type="string" value="true"/>           
+      <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"/>
+      <param name="RGBD/OptimizeFromGraphEnd"    type="string" value="true"/>
+      <param name="RGBD/OptimizeVarianceIgnored" type="string" value="false"/>
+      <param name="RGBD/PlanAngularVelocity"     type="string" value="1.0"/>  <!-- preference for path traversed forward -->
+      <param name="LccBow/Force2D"               type="string" value="true"/>
+      <param name="LccIcp/Type"                  type="string" value="2"/> 
     </node>
   </group>
+  
+  <!-- teleop -->
+  <node name="joy" pkg="joy" type="joy_node"/>
+  <group ns="teleop">
+     <remap from="joy" to="/joy"/>
+     <node name="teleop" pkg="nodelet" type="nodelet" args="standalone azimut_tools/Teleop"/>
+     <param name="cmd_eta/abtr_priority" value="50"/>
+  </group>
+
+  <!-- ROS navigation stack move_base -->
+  <group ns="planner">
+     <remap from="base_scan"             to="/kinect_scan"/>
+     <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" />
+     	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml"  command="load" ns="local_costmap" />
+    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params.yaml"      command="load" ns="local_costmap" />
+    	<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>
+
+  <node name="az3_abtr" pkg="azimut_tools" type="azimut_abtr_priority_node">
+     <remap from="abtr_cmd_eta" to="/base_controller/cmd_eta"/>
+  </node>
+
+  <!-- Arbitration between teleop and planner -->
+  <node name="register_cmd_eta" pkg="abtr_priority" type="register"
+        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"/>
+
+      <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"/>
+    </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="throttled_image_depth"/>
+      <remap from="depth/camera_info"  to="throttled_camera_info"/>
+      <remap from="cloud"              to="cloudXYZ" />
+      <param name="decimation"  type="int" value="2"/>
+      <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 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="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>   
+  </group>
 </launch>
diff --git a/launch/azimut3/config/costmap_common_params_2d.yaml b/launch/azimut3/config/costmap_common_params_2d.yaml
index 7c7e531d..9e7325f8 100644
--- a/launch/azimut3/config/costmap_common_params_2d.yaml
+++ b/launch/azimut3/config/costmap_common_params_2d.yaml
@@ -17,7 +17,7 @@ obstacle_layer:
   laser_scan_sensor: {
     data_type: LaserScan,
     topic: base_scan,
-    expected_update_rate: 0.2,
+    expected_update_rate: 0.1,
     marking: true,
     clearing: true
   }
@@ -26,7 +26,7 @@ obstacle_layer:
     sensor_frame: base_footprint,
     data_type: PointCloud2,
     topic: obstacles_cloud,
-    expected_update_rate: 0.5,
+    expected_update_rate: 0.2,
     marking: true,
     clearing: true,
     min_obstacle_height: 0.04
@@ -36,10 +36,10 @@ obstacle_layer:
     sensor_frame: base_footprint,
     data_type: PointCloud2,
     topic: ground_cloud,
-    expected_update_rate: 0.5,
+    expected_update_rate: 0.2,
     marking: false,
     clearing: true,
-    min_obstacle_height: -1.0 # make usre the ground is not filtered
+    min_obstacle_height: -1.0 # make sure the ground is not filtered
   }
 
 
diff --git a/launch/azimut3/config/local_costmap_params.yaml b/launch/azimut3/config/local_costmap_params.yaml
index 87950d24..1d7cf928 100644
--- a/launch/azimut3/config/local_costmap_params.yaml
+++ b/launch/azimut3/config/local_costmap_params.yaml
@@ -1,7 +1,7 @@
 global_frame: odom
 robot_base_frame: base_footprint
-update_frequency: 2
-publish_frequency: 1
+update_frequency: 5
+publish_frequency: 2
 rolling_window: true
 width: 3.0
 height: 3.0