Extend RecordReplayPlanner to take bounding boxes from the lidar_detector
Description
Building on the results of #170 (closed), the mock motion planner should be extended to support obstacles in a basic way.
Purpose
In order to be able to validate that the obstacle inputs are understood properly by the motion planner interface, the mock motion planner will be extended to be aware of obstacles.
Desired Behavior
The mock motion planner should command a stop of the vehicle whenever its planned trajectory intersects with a downstream obstacle. It will not attempt to avoid obstacles.
The way it commands the vehicle to stop is by shortening the trajectory to only states that do not have any collisions, and setting the final state's velocity to 0. Note that this only guarantees that there are no collisions if one can make the assumption that the controller and vehicle can follow this reference, i.e. change velocity to 0 within one trajectory step.
Doing this in a more general way would involve computing a dynamically feasible velocity profile for the controller to follow - this is not attempted here.
Definition of Done
-
Add obstacles to the inputs of the motion planner interface -
Implement stopping functionality -
Add unit tests for the stopping functionality
/label ~feature