diff --git a/launch/azimut3/az3_mapping_robot_nav.launch b/launch/azimut3/az3_mapping_robot_nav.launch
index 278d7252..751ac89f 100644
--- a/launch/azimut3/az3_mapping_robot_nav.launch
+++ b/launch/azimut3/az3_mapping_robot_nav.launch
@@ -18,9 +18,11 @@
       <param name="use_action_for_goal" type="bool" value="true"/>
       <param name="cloud_decimation" type="int" value="1"/>                    <!-- we already decimate in memory below -->
       <param name="grid_eroded" type="bool" value="true"/>
+      <param name="grid_cell_size" type="double" value="0.05"/>
+      <param name="scan_inf_is_valid" type="bool" value="true"/>
 
       <remap from="odom" to="/base_controller/odom"/>
-      <remap from="scan" to="/base_scan"/>
+      <remap from="scan" to="/scan"/>
       <remap from="mapData" to="mapData"/>
 
       <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
@@ -78,8 +80,9 @@
 
   <!-- ROS navigation stack move_base -->
   <group ns="planner">
-     <remap from="base_scan" to="/base_scan"/>
-     <remap from="openni_points" to="/local_costmap_cloud"/>
+     <remap from="base_scan" to="/scan"/>
+     <remap from="obstacles_cloud" to="/obstacles_cloud"/>
+     <remap from="ground_cloud" to="/ground_cloud"/>
      <remap from="map" to="/map"/>
      <remap from="move_base_simple/goal" to="/planner_goal"/>
         
@@ -87,8 +90,8 @@
         <param name="base_global_planner" value="navfn/NavfnROS"/>
     	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
      	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
-    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params_2d.yaml" command="load" />
-    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" />
+    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params_2d.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>
    		
@@ -109,7 +112,7 @@
   <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"/>
-      <param name="decimation" type="int" value="4"/>
+      <param name="decimation" type="int" value="2"/>
    
       <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
       <remap from="depth/image_in"     to="depth_registered/image_raw"/>
@@ -124,11 +127,22 @@
     <node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
       <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" />
+      <remap from="cloud"                  to="cloudXYZ" />
       <param name="decimation" type="int" value="1"/>                     <!-- already decimated above -->
-      <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"/>
+      <param name="max_depth" type="double" value="3.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"/>
     </node>  
   </group>
 </launch>
diff --git a/launch/azimut3/config/base_local_planner_params.yaml b/launch/azimut3/config/base_local_planner_params.yaml
index c21ee0ae..217ea3bc 100644
--- a/launch/azimut3/config/base_local_planner_params.yaml
+++ b/launch/azimut3/config/base_local_planner_params.yaml
@@ -3,24 +3,39 @@ TrajectoryPlannerROS:
   # Current limits based on AZ3 standalone configuration.
   acc_lim_x:  0.75
   acc_lim_y:  0.75
-  acc_lim_theta: 4.00
+  acc_lim_theta: 4
   # min_vel_x and max_rotational_vel were set to keep the ICR at
   # minimal distance of 0.48 m. 
   # Basically, max_rotational_vel * rho_min <= min_vel_x
-  max_vel_x:  0.700
+  max_vel_x:  0.7
   min_vel_x:  0.24
-  max_rotational_vel: 0.5
-  min_in_place_rotational_vel: 0.15
+  max_vel_theta: 0.5
+  min_vel_theta: -0.5
+  min_in_place_vel_theta: 0.25
+  holonomic_robot: true
 
-  xy_goal_tolerance:  0.20
-  yaw_goal_tolerance: 0.20
-
-  sim_time: 1.5
-  vtheta_samples: 30
+  xy_goal_tolerance:  0.25
+  yaw_goal_tolerance: 0.25
+  latch_xy_goal_tolerance: true
+  
+  # make sure that the minimum velocity multiplied by the sim_period is less than twice the tolerance on a goal. Otherwise, the robot will prefer to rotate in place just outside of range of its target position rather than moving towards the goal.
+  sim_time: 1.5 # set between 1 and 2. The higher he value, the smoother the path (though more samples would be required).
+  sim_granularity: 0.025
+  angular_sim_granularity: 0.05
+  vx_samples: 12
+  vtheta_samples: 20
 
   meter_scoring: true
 
+  pdist_scale: 0.95 # The higher will follow more the global path.
+  gdist_scale: 0.2
+  occdist_scale: 0.01
+  publish_cost_grid_pc: false
+
+#move_base
+controller_frequency: 10.0 #The robot can move faster when higher.
+
 #global planner 
 NavfnROS:
