Fix approach lanes bug

Signed-off-by: Michael X. Grey <[email protected]>
open-rmf · Nov 22, 2023 · 9ce2aa0 · 9ce2aa0
1 parent c95a491
commit 9ce2aa0
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp b/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp
@@ -1499,11 +1499,13 @@ class ScheduledDifferentialDriveExpander
         const auto time = approach_wp.time();
         auto parent = node;
         std::vector<Route> route_from_parent;
+        std::vector<std::size_t> approach_lanes;
         if (approach_route.trajectory().size() < 2)
         {
           // This is just an entry event so we will skip the unnecessary
           // intermediate node
           parent = node->parent;
+          approach_lanes = node->approach_lanes;
           if (!parent)
           {
             // If the top node is a root node, then don't skip it
@@ -1524,7 +1526,7 @@ class ScheduledDifferentialDriveExpander
               Side::Start
             },
             initial_waypoint_index,
-            {},
+            approach_lanes,
             p0,
             yaw,
             time,