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.05k stars 549 forks source link

teb_local_planner having difficulty making a 90 degree turn #236

Open darthShana opened 4 years ago

darthShana commented 4 years ago

HI All I have applied the teb_local_planner to my move_base for for my physical car like robot.. It reaches the goals i set successfully in my living room when just travelling in a straight line is required, or just soft turns are required to reach the goal pose

However when bigger turns are required the planner seems to abandon the initial feasible plan to make the turn and adopt more and more complex turn sequences, without giving the original plan a chance to work. I have a link to a youtube video of what happens

https://youtu.be/vGLCs41Mf8k

Im sure i have mis-configured it. But can anyonepoint me the direction what parameters i should be looking at? here is my _base_local_plannerparams.yaml

TebLocalPlannerROS:

  odom_topic: odom

    # Trajectory

  teb_autosize: True
  dt_ref: 0.3
  dt_hysteresis: 0.1
  max_samples: 500
  global_plan_overwrite_orientation: True
  allow_init_with_backwards_motion: True
  max_global_plan_lookahead_dist: 3.0
  global_plan_viapoint_sep: -1
  global_plan_prune_distance: 1
  exact_arc_length: False
  feasibility_check_no_poses: 2
  publish_feedback: True

    # Robot

  max_vel_x: 0.18
  max_vel_x_backwards: 0.18
  max_vel_y: 0.0
  max_vel_theta: 0.5 # the angular velocity is also bounded by min_turning_radius in case of a carlike robot (r = v / omega)
  acc_lim_x: 0.5
  acc_lim_theta: 0.7

  # ********************** Carlike robot parameters ********************
  min_turning_radius: 2.0        # Min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually)
  wheelbase: 0.3                 # Wheelbase of our robot
  cmd_angle_instead_rotvel: True # stage simulator takes the angle instead of the rotvel as input (twist message)
  # ********************************************************************

  footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
    type: "two_circles"
    radius: 0.2 # for type "circular"
    line_start: [0.0, 0.0] # for type "line"
    line_end: [0.4, 0.0] # for type "line"
    front_offset: 0.3 # for type "two_circles"
    front_radius: 0.2 # for type "two_circles"
    rear_offset: 0.0 # 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.4
  yaw_goal_tolerance: 0.25
  free_goal_vel: False
  complete_global_plan: True

    # Obstacles

  min_obstacle_dist: 0.27 # This value must also include our robot's expansion, since footprint_model is set to "line".
  inflation_dist: 0.1
  include_costmap_obstacles: True
  costmap_obstacles_behind_robot_dist: 1.0
  obstacle_poses_affected: 15

  dynamic_obstacle_inflation_dist: 0.6
  include_dynamic_obstacles: True

  costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC"
  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
  obstacle_cost_exponent: 4
  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 # must be > 0
  weight_shortest_path: 0
  weight_obstacle: 100
  weight_inflation: 0.2
  weight_dynamic_obstacle: 10 # not in use yet
  weight_dynamic_obstacle_inflation: 0.2
  weight_viapoint: 1
  weight_adapt_factor: 2

  # Homotopy Class Planner

  enable_homotopy_class_planning: True
  enable_multithreading: True
  max_number_classes: 4
  selection_cost_hysteresis: 1.0
  selection_prefer_initial_plan: 0.95
  selection_obst_cost_scale: 1.0
  selection_alternative_time_cost: False

  roadmap_graph_no_samples: 15
  roadmap_graph_area_width: 5
  roadmap_graph_area_length_scale: 1.0
  h_signature_prescaler: 0.5
  h_signature_threshold: 0.1
  obstacle_heading_threshold: 0.45
  switching_blocking_period: 0.0
  viapoints_all_candidates: True
  delete_detours_backwards: True
  max_ratio_detours_duration_best_duration: 3.0
  visualize_hc_graph: False
  visualize_with_time_as_z_axis_scale: False

  # Recovery

  shrink_horizon_backup: True
  shrink_horizon_min_duration: 10
  oscillation_recovery: True
  oscillation_v_eps: 0.1
  oscillation_omega_eps: 0.1
  oscillation_recovery_min_duration: 10
  oscillation_filter_duration: 10

