Fix UB in plan squashing (#106)

Signed-off-by: Michael X. Grey <[email protected]>
open-rmf · Dec 21, 2023 · be8f9b3 · be8f9b3
1 parent b98583d
commit be8f9b3
Showing 1 changed file with 5 additions and 3 deletions.
diff --git a/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp b/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp
@@ -220,9 +220,11 @@ std::vector<NodePtr> reconstruct_nodes(
       else
         last_midlane_node = node;
     }
-
-    const auto wp = *node->waypoint;
-    cruft_map[wp].insert(node);
+    else
+    {
+      const auto wp = *node->waypoint;
+      cruft_map[wp].insert(node);
+    }
   }
 
   if (first_midlane_node && last_midlane_node)