teb_local_planner
teb_local_planner copied to clipboard
Published
20 hours ago •
rst-tu-dortmund
rst-tu-dortmund
teb_local_planner having difficulty making a 90 degree turn
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_planner_params.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.
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.
