Integration with costmap

[geonhee-lee]

Integration with costmap

[Geonhee-LEE]

TEB can actually be seen as a special realization of MPC:

• Variable horizon length
• Terminal equality condition (however, less restrictive since the horizon length is variable)
• System dynamics are described in terms of a flat system, so optimization is just performed in the state space (SE2 poses) and
  inputs are afterward reconstructed from the pose trajectory.
• Discretization using direct collocation via finite-differences.
• Point-to-point motion (the reference trajectory is static in each open-loop step, i.e. the current intermediate goal from the global plan) [a quasi path following mode is achieved by via-points]
• Minimum-time objective
• Hard-constraints are approximated by quadratic penalty functions (soft constraints) and weight adaptation -> leads to an unconstrained nonlinear least squares problem

So it is comparable with a point-to-point time-optimal model predictive control approach.
Whereas the soft-constraint approximation and the dynamics description usually result in very fast computation times (compared to standard MPC), they are also introducing some restrictions:

• The soft-constraints do not scale well for very large optimization problems repsectively, the weights are hard to tune. Just thinking of autonomous truck parking, in which dimensions are much larger than for a small indoor robot.
• The geometric (/flat) system description just based on the pose trajectory makes introducing further constraints more difficult. E.g. for car-like robots limiting the steering rate introduces a very nested function which can result in an ill-defined optimization problem for some configurations. However, the current state without steering rate constraints works quite well, but requires the robot to steer relatively fast.

During the last year I implemented a generic MPC framework at my department which can realize variable-horizon time-optimal MPC but also standard quadratic-form receding-horizon MPC, etc.
I also wrote an mpc local planner plugin that includes the features of the TEB but with full state space robot models and hard-constraints. Also other MPC configurations are possible. The package is still private, however, I thought about pushing it in the next days to ros1 and ros2.
Of course, computation time is larger than for the TEB, but still very fast to my mind.

I am reusing some classes from the teb_local_planner, like PoseSE2, obstacles, robot footprint models, distance computations. This could be related to your last paragraph, as this is all required by gradient-based optimization and is not yet part of navigation or navigation2.
I thought about moving these classes to a separate ros repository which TEB and MPC planners depend on. This is also the reason why I am holding back the ros2 release of TEB right now.

Instead of copying the TEB (which I actually don't like as well), what about adding a dependency on it to navigation2 meta package? We can make sure to push the new TEB release quite soon after every new ROS release, as the number of dependencies is low. I could also add a further maintainer. What do you think?
Edit: okay, the circular dependency could be a problem as bloom releases all projects at once...

REF

ros-navigation/navigation2#1277