Add reached API to read-only fleet adapter

rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp

@@ -214,6 +214,15 @@ void FleetAdapterNode::push_route(
 
   it->second->cumulative_delay = std::chrono::seconds(0);
   it->second->route = make_route(state, _traits, it->second->sitting);
+  // Set checkpoints for this route
+  std::set<uint64_t> checkpoints;
+  uint64_t checkpoint_id = 0;
+  for (const auto& wp : it->second->route->trajectory())
+  {
+    checkpoints.insert(checkpoint_id);
+    checkpoint_id++;
+  }
+  it->second->route->checkpoints(checkpoints);
   it->second->schedule->push_routes({*it->second->route});
 }
 
@@ -234,6 +243,40 @@ void FleetAdapterNode::update_robot(
   const RobotState& state,
   const ScheduleEntries::iterator& it)
 {
+  if (it->second->route.has_value())
+  {
+    auto& participant = it->second->schedule->participant();
+    uint64_t route_id = participant.itinerary().size() - 1;
+    uint64_t last_checkpoint_reached = participant.reached()[route_id];
+
+    uint64_t checkpoint_id = 0;
+    for (const auto& wp : it->second->route.value().trajectory())
+    {
+      if (checkpoint_id <= last_checkpoint_reached)
+      {
+        checkpoint_id++;
+        continue;
+      }
+
+      Eigen::Vector2d current_location =
+        Eigen::Vector2d(state.location.x, state.location.y);
+      Eigen::Vector2d checkpoint_pose =
+        Eigen::Vector2d(wp.position()[0], wp.position()[1]);
+      Eigen::Vector2d diff = current_location - checkpoint_pose;
+      // TODO(@xiyuoh) Make this merge_waypoint_distance configurable
+      if (diff.norm() < 0.3)
+      {
+        // The robot is close enough to the checkpoint, mark as reached.
+        participant.reached(
+          participant.current_plan_id(), route_id, checkpoint_id);
+      }
+      else
+        break;
+
+      checkpoint_id++;
+    }
+  }
+
   if (handle_delay(state, it))
     return;