Path Following

[matthewashton-k]

Finding a path around obstacles is only half the battle, we still need to be able to follow that path once it is generated.

Acceptance Criteria:

1. given a set of 2d way points (the generated path), the robot should be able to drive to each of those way points in order.
2. The behavior tree needs to have a new branch for stepping along a generated path
3. The branch of the behavior tree needs to be cancellable (if the robot is following the path and the user requests for software stop mode to be engaged, the robot should stop following the path and enter soft stop mode).
4. There should be some set of criteria that when true, the robot has determined it has failed to follow a path. e.g. detecting if the robot is stuck, detecting if its position is too far away from the path.
5. There should be branches in the behavior tree for handling each of those failure states.
6. The robot should demonstrate all of the above in the simulation environment first, and then in the real world.

[matthewashton-k]

This issue is hard blocked by sensor input integration, and motor control integration (path following wont work without simulated sensor inputs, because without sensor inputs there is no localization), but once those are done progress can be made on this even if obstacle detection, etc dont work. You could test by just hard coding some waypoints/paths to follow until the path finding works.