ros-pkg: moved az3 planner config in this project

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

launch/azimut3/az3_mapping_robot_stereo_nav.launch

@@ -11,30 +11,27 @@
 	<include file="$(find az3_bringup)/az3_standalone.launch"/>
 	
 	<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"/>
+        <node name="teleop" pkg="nodelet" type="nodelet" args="standalone azimut_tools/Teleop"/>
         <param name="cmd_eta/abtr_priority" value="50"/>
     </group>
 
-    <group ns="go_back">
-        <remap from="joy"  to="/joy"/>
-        <remap from="goal" to="/planner_goal"/>
-        <node name="go_back" pkg="azimut_tools" type="go_back"/>
-        <rosparam>
-            btn_save: 1
-            btn_goto: 3
-        </rosparam>
-    </group>
-
+	<!-- ROS navigation stack move_base -->
     <group ns="planner">
         <remap from="openni_points" to="/planner_cloud"/>
         <remap from="base_scan" to="/base_scan"/>
         <remap from="map" to="/map"/>
         <remap from="move_base_simple/goal" to="/planner_goal"/>
-        <include file="$(find az3_navigation)/launch/az3_move_base_slam.launch"/>
+        
+        <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    	  <rosparam file="$(find rtabmap)/launch/config/azimut3/costmap_common_params.yaml" command="load" ns="global_costmap" />
+     	  <rosparam file="$(find rtabmap)/launch/config/azimut3/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    	  <rosparam file="$(find rtabmap)/launch/config/azimut3/local_costmap_params.yaml" command="load" />
+    	  <rosparam file="$(find rtabmap)/launch/config/azimut3/global_costmap_params.yaml" command="load" />
+    	  <rosparam file="$(find rtabmap)/launch/config/azimut3/base_local_planner_params.yaml" command="load" />
+   		</node>
+   		
         <param name="cmd_vel/abtr_priority" value="10"/>
     </group>
 
@@ -42,6 +39,7 @@
         <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"
@@ -87,14 +85,11 @@
 			<remap from="right/image_raw"   to="right/image_raw_throttle"/>
 			<remap from="right/camera_info" to="right/camera_info_throttle"/>
 		</node>
-		<node pkg="nodelet" type="nodelet" name="disparity_odom_sync" args="load rtabmap/disparity_odom_sync stereo_nodelet">
-			<remap from="disparity"       to="disparity"/>
-		    <remap from="camera_info"     to="right/camera_info_throttle"/>
-		    <remap from="odom"            to="/stereo_odometer/odometry"/>
-		</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_sync"/>
-			<remap from="disparity/camera_info" to="right/camera_info_throttle_sync"/>
+			<remap from="disparity/image"       to="disparity"/>
+			<remap from="disparity/camera_info" to="right/camera_info_throttle"/>
 			<remap from="cloud"                 to="/planner_cloud"/>
 			
 			<param name="voxel_size" type="double" value="0.05"/>

launch/config/azimut3/base_local_planner_params.yaml

@@ -0,0 +1,32 @@
+TrajectoryPlannerROS:
+
+  # Current limits based on AZ3 standalone configuration.
+  acc_lim_x:  0.75
+  acc_lim_y:  0.75
+  acc_lim_th: 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
+  max_vel_x:  0.500
+  min_vel_x:  0.212
+  max_rotational_vel: 0.550
+  min_in_place_rotational_vel: 0.15
+  escape_vel: -0.10
+  holonomic_robot: false
+
+  xy_goal_tolerance:  0.20
+  yaw_goal_tolerance: 0.20
+
+  sim_time: 1.7
+  sim_granularity: 0.025
+  vx_samples: 3
+  vtheta_samples: 3
+  vtheta_samples: 20
+
+  goal_distance_bias: 0.8
+  path_distance_bias: 0.6
+  occdist_scale: 0.01
+  heading_lookahead: 0.325
+  dwa: true
+
+  oscillation_reset_dist: 0.05

launch/config/azimut3/costmap_common_params.yaml

@@ -0,0 +1,11 @@
+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]]
+#robot_radius: ir_of_robot
+inflation_radius: 0.55
+
+observation_sources: laser_scan_sensor point_cloud_sensor
+
+laser_scan_sensor: {data_type: LaserScan, topic: base_scan, marking: true, clearing: true}
+
+point_cloud_sensor: {data_type: PointCloud2, topic: openni_points, marking: true, clearing: true}

launch/config/azimut3/global_costmap_params.yaml

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

launch/config/azimut3/local_costmap_params.yaml

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