diff --git a/launch/azimut3/az3_mapping_robot_nav.launch b/launch/azimut3/az3_mapping_robot_nav.launch
index f2df4ea5..a19d4638 100644
--- a/launch/azimut3/az3_mapping_robot_nav.launch
+++ b/launch/azimut3/az3_mapping_robot_nav.launch
@@ -28,6 +28,7 @@
 
       <remap from="goal_out" to="current_goal"/>	
       <remap from="move_base" to="/planner/move_base"/>
+      <remap from="grid_map" to="/map"/>
 
       <param name="use_action_for_goal" type="bool" value="true"/>
       <param name="queue_size" type="int" value="10"/>
@@ -65,13 +66,6 @@
       <param name="LccBow/MinInliers" type="string" value="5"/>
       <param name="LccBow/InlierDistance" type="string" value="0.1"/>
     </node>
-  
-  
-    <!-- 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>
   
   <!-- teleop -->
@@ -85,13 +79,14 @@
   <!-- 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="map" to="/map"/>
      <remap from="move_base_simple/goal" to="/planner_goal"/>
         
      <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
     	<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_laser_only.yaml" command="load" />
+    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params_laser.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/base_local_planner_params.yaml" command="load" />
      </node>
@@ -109,4 +104,27 @@
   <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/data_throttle camera_nodelet_manager" output="screen">
+      <param name="max_rate" type="double" value="2.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>
+
+    <!-- for the planner -->
+    <node pkg="nodelet" type="nodelet" name="points_xyzrgb_planner" args="load rtabmap/point_cloud_xyzrgb camera_nodelet_manager">
+      <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="cloud"           to="/local_costmap_cloud" />
+      <param name="decimation" type="int" value="4"/>
+    </node>  
+  </group>
 </launch>
diff --git a/launch/azimut3/config/costmap_common_params.yaml b/launch/azimut3/config/costmap_common_params.yaml
index 12f69fe0..45253e38 100644
--- a/launch/azimut3/config/costmap_common_params.yaml
+++ b/launch/azimut3/config/costmap_common_params.yaml
@@ -3,7 +3,7 @@ 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
 #robot_radius: ir_of_robot
-inflation_radius: 0.55
+inflation_radius: 0.60
 transform_tolerance: 1
 
 controller_patience: 2.0
diff --git a/launch/azimut3/config/local_costmap_params_laser.yaml b/launch/azimut3/config/local_costmap_params_laser.yaml
new file mode 100644
index 00000000..dd2fc475
--- /dev/null
+++ b/launch/azimut3/config/local_costmap_params_laser.yaml
@@ -0,0 +1,32 @@
+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
+
+  #observation_sources: laser_scan_sensor
+  observation_sources: laser_scan_sensor point_cloud_sensor
+
+  laser_scan_sensor: {
+    data_type: LaserScan, 
+    topic: base_scan, 
+    expected_update_rate: 0.2, 
+    marking: true, 
+    clearing: true}
+
+  point_cloud_sensor: {
+    sensor_frame: base_footprint,
+    data_type: PointCloud2, 
+    topic: openni_points, 
+    expected_update_rate: 0.5, 
+    marking: true, 
+    clearing: true,
+    min_obstacle_height: 0.05,
+    max_obstacle_height: 0.4}
diff --git a/launch/azimut3/config/local_costmap_params_laser_only.yaml b/launch/azimut3/config/local_costmap_params_laser_only.yaml
deleted file mode 100644
index a8a2be41..00000000
--- a/launch/azimut3/config/local_costmap_params_laser_only.yaml
+++ /dev/null
@@ -1,33 +0,0 @@
-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
-
-  observation_sources: laser_scan_sensor
-  #observation_sources: laser_scan_sensor point_cloud_sensor
-
-  laser_scan_sensor: {
-    data_type: LaserScan, 
-    topic: base_scan, 
-    expected_update_rate: 0.2, 
-    marking: true, 
-    clearing: true}
-
-  # assuming receiving a cloud from rtabmap/obstacles_detection node
-  #point_cloud_sensor: {
-  #  sensor_frame: base_footprint,
-  #  data_type: PointCloud2, 
-  #  topic: openni_points, 
-  #  expected_update_rate: 0.5, 
-  #  marking: true, 
-  #  clearing: true,
-  #  min_obstacle_height: -99999.0,
-  #  max_obstacle_height: 99999.0}