TEB planner tuning help to follow a path strictly

Question

So what I am trying to do, is let the robot follow a global path very strictly.
I know this path doesn't make sense, as it's not taking the shortest route, etc, but let's just say it's like a street the robot has to follow.

My question is, if it is even possible to limit the planner in such a way, that it follows the path and doesn't take a shortcut.
I tried to enable via points and decreased the global plan lookahead but it doesn't affect planning at all.

Also when looking at the second gif, the robot moves back to the beginning of the path.
But for this case, I thought the planner is pruning the already passed global path with calling this function pruneGlobalPlan() , similar to dwb is doing it.

controller_frequency: 10.
controller_plugin_ids: ["FollowPath"]
controller_plugin_types: ["teb_local_planner::TebLocalPlannerROS"]
min_theta_velocity_threshold: 0.0001
min_x_velocity_threshold: 0.0001
min_y_velocity_threshold: 0.0001
FollowPath.acc_lim_theta: 3.
FollowPath.acc_lim_x: 2.
FollowPath.acc_lim_y: 0.
FollowPath.allow_init_with_backwards_motion: False
FollowPath.cmd_angle_instead_rotvel: False
FollowPath.complete_global_plan: True # true prevents the robot from ending the path early when it cross the end goal
FollowPath.costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
FollowPath.costmap_converter_spin_thread: True
FollowPath.costmap_obstacles_behind_robot_dist: 1.0
FollowPath.delete_detours_backwards: True
FollowPath.detours_orientation_tolerance: 1.5708
FollowPath.dt_hysteresis: 0.05
FollowPath.dt_ref: 0.3
FollowPath.dynamic_obstacle_inflation_dist: 0.6
FollowPath.enable_homotopy_class_planning: True
FollowPath.enable_multithreading: True
FollowPath.exact_arc_length: False
FollowPath.feasibility_check_no_poses: 5
FollowPath.footprint_model.radius: 0.6
FollowPath.footprint_model.type: "circular"
FollowPath.force_reinit_new_goal_dist: 1.
FollowPath.free_goal_vel: False
FollowPath.global_plan_overwrite_orientation: True
FollowPath.global_plan_prune_distance: 2.0
FollowPath.h_signature_prescaler: 1.
FollowPath.h_signature_threshold: 0.1
FollowPath.include_costmap_obstacles: True
FollowPath.include_dynamic_obstacles: True
FollowPath.inflation_dist: 0.1
FollowPath.is_footprint_dynamic: False
FollowPath.legacy_obstacle_association: False
FollowPath.length_start_orientation_vector: 0.4
FollowPath.map_frame: "map"
FollowPath.max_global_plan_lookahead_dist: 0.5
FollowPath.max_number_classes: 5
FollowPath.max_ratio_detours_duration_best_duration: 3.
FollowPath.max_samples: 20
FollowPath.max_vel_theta: 1.
FollowPath.max_vel_x: 1.3
FollowPath.max_vel_x_backwards: 0.3
FollowPath.max_vel_y: 0.0
FollowPath.min_obstacle_dist: 0.1
FollowPath.min_resolution_collision_check_angular: 3.14159
FollowPath.min_samples: 3
FollowPath.min_turning_radius: 0.0
FollowPath.no_inner_iterations: 5
FollowPath.no_outer_iterations: 4
FollowPath.obstacle_association_cutoff_factor: 5.
FollowPath.obstacle_association_force_inclusion_factor: 1.5
FollowPath.obstacle_cost_exponent: 1.
FollowPath.obstacle_heading_threshold: 0.45
FollowPath.obstacle_keypoint_offset: 0.1
FollowPath.obstacle_poses_affected: 25
FollowPath.odom_topic: "odom"
FollowPath.optimization_activate: True
FollowPath.optimization_verbose: True
FollowPath.oscillation_filter_duration: 10.
FollowPath.oscillation_omega_eps: 0.1
FollowPath.oscillation_recovery: True
FollowPath.oscillation_recovery_min_duration: 10.
FollowPath.oscillation_v_eps: 0.1
FollowPath.penalty_epsilon: 0.1
FollowPath.publish_feedback: False
FollowPath.roadmap_graph_area_length_scale: 1.
FollowPath.roadmap_graph_area_width: 2.
FollowPath.roadmap_graph_samples: 15
FollowPath.selection_alternative_time_cost: False
FollowPath.selection_cost_hysteresis: 1.
FollowPath.selection_obst_cost_scale: 100.
FollowPath.selection_prefer_initial_plan: 0.95
FollowPath.selection_viapoint_cost_scale: 1.
FollowPath.shrink_horizon_backup: True
FollowPath.shrink_horizon_min_duration: 10.
FollowPath.simple_exploration: False
FollowPath.switching_blocking_period: 0.
FollowPath.teb_autosize: 1.
FollowPath.global_plan_viapoint_sep: 1. # If positive, via-points are extracted from the global plan (path-following mode). The value determines the resolution of the reference path (min. separation between each two consecutive via-points along the global plan, if negative: disabled). Refer to parameter weight_viapoint for adjusting the intensity.
FollowPath.via_points_ordered: True # If true, the planner adheres to the order of via-points in the storage container
FollowPath.viapoints_all_candidates: True
FollowPath.visualize_hc_graph: True
FollowPath.visualize_with_time_as_z_axis_scale: 0.
FollowPath.weight_acc_lim_theta: 1.
FollowPath.weight_acc_lim_x: 1.
FollowPath.weight_acc_lim_y: 1.
FollowPath.weight_adapt_factor: 2.
FollowPath.weight_dynamic_obstacle: 50.
FollowPath.weight_dynamic_obstacle_inflation: 0.1
FollowPath.weight_inflation: 0.1
FollowPath.weight_kinematics_forward_drive: 1.
FollowPath.weight_kinematics_nh: 1000. # Optimization weight for satisfying the non-holonomic kinematics (this parameter must be high since the kinematics equation constitutes an equality constraint, even a value of 1000 does not imply a bad matrix condition due to small 'raw' cost values in comparison to other costs)
FollowPath.weight_kinematics_turning_radius: 0.5
FollowPath.weight_max_vel_theta: 1.
FollowPath.weight_max_vel_x: 2.
FollowPath.weight_max_vel_y: .0
FollowPath.weight_obstacle: 50.
FollowPath.weight_optimaltime: 1.
FollowPath.weight_prefer_rotdir: 50.
FollowPath.weight_shortest_path: 0.0
FollowPath.weight_viapoint: 1.
FollowPath.wheelbase: 1. # only required if cmd_angle_instead_rotvelis set to true
FollowPath.xy_goal_tolerance: 0.15
FollowPath.yaw_goal_tolerance: 0.15

