search

rst-tu-dortmund / teb_local_planner

An optimal trajectory planner considering distinctive topologies for mobile robots based on Timed-Elastic-Bands (ROS Package)
http://wiki.ros.org/teb_local_planner
BSD 3-Clause "New" or "Revised" License
1.03k stars 546 forks source link

Teb prefers forward motion #137

Open doisyg opened 5 years ago

doisyg commented 5 years ago

Hello, I have a platform that is symmetrical and I don't want it to rotate to favorite forward motion when it doesn't need too. When sending goals close to the robot it works (i guess it is close enough that the local planner can directly plan up to the goal): backward_okay_short

Whereas when sending goals further away it doesn't. The local planner tries to reach the furthest global plan point it can, but it tries to end with a forward motion, hence doing 2 unnecessary 180° rotation: backward_long_rotate

Here are my parameters (it tried to make max_vel_x_backwards > max_vel_x without effect)


TebLocalPlannerROS:

 odom_topic: odom
 map_frame: map

 # Trajectory

 teb_autosize: True
 dt_ref: 0.1
 dt_hysteresis: 0.01
 global_plan_overwrite_orientation: True
 allow_init_with_backwards_motion: True
 max_global_plan_lookahead_dist: 3.0
 feasibility_check_no_poses: 5
 force_reinit_new_goal_dist: 0.1

 # Robot

 max_vel_x: 0.5
 max_vel_x_backwards: 0.5
 max_vel_y: 0.0
 max_vel_theta: 1.0
 acc_lim_x: 0.6
 acc_lim_theta: 1.5
 min_turning_radius: 0.0 # diff-drive robot (can turn on place!)

 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
  type: "polygon"
  vertices: [[1.201, 0.706], [-1.201,  0.706], [-1.201, -0.706], [1.201, -0.706]] # for type "polygon"
  front_offset: 0.5 # for type "two_circles"
  front_radius: 1.0 # for type "two_circles"
  rear_offset: 0.5 # for type "two_circles"
  rear_radius: 1.0 # for type "two_circles"

 # GoalTolerance

 xy_goal_tolerance: 0.5
 yaw_goal_tolerance: 0.2
 free_goal_vel: False

 # Obstacles

 min_obstacle_dist: 0.2
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 4.0
 obstacle_poses_affected: 20
 ## Costmap converter plugin  
 costmap_converter_spin_thread: True
 costmap_converter_rate: 10
 costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
 #costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC"
 #costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSMCCH"
 #costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSConcaveHull"
 #costmap_converter_plugin: "" # deactivate plugin

 ## Configure plugins (namespace move_base/TebLocalPlannerROS/PLUGINNAME)
 ## The parameters must be added for each plugin separately
 costmap_converter/CostmapToLinesDBSRANSAC:
  cluster_max_distance: 0.4
  cluster_min_pts: 2
  cluster_max_pts: 2000
  ransac_inlier_distance: 0.15
  ransac_min_inliers: 10
  ransac_no_iterations: 2000
  ransac_remainig_outliers: 3
  ransac_convert_outlier_pts: True
  ransac_filter_remaining_outlier_pts: False
  convex_hull_min_pt_separation: 0.1

 # Optimization

 no_inner_iterations: 1
 no_outer_iterations: 1
 optimization_activate: True
 optimization_verbose: False
 penalty_epsilon: 0.1
 weight_max_vel_x: 2
 weight_max_vel_theta: 1
 weight_acc_lim_x: 15
 weight_acc_lim_theta: 20
 weight_kinematics_nh: 1000
 weight_kinematics_forward_drive: 0.1
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1
 weight_obstacle: 50
 weight_dynamic_obstacle: 10 # not in use yet
 weight_adapt_factor: 2

 # Homotopy Class Planner

 enable_homotopy_class_planning: True
 enable_multithreading: True
 simple_exploration: False
 max_number_classes: 4
 selection_cost_hysteresis: 1.0
 selection_obst_cost_scale: 1.0
 selection_alternative_time_cost: False

 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
doisyg commented 5 years ago

Once again, I shooted too fast. Solved by setting teb param global_plan_overwrite_orientation to false and GlobalPlanner orientation mode to Interpolate

doisyg commented 5 years ago

Reopening, I would expect the robot to continue moving backward when global_plan_overwrite_orientation is set to True, no matter the orientation of the global plan

croesmann commented 5 years ago

Hi,

in the first case, you placed the goal inside the local costmap and if allow_init_with_backwards_motion = true, the optimizer is likely to find the backward motion directly. However, for goals outside the local costmap, intermediate goal poses are obtained from the global plan either close to the costmap border or as soon as max_global_plan_lookahead_dist is exceeded.

In general, we need to assume that the global planner already provides knowledge on intermediate robot orientations. Just imagine a car park, in which parking lots close to the walls can only be accessed by backward motions. However, guessing this from just the local costmap which does not reveal the existence/shape of the parking lot at first is often impossible. However, since not all global planners provide information on the orientation, global_plan_overwrite_orientation=true is just a workaround to guess the orientation. And the most likely guess is to drive forward if not any further information like path lengths, global costmap etc. is considered.

See this FAQ entry and #115 for more information.