From 9ce2aa02dbfba279fc64ae22173e520f5b958a9c Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <mxgrey@intrinsic.ai>
Date: Wed, 22 Nov 2023 15:47:58 +0000
Subject: [PATCH] Fix approach lanes bug

Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>
---
 .../src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp b/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp
index d2a86117..a6181517 100644
--- 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,