Extension: Global planner

[jens-hj]

Motivation

The GBP algorithm on its own has so much trouble with many obstacles, especially longer, thinner walls.

Issues

• Horizon variable might sometimes phase through obstacles
• Two adjacent variables can end up on either side of a wall, thus predicting the robot to drive through the wall
• A waypoint is reached when the horizon state gets close, which means the robot never gets to the waypoint, and incentivises simply predicting a path that would theoretically get there, without the robot actually having to go anywhere

Idea

Incorporate global path as pose factors for each variable in the planning horizon