dwa_planner解读

官方文档： http://wiki.ros.org/dwa_local_planner
如何使用dwa_planner： http://answers.ros.org/question/209404/how-to-use-dwa_local_planner-in-move_base/
ROS局部规划模块分析：http://dkkkry.blog.51cto.com/8090800/1627993

出现如下报错

[ WARN] [1465964041.914251209]: Control loop missed its desired rate of 3.0000Hz... the loop actually took 0.4875 seconds
[ERROR] [1465964059.033444014]: Aborting because a valid plan could not be found. Even after executing all recovery behaviors

TODO

http://wiki.ros.org/teb_local_planner/Tutorials

teb_local_planer

teb_local_planner_params.yaml

TebLocalPlannerROS:

 odom_topic: odom
 map_frame: /odom

 # Trajectory

 teb_autosize: True
 dt_ref: 0.3
 dt_hysteresis: 0.1
 global_plan_overwrite_orientation: True
 max_global_plan_lookahead_dist: 3.0
 feasibility_check_no_poses: 5

 # Robot

 max_vel_x: 0.2
 max_vel_x_backwards: 0.1
 max_vel_theta: 0.3
 acc_lim_x: 0.2
 acc_lim_theta: 0.2
 min_turning_radius: 0.0
 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
   radius: 0.222 # for type "circular"

 # GoalTolerance

 xy_goal_tolerance: 0.2
 yaw_goal_tolerance: 0.1
 free_goal_vel: False

 # Obstacles

 min_obstacle_dist: 0.3
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 1.0
 obstacle_poses_affected: 15
 costmap_converter_plugin: ""
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5

 # Optimization

 no_inner_iterations: 5
 no_outer_iterations: 4
 optimization_activate: True
 optimization_verbose: False
 penalty_epsilon: 0.1
 weight_max_vel_x: 2
 weight_max_vel_theta: 1
 weight_acc_lim_x: 1
 weight_acc_lim_theta: 1
 weight_kinematics_nh: 1000
 weight_kinematics_forward_drive: 1
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1
 weight_obstacle: 300
 weight_dynamic_obstacle: 10 # not in use yet
 alternative_time_cost: False # not in use yet

 # Homotopy Class Planner

 enable_homotopy_class_planning: False
 enable_multithreading: True
 simple_exploration: False
 max_number_classes: 4
 roadmap_graph_no_samples: 15
 roadmap_graph_area_width: 5
 h_signature_prescaler: 0.5
 h_signature_threshold: 0.1
 obstacle_keypoint_offset: 0.1
 obstacle_heading_threshold: 0.45
 visualize_hc_graph: False

teb_move_base.launch

  "move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    file="$(find dashgo_nav)/config/dashgo/costmap_common_params.yaml" command="load" ns="global_costmap" />
    file="$(find dashgo_nav)/config/dashgo/costmap_common_params.yaml" command="load" ns="local_costmap" />
    file="$(find dashgo_nav)/config/dashgo/local_costmap_params.yaml" command="load" />
    file="$(find dashgo_nav)/config/dashgo/global_costmap_params.yaml" command="load" />
    file="$(find dashgo_nav)/config/dashgo/teb_local_planner_params.yaml" command="load" />

     file="$(find dashgo_nav)/config/nav_obstacles_params.yaml" command="load" />
     <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
  

teb_move_base_blank_map.launch

<launch>
   <node name="arduino" pkg="dashgo_bringup" type="dashgo_driver.py" output="screen">
      <rosparam file="$(find dashgo_bringup)/config/my_dashgo_params.yaml" command="load" />
   node>

  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
  <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>  
  <param name="serial_baudrate"     type="int"    value="115200"/>
  <param name="frame_id"            type="string" value="laser_frame"/>
  <param name="inverted"            type="bool"   value="false"/>
  <param name="angle_compensate"    type="bool"   value="true"/>
  node>
 <include file="$(find dashgo_description)/launch/dashgo_description.launch"/>
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser4" 
    args="0.0 0.0 0.0 0.0 3.1415926 0.0 /base_link /laser_frame 40" />


  
  <include file="$(find dashgo_nav)/launch/teb_move_base.launch" />

  
  <node name="map_server" pkg="map_server" type="map_server" args="$(find dashgo_nav)/maps/blank_map.yaml" />

  
  <node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />

launch>

teb_amcl_demo.launch

<launch>
   <node name="arduino" pkg="dashgo_bringup" type="dashgo_driver.py" output="screen">
      <rosparam file="$(find dashgo_bringup)/config/my_dashgo_params.yaml" command="load" />
   node>

  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
  <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>  
  <param name="serial_baudrate"     type="int"    value="115200"/>
  <param name="frame_id"            type="string" value="laser_frame"/>
  <param name="inverted"            type="bool"   value="false"/>
  <param name="angle_compensate"    type="bool"   value="true"/>
  node>
 <include file="$(find dashgo_description)/launch/dashgo_description.launch"/>
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser4" 
    args="0.0 0.0 0.0 0.0 3.1415926 0.0 /base_link /laser_frame 40" />
  
  <arg name="map_file" default="$(find dashgo_nav)/maps/amcl_test.yaml"/>
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

  <arg name="initial_pose_x" default="0.0"/> 
  <arg name="initial_pose_y" default="0.0"/> 
  <arg name="initial_pose_a" default="0.0"/>
  <include file="$(find dashgo_nav)/launch/amcl.launch.xml">
    <arg name="initial_pose_x" value="$(arg initial_pose_x)"/>
    <arg name="initial_pose_y" value="$(arg initial_pose_y)"/>
    <arg name="initial_pose_a" value="$(arg initial_pose_a)"/>
  include>
<include file="$(find dashgo_nav)/launch/teb_move_base.launch"/>

launch>

运行

roslaunch dashgo_nav  teb_move_base_blank_map.launch
rviz

---

teb_local_planer支持自定义虚拟的障碍物

   /**
    * @brief Callback for custom obstacles that are not obtained from the costmap 
    * @param obst_msg pointer to the message containing a list of polygon shaped obstacles
    */
  void customObstacleCB(const teb_local_planner::ObstacleMsg::ConstPtr& obst_msg);