-    allow_unknown: false
-    visualize_potential: true
+    allow_unknown: true
+    visualize_potential: false
diff --git a/launch/azimut3/config/costmap_common_params.yaml b/launch/azimut3/config/costmap_common_params.yaml
index 1867d026..af1db8b5 100644
--- a/launch/azimut3/config/costmap_common_params.yaml
+++ b/launch/azimut3/config/costmap_common_params.yaml
@@ -1,9 +1,9 @@
-obstacle_range: 2.5
-raytrace_range: 3.0
 footprint: [[ 0.3,  0.3], [-0.3,  0.3], [-0.3, -0.3], [ 0.3, -0.3]]
-footprint_padding: 0.03
+footprint_padding: 0.02
+#robot_radius: 0.38
 #robot_radius: ir_of_robot
-inflation_radius: 0.55
+inflation_layer:
+  inflation_radius: 0.7 # 2xfootprint, it helps to keep the global planned path farther from obstacles
 transform_tolerance: 2
 
 
diff --git a/launch/azimut3/config/global_costmap_params.yaml b/launch/azimut3/config/global_costmap_params.yaml
index 8e3cea3f..8b6b87f2 100644
--- a/launch/azimut3/config/global_costmap_params.yaml
+++ b/launch/azimut3/config/global_costmap_params.yaml
@@ -1,10 +1,10 @@
-global_costmap:
-  global_frame: map
-  robot_base_frame: base_footprint
-  update_frequency: 1
-  publish_frequency: 1
-  always_send_full_costmap: true
-  plugins: [
-    {name: static_layer, type: "rtabmap_ros::StaticLayer"},
-    {name: inflation_layer, type: "costmap_2d::InflationLayer"}
-  ]
+#global_costmap
+global_frame: map
+robot_base_frame: base_footprint
+update_frequency: 1
+publish_frequency: 2
+always_send_full_costmap: false
+plugins:
+   - {name: static_layer, type: "rtabmap_ros::StaticLayer"}
+   - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+
diff --git a/launch/azimut3/config/local_costmap_params_2d.yaml b/launch/azimut3/config/local_costmap_params_2d.yaml
index 0dfbfb42..df421a87 100644
--- a/launch/azimut3/config/local_costmap_params_2d.yaml
+++ b/launch/azimut3/config/local_costmap_params_2d.yaml
@@ -1,33 +1,50 @@
-local_costmap:
-  global_frame: map
-  robot_base_frame: base_footprint
-  update_frequency: 2.0
-  publish_frequency: 2.0
-  static_map: false
-  rolling_window: true
-  width: 4.0
-  height: 4.0
-  resolution: 0.025
-  origin_x: -2.0
-  origin_y: -2.0
+global_frame: map
+robot_base_frame: base_footprint
+update_frequency: 2.0
+publish_frequency: 2.0
+rolling_window: true
+width: 4.0
+height: 4.0
+resolution: 0.025
+origin_x: -2.0
+origin_y: -2.0
 
-  #observation_sources: laser_scan_sensor
-  observation_sources: laser_scan_sensor point_cloud_sensor
+plugins:
+  - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer,  type: "costmap_2d::InflationLayer"}
+
+obstacle_layer:
+  obstacle_range: 2.5
+  raytrace_range: 3.0
+  max_obstacle_height: 0.4
+ 
+  observation_sources: laser_scan_sensor point_cloud_sensorA point_cloud_sensorB
 
   laser_scan_sensor: {
     data_type: LaserScan, 
     topic: base_scan, 
     expected_update_rate: 0.2, 
     marking: true, 
-    clearing: false}
+    clearing: true
+  }
 
-  point_cloud_sensor: {
+  point_cloud_sensorA: {
     sensor_frame: base_footprint,
     data_type: PointCloud2, 
-    topic: openni_points, 
+    topic: obstacles_cloud, 
     expected_update_rate: 0.5, 
     marking: true, 
     clearing: true,
-    min_obstacle_height: 0.05,
-    max_obstacle_height: 0.4}
+    min_obstacle_height: 0.04
+  }
+
+  point_cloud_sensorB: {
+    sensor_frame: base_footprint,
+    data_type: PointCloud2,
+    topic: ground_cloud,
+    expected_update_rate: 0.5,
+    marking: false,
+    clearing: true,
+    min_obstacle_height: -1.0 # make usre the ground is not filtered
+  }