The current behavior is intended. Let me try to explain:
The local planner follows the global reference plan by tracking the farest available intermediate pose from the global plan within the local costmap (and also not exceeding max_global_plan_lookahead_dist
).
Ideally, the global plan already contains reasonable and valid orientations since the global planner is aware of the complete map.
Since some global planners do not even provide the orientation part for intermediate poses (NavFcn) or just support forward orientations (default in GlobalPlanner) the teb_local_planner may overwrite orientations by default (global_plan_overwrite_orientation
) except the final orientation.
The following strategy is implemented currently for global_plan_overwrite_orientation==true
:
Reference goal orientation:
- if global goal (last pose of the global plan): take desired final orientation
- if intermediate goal: the reference orientation of local goals is chosen according to the tangential vectors between pose k and pose k+1 in the global plan (always forward).
Initial Trajectory (TEB) for optimziation
The local trajectory is initialized to the current reference goal (see above, within local costmap). Note, the optimizer just finds local minima, hence the initialization is quite important.
- if
allow_init_with_backwards_motion==true
and goal is located behind the robot (w.r.t. heading):
initialize all orientations backwards for the subsequent optimization. - Else: initialize with forward orientations.
This strategy has the following implications:
If the global goal is close to the robot (within the local costmap window) and is located behind it, the robot drives backwards.
Otherwise, the robot always tries to drive forward.
In my opinion, if you would also initialize backwards for goals outside of the local costmap window, you would have to drive backwards along the complete global plan until you reach the global goal. Otherwise, we would need to add more assumptions and heuristics which is not appropriate for a local planner that has just a limited view of the environment.
I think, this might be better decided by a global planner in general as mentioned above. Furthermore, most robots have more sensors in their front hence keeping the default strategy to drive forwards for long distances and backwards just for short ways is reasonable in my eyes.
But I would be happy to discuss whether better or more intuitive strategies exist.
Note, weight_kinematics_turning_radius
defines the penalty weight for satisfying min_turning_radius
for carlike robots.