Add reached API to read-only fleet adapter

[xiyuoh]

This PR adds the reached() API to the read-only fleet adapter.

Currently when a read-only robot's path is populated in RobotState, a route is created and stored in the FleetAdapterNode. This PR further populates the route with checkpoints using all the waypoints in the RobotState path. When the read-only robot reaches a new checkpoint, the API is called and allows better tracking of the robot progress around the map, which would help the full-control robots in the same map to move out of the read-only robot's way.

This PR uses the shortest distance away from the next checkpoint to measure whether the robot has reached, and assumes that the robot is always using the newest route in the itinerary.

[mxgrey]

I've left a comment that will help make the reach detection much more robust. The issues are very subtle but could end up being important in some scenarios.

[mxgrey]

Being within some distance of the waypoint is not a robust criteria. There are two opposite issues to consider:

1. 30cm can be a considerable distance in tight spaces. If a robot gets stopped 30cm from its goal but we report it as reached, that could trigger a waiting robot to approach prematurely, thinking that this read-only robot is completely out of the way already.
2. If a robot manages to move through the 30cm radius before a state update takes place then we won't see that it reached the point until it reaches the next one, causing avoidable delays.

This also isn't quite the right location to identify where a robot has reached. At this point in the code we don't know if the robot is even following the same path as its last update, so we might report reaching a waypoint of an out-of-date path.

In the function handle_delay we examine whether the path of the new state update matches the path that was previously put into the traffic schedule. This is where we verify that the waypoints in the state message match the tail of the waypoints for this robot in the traffic schedule.

This point in handle_delay is where we update the delay to the previously reported path, so that's where we should be reporting the most recently reached waypoint. Since we've already verified that the new and old paths match by that point in the code, we can actually do a simple difference between the number of waypoints in the route and the number of waypoints in the remaining path as reported by the state message. It would look like:

const auto route_size = it->second->route.trajectory().size();
const auto remaining_path_size = state.path.size();
if (route_size > remaining_path_size)
{
  participant.reached(
    participant.current_plan_id(),
    0,
    route_size - remaining_path_size - 1);
}

The - 1 is because we should only report that we have "reached" points which are no longer included in the remaining path of the state message.

[xiyuoh]

These are extremely good points; using the difference in number of waypoints left in the path makes a lot more sense. Thank you for pointing me to this solution, I've added them in 379d93b and tested in simulation.