---

Originally posted by madmax on ROS Answers with karma: 496 on 2020-02-14

Post score: 0

Answer 1

I think TEB is fundamentally the incorrect technology choice if you want exact waypoint following. Implementing something like a pure pursuit (or "carrot") controller would be your better bet.

---

Originally posted by stevemacenski with karma: 8272 on 2020-02-14

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by madmax on 2020-02-14:
Well that‘s true. Maybe strict and exact is too heavy, it should prefer aligning to the plan. We also have a pure pursuit controller, but next step was to take some constraints into account and thought about an mpc controller. And the teb planner feels a bit like a mpc controller, with added planner and obstacle avoidance which would also be a nice feature. Replanning the global plan will be difficult, as it‘s coming from a remote server.

Comment by stevemacenski on 2020-02-14:
I don't think you're going to get that here. You should look at DWB for something like that. Just a note though, if you tune any controller to be a nearly exact path follower, its not going to be able to handle dynamic obstacles well (or at all in some cases) since you've trained it not to deviate from the path.

I wouldn't consider that to be good behavior unless youre in a static environment, have a fast replanning rate, or need to navigate in very narrow places.

Comment by madmax on 2020-02-14:
Thanks, I get your point. Not exactly sure about the DWB, cause I think it‘s also a „cost optimizer“ which is then tuned to follow the path. But to some this up, it looks like I need two local controllers. A „dumb“ controller which just follows the path and just stops if there is an obstacle on the path. And a planning controller to drive around obstacles if the road is blocked or in complete autonomous situations.

Comment by stevemacenski on 2020-02-14:
that sounds reasonable.

If I've answered your question, can you hit the checkmark so its removed from our unanswered questions queue?