From 80addaf9264b3057f2fb45ec60117d65b0067247 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Fri, 6 Nov 2015 16:12:28 -0500
Subject: [PATCH] Updated az3_mapping_robot_kinect_scan.launch (fixed openni
 rate)

---
 .../az3_mapping_robot_kinect_scan.launch      | 19 +++++++++++--------
 launch/azimut3/az3_openni.launch              |  3 ++-
 .../config/costmap_common_params_2d.yaml      | 10 ++++------
 .../azimut3/config/global_costmap_params.yaml |  2 +-
 .../azimut3/config/local_costmap_params.yaml  |  4 ++--
 5 files changed, 20 insertions(+), 18 deletions(-)

diff --git a/launch/azimut3/az3_mapping_robot_kinect_scan.launch b/launch/azimut3/az3_mapping_robot_kinect_scan.launch
index 546ea967..1c5158b8 100644
--- a/launch/azimut3/az3_mapping_robot_kinect_scan.launch
+++ b/launch/azimut3/az3_mapping_robot_kinect_scan.launch
@@ -2,7 +2,7 @@
 <launch>
 
   <arg name="rtabmap_args" default="" />
-  <arg name="localization_mode" default="false" />
+  <arg name="localization" default="false" />
 
   <!-- AZIMUT 3 bringup: launch motors/odometry -->
   <include file="$(find az3_bringup)/az3_standalone.launch"/>
@@ -28,8 +28,9 @@
       <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 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/LocalRadius"             type="string" value="4"/>  
       <param name="Mem/RehearsalSimilarity"      type="string" value="0.30"/>
@@ -39,7 +40,8 @@
       <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"/> 
+      <param name="LccIcp/Type"                  type="string" value="2"/>
+      <param name="LccIcp2/CorrespondenceRatio"  type="string" value="0.2"/>
     </node>
   </group>
   
@@ -53,7 +55,7 @@
 
   <!-- ROS navigation stack move_base -->
   <group ns="planner">
-     <remap from="base_scan"             to="/kinect_scan"/>
+     <remap from="scan"             to="/kinect_scan"/>
      <remap from="obstacles_cloud"       to="/obstacles_cloud"/>
      <remap from="ground_cloud"          to="/ground_cloud"/>
      <remap from="map"                   to="/map"/>
@@ -83,7 +85,7 @@
   <!-- 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="rate" type="double" value="3"/>
 
       <remap from="rgb/image_in"        to="rgb/image_rect_color"/>
       <remap from="depth/image_in"      to="depth_registered/image_raw"/>
@@ -95,7 +97,8 @@
     </node>
 
     <!-- for the planner -->
-    <node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
+    <node pkg="nodelet" type="nodelet" name="obstacle_nodelet_manager"  args="manager" output="screen"/>
+    <node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz obstacle_nodelet_manager">
       <remap from="depth/image"        to="throttled_image_depth"/>
       <remap from="depth/camera_info"  to="throttled_camera_info"/>
       <remap from="cloud"              to="cloudXYZ" />
@@ -104,7 +107,7 @@
       <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">
+    <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"/>
diff --git a/launch/azimut3/az3_openni.launch b/launch/azimut3/az3_openni.launch
index 8982436d..aae74211 100644
--- a/launch/azimut3/az3_openni.launch
+++ b/launch/azimut3/az3_openni.launch
@@ -2,6 +2,7 @@
 <launch>
 
   <!-- Xtion -->
+  <param name="/camera/driver/data_skip" value="1" />
   <include file="$(find openni2_launch)/launch/openni2.launch">
     <arg name="depth_registration" value="True" />
     <arg name="rgb_camera_info_url"
@@ -14,4 +15,4 @@
   <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>
\ No newline at end of file
+</launch>
diff --git a/launch/azimut3/config/costmap_common_params_2d.yaml b/launch/azimut3/config/costmap_common_params_2d.yaml
index 9e7325f8..9084d0d8 100644
--- a/launch/azimut3/config/costmap_common_params_2d.yaml
+++ b/launch/azimut3/config/costmap_common_params_2d.yaml
@@ -1,7 +1,5 @@
 footprint: [[ 0.3,  0.3], [-0.3,  0.3], [-0.3, -0.3], [ 0.3, -0.3]]
-footprint_padding: 0.02
-#robot_radius: 0.38
-#robot_radius: ir_of_robot
+footprint_padding: 0.04
 inflation_layer:
   inflation_radius: 0.7 # 2xfootprint, it helps to keep the global planned path farther from obstacles
 transform_tolerance: 2
@@ -16,7 +14,7 @@ obstacle_layer:
 
   laser_scan_sensor: {
     data_type: LaserScan,
-    topic: base_scan,
+    topic: scan,
     expected_update_rate: 0.1,
     marking: true,
     clearing: true
@@ -26,7 +24,7 @@ obstacle_layer:
     sensor_frame: base_footprint,
     data_type: PointCloud2,
     topic: obstacles_cloud,
-    expected_update_rate: 0.2,
+    expected_update_rate: 0.5,
     marking: true,
     clearing: true,
     min_obstacle_height: 0.04
@@ -36,7 +34,7 @@ obstacle_layer:
     sensor_frame: base_footprint,
     data_type: PointCloud2,
     topic: ground_cloud,
-    expected_update_rate: 0.2,
+    expected_update_rate: 0.5,
     marking: false,
     clearing: true,
     min_obstacle_height: -1.0 # make sure the ground is not filtered
diff --git a/launch/azimut3/config/global_costmap_params.yaml b/launch/azimut3/config/global_costmap_params.yaml
index 643ec7e6..12895d12 100644
--- a/launch/azimut3/config/global_costmap_params.yaml
+++ b/launch/azimut3/config/global_costmap_params.yaml
@@ -2,7 +2,7 @@
 global_frame: map
 robot_base_frame: base_footprint
 update_frequency: 1
-publish_frequency: 2
+publish_frequency: 1
 always_send_full_costmap: false
 plugins:
    - {name: static_layer, type: "rtabmap_ros::StaticLayer"}
diff --git a/launch/azimut3/config/local_costmap_params.yaml b/launch/azimut3/config/local_costmap_params.yaml
index 1d7cf928..87950d24 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: 5
-publish_frequency: 2
+update_frequency: 2
+publish_frequency: 1
 rolling_window: true
 width: 3.0
 height: 3.0