Skip in-place rotation after Dock event to avoid double docking

rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp

@@ -554,6 +554,32 @@ std::vector<Plan::Waypoint> find_dependencies(
   return waypoints;
 }
 
+//==============================================================================
+namespace {
+class DockFinder : public Graph::Lane::Executor
+{
+public:
+  DockFinder()
+  {
+    // Do nothing
+  }
+
+  void execute(const DoorOpen&) override {}
+  void execute(const DoorClose&) override {}
+  void execute(const LiftSessionBegin&) override {}
+  void execute(const LiftDoorOpen&) override {}
+  void execute(const LiftSessionEnd&) override {}
+  void execute(const LiftMove&) override {}
+  void execute(const Wait&) override {}
+  void execute(const Dock& dock) override
+  {
+    found = true;
+  }
+
+  bool found = false;
+};
+} // anonymous namespace
+
 //==============================================================================
 template<typename NodePtr>
 std::pair<std::vector<Route>, std::vector<Plan::Waypoint>>
@@ -635,15 +661,29 @@ reconstruct_waypoints(
     const bool same_ori = same_orientation(prev_wp.position[2], node->yaw);
     if (!same_pos && !same_ori)
     {
-      candidates.push_back(WaypointCandidate{
-        true,
-        Plan::Waypoint::Implementation{
-          Eigen::Vector3d{prev_pos[0],  prev_pos[1], node->yaw},
-          prev_wp.time, prev_wp.graph_index,
-          {}, {}, {}, prev_wp.event, {}
-        },
-        prev_candidate.velocity
-      });
+      // Check if the previous waypoint has a Dock event.
+      // If it does, skip adding an in-place rotation to avoid duplicate
+      // dock event.
+      bool found_dock_event = false;
+      if (prev_wp.event != nullptr)
+      {
+        DockFinder dock_finder;
+        prev_wp.event->execute(dock_finder);
+        found_dock_event = dock_finder.found;
+      }
+
+      if (!found_dock_event)
+      {
+        candidates.push_back(WaypointCandidate{
+          true,
+          Plan::Waypoint::Implementation{
+            Eigen::Vector3d{prev_pos[0],  prev_pos[1], node->yaw},
+            prev_wp.time, prev_wp.graph_index,
+            {}, {}, {}, prev_wp.event, {}
+          },
+          prev_candidate.velocity
+        });
+      }
     }
 
     if (node->approach_lanes.empty())