The other config files i have not changes apart from ajusting to the robots foot print.. Bellow is the output from the teb_local_planner also included in the log is the velocity and steering sent to the ackerman drive

[ WARN] [1603053883.684021065]: global_costmap: Pre-Hydro parameter "static_map" unused since "plugins" is provided
[ WARN] [1603053883.687325975]: global_costmap: Pre-Hydro parameter "map_type" unused since "plugins" is provided
[ INFO] [1603053883.689678261]: global_costmap: Using plugin "static_layer"
[ INFO] [1603053883.714386789]: Requesting the map...
[ INFO] [1603053885.119749955]: Resizing costmap to 106 X 88 at 0.050000 m/pix
[ INFO] [1603053885.219555149]: Received a 106 X 88 map at 0.050000 m/pix
[ INFO] [1603053885.227196674]: global_costmap: Using plugin "obstacle_layer"
[ INFO] [1603053885.232439371]:     Subscribed to Topics: laser_scan_sensor
[ INFO] [1603053885.268588499]: global_costmap: Using plugin "inflation_layer"
[ WARN] [1603053885.370971354]: local_costmap: Pre-Hydro parameter "static_map" unused since "plugins" is provided
[ WARN] [1603053885.371692269]: local_costmap: Pre-Hydro parameter "map_type" unused since "plugins" is provided
[ INFO] [1603053885.373204099]: local_costmap: Using plugin "static_layer"
[ INFO] [1603053885.380242417]: Requesting the map...
[ INFO] [1603053885.383072536]: Resizing static layer to 106 X 88 at 0.050000 m/pix
[ INFO] [1603053885.483166063]: Received a 106 X 88 map at 0.050000 m/pix
[ INFO] [1603053885.489507091]: local_costmap: Using plugin "obstacle_layer"
[ INFO] [1603053885.493790914]:     Subscribed to Topics: laser_scan_sensor
[ INFO] [1603053885.591892405]: Created local_planner teb_local_planner/TebLocalPlannerROS
[ INFO] [1603053885.689316396]: Footprint model 'two_circles' (front_offset: 0.3m, front_radius: 0.2m, rear_offset: 0m, rear_radius: 0.2m) loaded for trajectory optimization.
[ INFO] [1603053885.689512062]: Parallel planning in distinctive topologies enabled.
[ INFO] [1603053886.258780463]: Costmap conversion plugin costmap_converter::CostmapToLinesDBSRANSAC loaded.
[ INFO] [1603053887.559205945]: Recovery behavior will clear layer 'obstacles'
[ INFO] [1603053887.566967344]: Recovery behavior will clear layer 'obstacles'
[ INFO] [1603053938.091310244]: Resizing costmap to 107 X 88 at 0.050000 m/pix
[ INFO] [1603053938.091879784]: Resizing static layer to 107 X 88 at 0.050000 m/pix
[ INFO] [1603053943.091325121]: Resizing costmap to 108 X 88 at 0.050000 m/pix
[ INFO] [1603053943.091799120]: Resizing static layer to 108 X 88 at 0.050000 m/pix
[INFO] [1603053964.611536]: message:{"velocity": 0.0, "steering": 0.0}
[INFO] [1603053965.160557]: message:{"velocity": 0.19, "steering": 13.888723302019102}
[INFO] [1603053965.663271]: message:{"velocity": 0.19, "steering": 13.888723302019102}
[ INFO] [1603053966.091574623]: Resizing costmap to 110 X 89 at 0.050000 m/pix
[ INFO] [1603053966.092169413]: Resizing static layer to 110 X 89 at 0.050000 m/pix
[INFO] [1603053966.157327]: message:{"velocity": 0.10989223804505407, "steering": 22.2773236227058}
[INFO] [1603053966.657248]: message:{"velocity": -0.16800924618966745, "steering": 15.190033651758181}
[ INFO] [1603053967.093728727]: Resizing costmap to 137 X 101 at 0.050000 m/pix
[ INFO] [1603053967.094346892]: Resizing static layer to 137 X 101 at 0.050000 m/pix
[INFO] [1603053967.152982]: message:{"velocity": -0.09639740146564793, "steering": 25.08306404163337}
[INFO] [1603053967.653306]: message:{"velocity": 0.19, "steering": 13.888723302019102}
[INFO] [1603053968.153615]: message:{"velocity": 0.15516064931167317, "steering": 16.22307184744839}
[INFO] [1603053968.671711]: message:{"velocity": 0.1567401917013205, "steering": 15.917114169782378}
[ INFO] [1603053969.092987821]: Resizing costmap to 140 X 122 at 0.050000 m/pix
[ INFO] [1603053969.094202610]: Resizing static layer to 140 X 122 at 0.050000 m/pix
[INFO] [1603053969.138596]: message:{"velocity": 0.19, "steering": 13.888723302019102}
[INFO] [1603053969.645336]: message:{"velocity": 0.19, "steering": 13.888723302019102}
[ INFO] [1603053970.094072345]: Resizing costmap to 141 X 124 at 0.050000 m/pix
[ INFO] [1603053970.094764426]: Resizing static layer to 141 X 124 at 0.050000 m/pix
[INFO] [1603053970.152572]: message:{"velocity": 0.157085226037504, "steering": -15.87299020556166}
[INFO] [1603053970.652028]: message:{"velocity": 0.14958147768053567, "steering": 16.62645188424539}
[ INFO] [1603053971.093823412]: Resizing costmap to 141 X 124 at 0.050000 m/pix
[ INFO] [1603053971.094642785]: Resizing static layer to 141 X 124 at 0.050000 m/pix
[INFO] [1603053971.153260]: message:{"velocity": 0.17994646606031478, "steering": 14.465086504043498}
[INFO] [1603053971.648675]: message:{"velocity": 0.18221940256730185, "steering": 13.77384053597151}
[ INFO] [1603053972.094260937]: Resizing costmap to 143 X 124 at 0.050000 m/pix
[ INFO] [1603053972.095232143]: Resizing static layer to 143 X 124 at 0.050000 m/pix
[INFO] [1603053972.154345]: message:{"velocity": 0.19, "steering": 13.888723302019102}
[INFO] [1603053972.655336]: message:{"velocity": 0.19, "steering": 8.07889278095199}
[INFO] [1603053973.154964]: message:{"velocity": 0.142306098654834, "steering": -17.426177706739683}
[INFO] [1603053973.648808]: message:{"velocity": 0.14657802805769973, "steering": -16.950059361518434}
[INFO] [1603053974.154317]: message:{"velocity": 0.19, "steering": -13.888723302019102}
[INFO] [1603053974.638130]: message:{"velocity": 0.19, "steering": -13.888723302019102}
[ INFO] [1603053975.094530679]: Resizing costmap to 143 X 127 at 0.050000 m/pix
[ INFO] [1603053975.095246261]: Resizing static layer to 143 X 127 at 0.050000 m/pix
[INFO] [1603053975.144288]: message:{"velocity": 0.14957502857852584, "steering": -16.62711432427568}
[INFO] [1603053975.658574]: message:{"velocity": 0.17237280199585817, "steering": -14.544530994577318}
[INFO] [1603053976.146751]: message:{"velocity": 0.15681261959386938, "steering": -6.483617910525095}
[INFO] [1603053976.673788]: message:{"velocity": 0.16096289651362394, "steering": -15.62024572025678}
[ INFO] [1603053977.094809105]: Resizing costmap to 146 X 127 at 0.050000 m/pix
[ INFO] [1603053977.095565103]: Resizing static layer to 146 X 127 at 0.050000 m/pix
[INFO] [1603053977.174403]: message:{"velocity": 0.14946121800906448, "steering": -16.651839684197473}
[INFO] [1603053977.674965]: message:{"velocity": 0.14422454447112273, "steering": -17.23164445965255}
[INFO] [1603053978.172781]: message:{"velocity": 0.19, "steering": -13.888723302019102}
[INFO] [1603053978.673368]: message:{"velocity": 0.19, "steering": -13.888723302019102}
[INFO] [1603053979.174779]: message:{"velocity": 0.19, "steering": 3.8121634567749303}
[INFO] [1603053979.673570]: message:{"velocity": 0.15654355902563646, "steering": 16.12015239814894}
[INFO] [1603053980.172913]: message:{"velocity": 0.16180767019597248, "steering": 15.643644279449108}
[INFO] [1603053980.664372]: message:{"velocity": 0.1723745591007421, "steering": 14.765155688994692}
[ INFO] [1603053981.095889329]: Resizing costmap to 147 X 127 at 0.050000 m/pix
[ INFO] [1603053981.096645994]: Resizing static layer to 147 X 127 at 0.050000 m/pix
[INFO] [1603053981.162882]: message:{"velocity": 0.17121367741466736, "steering": 14.837118010477532}
[INFO] [1603053981.666922]: message:{"velocity": 0.16760487341146701, "steering": 15.129496346862569}
[INFO] [1603053982.163811]: message:{"velocity": 0.17466605325823645, "steering": 14.559698047581685}
[INFO] [1603053982.661197]: message:{"velocity": 0.16975629243693774, "steering": 14.952262868768113}
[INFO] [1603053983.157166]: message:{"velocity": 0.19, "steering": -13.888723302019102}
[INFO] [1603053983.666969]: message:{"velocity": 0.1593422437671581, "steering": -15.842045431465138}
[ INFO] [1603053984.096806945]: Resizing costmap to 147 X 127 at 0.050000 m/pix
[ INFO] [1603053984.097523527]: Resizing static layer to 147 X 127 at 0.050000 m/pix
[INFO] [1603053984.174264]: message:{"velocity": 0.19, "steering": -13.888723302019102}
[INFO] [1603053984.667880]: message:{"velocity": 0.18496460630770492, "steering": 4.345956139460004}
[INFO] [1603053985.161706]: message:{"velocity": 0.16663444001602204, "steering": 15.171797817902506}
[INFO] [1603053985.668552]: message:{"velocity": 0.17035605509820967, "steering": 15.01833994804229}
[INFO] [1603053986.162085]: message:{"velocity": 0.14602740780631962, "steering": 17.61115292457004}
[INFO] [1603053986.665442]: message:{"velocity": 0.19, "steering": 13.888723302019102}
[INFO] [1603053987.173497]: message:{"velocity": 0.19, "steering": -13.888723302019102}
[INFO] [1603053987.706693]: message:{"velocity": 0.19, "steering": -13.888723302019102}
[ INFO] [1603053988.097532295]: Resizing costmap to 148 X 127 at 0.050000 m/pix
[ INFO] [1603053988.098260752]: Resizing static layer to 148 X 127 at 0.050000 m/pix
[INFO] [1603053988.231491]: message:{"velocity": -0.11132887272481111, "steering": -22.879648171587206}
[INFO] [1603053988.742786]: message:{"velocity": -0.19, "steering": -13.888723302019102}
[INFO] [1603053989.275368]: message:{"velocity": -0.19, "steering": -13.888723302019102}
[ WARN] [1603053989.774449607]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[INFO] [1603053989.778356]: message:{"velocity": 0.0, "steering": 0.0}
[ INFO] [1603053990.097118347]: Resizing costmap to 152 X 127 at 0.050000 m/pix
[ INFO] [1603053990.097837637]: Resizing static layer to 152 X 127 at 0.050000 m/pix
[ WARN] [1603053990.206409771]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[INFO] [1603053990.208966]: message:{"velocity": 0.0, "steering": 0.0}
[INFO] [1603053990.681383]: message:{"velocity": 0.19, "steering": -13.888723302019102}
[INFO] [1603053991.226944]: message:{"velocity": 0.15722066497171583, "steering": -16.63715867104567}
[ WARN] [1603053991.737042366]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ERROR] [1603053991.750099712]: Failed to get a plan.
antoszy commented 2 years ago

Your robot position drifts a lot, which you can see base on the that laser scans move. Robot position drifting causes planner to change plane after every position estimation jump. You may try different mapping package to stabilize position estimation. I would recommend mapping package that takes odometry as input.