Teb_local_planner trajectory is not feasible

asked 2018-07-21 21:02:10 -0500

AutoCar gravatar image

updated 2018-07-21 22:19:58 -0500

I am building a MIT Racecar with Hokuyo Laser, Ackerman, and VESC. To navigate, I used move_base with teb_local_planner. When I set the goal using "2D Nav Goal" in rviz, it always complained "trajectory is not feasible". Please see the screenshot here http://www.artlystyles.com/tmp/teb.png . Error message below:

[ WARN] [1532222598.554825198]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1532222598.555201267]: Clearing costmap to unstuck robot (1.840000m).
[ WARN] [1532222598.555949503]: Map update loop missed its desired rate of 4.0000Hz... the loop actually took 14.2264 seconds
[ WARN] [1532222603.718322427]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1532222603.718685345]: Rotate recovery behavior started.
[ERROR] [1532222603.718853923]: Rotate recovery can't rotate in place because there is a potential collision. Cost: -1.00
[ WARN] [1532222603.719832371]: Map update loop missed its desired rate of 4.0000Hz... the loop actually took 4.6639 seconds
[ WARN] [1532222611.031567436]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ERROR] [1532222611.031753455]: Aborting because a valid control could not be found. Even after executing all recovery behaviors
[ WARN] [1532222611.033439817]: Map update loop missed its desired rate of 4.0000Hz... the loop actually took 6.8136 seconds

The path seems very feasible to me. What is wrong? Here are some parameters:

costmap===========================

footprint: [[-0.5, -0.33], [-0.5, 0.33], [0.5, 0.33], [0.5, -0.33]]
footprint_padding: 0.01

robot_base_frame: base_link
update_frequency: 4.0
publish_frequency: 3.0
transform_tolerance: 0.5

resolution: 0.05

obstacle_range: 5.5
raytrace_range: 6.0

#layer definitions
static:
    map_topic: /map
    subscribe_to_updates: true

obstacles_laser:
    observation_sources: laser
    laser: {data_type: LaserScan, clearing: true, marking: true, topic: scan, inf_is_valid: true}

inflation:
    inflation_radius: 0.1
~                                  

Planner=============================

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.4
 max_vel_x_backwards: 0.2
 max_vel_theta: 0.3
 acc_lim_x: 0.5
 acc_lim_theta: 0.5
 min_turning_radius: 0.0
 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
   type: "point"
   radius: 0.2 # for type "circular"
   line_start: [-0.3, 0.0] # for type "line"
   line_end: [0.3, 0.0] # for type "line"
   front_offset: 0.2 # for type "two_circles"
   front_radius: 0.2 # for type "two_circles"
   rear_offset: 0.2 # for type "two_circles"
   rear_radius: 0.2 # for type "two_circles"
   vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"

 # GoalTolerance

 xy_goal_tolerance: 0.2
 yaw_goal_tolerance: 0.1
 free_goal_vel: False

 # Obstacles

 min_obstacle_dist: 0.2
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 0.5
 obstacle_poses_affected: 30
 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: 50
 weight_dynamic_obstacle: 10 # not in use yet
 alternative_time_cost: False # not in use yet

 # Homotopy Class Planner

 enable_homotopy_class_planning: True
 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 ...
(more)
edit retag flag offensive close merge delete

Comments

Can you please update your question with a copy and paste of the errors that you're getting?

jayess gravatar image jayess  ( 2018-07-21 21:42:49 -0500 )edit

This error message occurs if the robot footprint defined in the costmap_2d settings collides with obstacles for at least one the first feasibility_check_no_poses (here 5) poses.

croesmann gravatar image croesmann  ( 2018-08-06 05:07:18 -0500 )edit