From 14ec7418e8bf4b6950049d1dd233dce755956eea Mon Sep 17 00:00:00 2001
From: Xiyu Oh <xiyu@openrobotics.org>
Date: Tue, 1 Oct 2024 16:47:42 +0800
Subject: [PATCH 1/2] Set checkpoints and mark them as reached

Signed-off-by: Xiyu Oh <xiyu@openrobotics.org>
---
 .../src/read_only/FleetAdapterNode.cpp        | 45 +++++++++++++++++++
 1 file changed, 45 insertions(+)

diff --git a/rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp b/rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp
index fa2e921e..40478e38 100644
--- a/rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp
+++ b/rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp
@@ -20,6 +20,8 @@
 #include <rmf_traffic/DetectConflict.hpp>
 
 #include <rmf_traffic/agv/Interpolate.hpp>
+#include <rmf_traffic/agv/Planner.hpp>
+#include <rmf_traffic/Time.hpp>
 
 #include <rmf_traffic_ros2/StandardNames.hpp>
 #include <rmf_traffic_ros2/Time.hpp>
@@ -214,6 +216,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 +245,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;
 

From 0e7c613ddbd5b3d0651d3d318e0e90e3d4e8e681 Mon Sep 17 00:00:00 2001
From: Xiyu Oh <xiyu@openrobotics.org>
Date: Fri, 4 Oct 2024 07:51:31 +0000
Subject: [PATCH 2/2] Remove unnecessary headers

Signed-off-by: Xiyu Oh <xiyu@openrobotics.org>
---
 rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp | 2 --
 1 file changed, 2 deletions(-)

diff --git a/rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp b/rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp
index 40478e38..eef7808a 100644
--- a/rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp
+++ b/rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp
@@ -20,8 +20,6 @@
 #include <rmf_traffic/DetectConflict.hpp>
 
 #include <rmf_traffic/agv/Interpolate.hpp>
-#include <rmf_traffic/agv/Planner.hpp>
-#include <rmf_traffic/Time.hpp>
 
 #include <rmf_traffic_ros2/StandardNames.hpp>
 #include <rmf_traffic_ros2/Time.hpp>