ros-pkg: working navigation with stereo camera only and obstacles avoidance (az3_mapping_stereo_nav.launch)

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1924 f169173b-cf89-36c8-b27e-44dbe73f0c83

launch/azimut3/az3_mapping_robot_stereo_nav.launch

@@ -62,7 +62,7 @@
 	<node pkg="tf" type="static_transform_publisher" name="stereo_camera_base_link"
 	    args="$(arg optical_rotate) stereo_camera_base stereo_camera 100" />  
 	<node pkg="tf" type="static_transform_publisher" name="base_to_stereo_camera_base_link"
-	    args="0.01 0.06 0.90 0 0.2 0 base_link stereo_camera_base 100" />  
+	    args="0.01 0.06 0.90 0 0.45 0 base_link stereo_camera_base 100" />  
 	    
 	<!-- Run the ROS package stereo_image_proc for image rectification-->
 	<group ns="/stereo_camera" >
@@ -84,17 +84,28 @@
 			<remap from="left/camera_info"  to="left/camera_info_throttle"/>
 			<remap from="right/image_raw"   to="right/image_raw_throttle"/>
 			<remap from="right/camera_info" to="right/camera_info_throttle"/>
+			<param name="disparity_range" value="128"/>
 		</node>
 		
 		<!-- Create point cloud for the planner -->
 		<node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap/point_cloud_xyz stereo_nodelet">
 			<remap from="disparity/image"       to="disparity"/>
 			<remap from="disparity/camera_info" to="right/camera_info_throttle"/>
-			<remap from="cloud"                 to="/planner_cloud"/>
+			<remap from="cloud"                 to="cloudXYZ"/>
 			
 			<param name="voxel_size" type="double" value="0.05"/>
 			<param name="decimation" type="int" value="4"/>
+			<param name="max_depth" type="double" value="4"/>
 		</node>
+		<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap/obstacles_detection stereo_nodelet">
+    			<remap from="cloud" to="cloudXYZ"/>
+			<remap from="obstacles_2d" to="/planner_cloud"/>
+
+			<param name="frame_id" type="string" value="base_footprint"/>
+			<param name="map_frame_id" type="string" value="map"/>
+			<param name="min_cluster_size" type="int" value="20"/>
+			<param name="max_obstacles_height" type="double" value="0.0"/>
+  		</node>
 	</group>
 	
 	<!-- Visual Odometry -->

launch/config/azimut3/base_local_planner_params.yaml

@@ -3,7 +3,7 @@ TrajectoryPlannerROS:
   # Current limits based on AZ3 standalone configuration.
   acc_lim_x:  0.75
   acc_lim_y:  0.75
-  acc_lim_th: 4.00
+  acc_lim_theta: 4.00
   # 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
@@ -30,3 +30,4 @@ TrajectoryPlannerROS:
   dwa: true
 
   oscillation_reset_dist: 0.05
+  meter_scoring: true

launch/config/azimut3/costmap_common_params.yaml

@@ -1,11 +1,40 @@
 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.2
 #robot_radius: ir_of_robot
 inflation_radius: 0.55
+transform_tolerance: 1
 
-observation_sources: laser_scan_sensor point_cloud_sensor
+#observation_sources: laser_scan_sensor point_cloud_sensor
+observation_sources: point_cloud_sensor
 
-laser_scan_sensor: {data_type: LaserScan, topic: base_scan, marking: true, clearing: true}
+laser_scan_sensor: {
+  data_type: LaserScan, 
+  topic: base_scan, 
+  expected_update_rate: 0.2, 
+  marking: true, 
+  clearing: true}
 
-point_cloud_sensor: {data_type: PointCloud2, topic: openni_points, 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}
+
+controller_patience: 2.0
+
+NavfnROS:
+    allow_unknown: true
+
+recovery_behaviors: [
+    {name: conservative_clear, type: clear_costmap_recovery/ClearCostmapRecovery},
+    {name: aggressive_clear, type: clear_costmap_recovery/ClearCostmapRecovery}
+]
+
+conservative_clear: 
+    reset_distance: 3.00
+aggressive_clear:
+    reset_distance: 1.84

launch/config/azimut3/global_costmap_params.yaml

@@ -1,5 +1,6 @@
 global_costmap:
   global_frame: map
   robot_base_frame: base_footprint
-  update_frequency: 5.0
+  update_frequency: 0.5
+  publish_frequency: 0.5
   static_map: true

launch/config/azimut3/local_costmap_params.yaml

@@ -1,10 +1,12 @@
 local_costmap:
-  global_frame: odom
+  global_frame: map
   robot_base_frame: base_footprint
-  update_frequency: 5.0
+  update_frequency: 2.0
   publish_frequency: 2.0
   static_map: false
   rolling_window: true
   width: 4.0
   height: 4.0
-  resolution: 0.05
+  resolution: 0.025
+  origin_x: -2.0
+  origin_y: -2.0