From 344559c3e20a992afa8a3357796b6084a8fa0af1 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <greyxmike@gmail.com>
Date: Thu, 19 Dec 2024 21:17:58 +0800
Subject: [PATCH] Fix orientation test and checkpoint identification for manual
 turn-in-place insertions

Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
---
 .../agv/planning/DifferentialDrivePlanner.cpp | 78 ++++++++++++++++++-
 1 file changed, 74 insertions(+), 4 deletions(-)

diff --git a/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp b/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp
index e751eac8..ef5d95a6 100644
--- a/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp
+++ b/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp
@@ -280,6 +280,12 @@ void stream_trajectory(
   ss << "(finished)\n";
 }
 
+//==============================================================================
+inline bool same_orientation(double yaw0, double yaw1)
+{
+  return std::abs(rmf_utils::wrap_to_pi(yaw0 - yaw1))*180.0 / M_PI < 1e-2;
+}
+
 //==============================================================================
 std::vector<Plan::Waypoint> find_dependencies(
   std::vector<Route>& itinerary,
@@ -473,6 +479,72 @@ std::vector<Plan::Waypoint> find_dependencies(
     }
   }
 
+  std::size_t c_last = 0;
+  std::size_t c = 1;
+  std::size_t c_next = 2;
+  for (; c_next < candidates.size(); ++c_last, ++c, ++c_next)
+  {
+    auto& candidate = candidates[c];
+    if (candidate.waypoint.arrival.empty())
+    {
+      // This was a manually inserted turn-in-place, so let's try to match it
+      // with a trajectory checkpoint
+      const auto p_candidate = candidate.waypoint.position;
+      for (std::size_t r = 0; r < itinerary.size(); ++r)
+      {
+        std::optional<std::size_t> floor_opt;
+        for (const auto& checkpoint : candidates[c_last].waypoint.arrival)
+        {
+          if (checkpoint.route_id != r)
+            continue;
+
+          const auto id = checkpoint.checkpoint_id;
+          if (!floor_opt.has_value())
+            floor_opt = id;
+          else if (*floor_opt < id)
+            floor_opt = id;
+        }
+
+        if (!floor_opt.has_value())
+          continue;
+        const auto floor_id = floor_opt.value();
+
+        std::optional<std::size_t> ceil_opt;
+        for (const auto& checkpoint : candidates[c_next].waypoint.arrival)
+        {
+          if (checkpoint.route_id != r)
+            continue;
+
+          const auto id = checkpoint.checkpoint_id;
+          if (!ceil_opt.has_value())
+            ceil_opt = id;
+          else if (id < *ceil_opt)
+            ceil_opt = id;
+        }
+
+        if (!ceil_opt.has_value())
+          continue;
+        const auto ceil_id = ceil_opt.value();
+
+        // Between the floor and the ceiling, try to find a waypoint in this
+        // trajectory that matches the position of the inserted waypoint.
+        for (std::size_t id = floor_id; id < ceil_id; ++id)
+        {
+          const auto& trajectory_checkpoint = itinerary[r].trajectory()[id];
+          const auto p_trajectory = trajectory_checkpoint.position();
+          const bool same_pos = (p_candidate.block<2,1>(0,0) - p_trajectory.block<2,1>(0,0)).norm() < 1e-2;
+          const bool same_ori = same_orientation(p_candidate[2], p_trajectory[2]);
+          if (same_pos && same_ori)
+          {
+            // We have a matching checkpoint
+            candidate.waypoint.arrival.push_back(Plan::Checkpoint { r, id });
+            candidate.waypoint.time = trajectory_checkpoint.time();
+          }
+        }
+      }
+    }
+  }
+
   std::vector<Plan::Waypoint> waypoints;
   waypoints.reserve(candidates.size());
   for (const auto& c : candidates)
@@ -556,12 +628,10 @@ reconstruct_waypoints(
     // are disconnected
     const auto& prev_candidate = candidates.back();
     const auto& prev_wp = prev_candidate.waypoint;
-    const auto& current_wp = node->waypoint;
     const Eigen::Vector2d prev_pos =
       Eigen::Vector2d{prev_wp.position[0], prev_wp.position[1]};
     const bool same_pos = (prev_pos - node->position).norm() < 1e-3;
-    const bool same_ori =
-      std::abs(prev_wp.position[2] - node->yaw)*180.0 / M_PI < 1e-2;
+    const bool same_ori = same_orientation(prev_wp.position[2], node->yaw);
     if (!same_pos && !same_ori)
     {
       candidates.push_back(WaypointCandidate{
@@ -569,7 +639,7 @@ reconstruct_waypoints(
         Plan::Waypoint::Implementation{
           Eigen::Vector3d{prev_pos[0],  prev_pos[1], node->yaw},
           prev_wp.time, prev_wp.graph_index,
-          prev_wp.approach_lanes, {}, {}, prev_wp.event, {}
+          {}, {}, {}, prev_wp.event, {}
         },
         prev_candidate.velocity
       });