fix(mission_planner): fix check_reroute_safety (#5766)

Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Dec 5, 2023 · 04aa54b · 04aa54b
1 parent 37628dd
commit 04aa54b
Showing 1 changed file with 47 additions and 10 deletions.
diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp
@@ -729,9 +729,6 @@ bool MissionPlanner::check_reroute_safety(
     return false;
   }
 
-  // find the index of the original route that has same idx with the front segment of the new route
-  const auto target_front_primitives = target_route.segments.front().primitives;
-
   auto hasSamePrimitives = [](
                              const std::vector<LaneletPrimitive> & original_primitives,
                              const std::vector<LaneletPrimitive> & target_primitives) {
@@ -748,15 +745,55 @@ bool MissionPlanner::check_reroute_safety(
     return is_same;
   };
 
-  // find idx that matches the target primitives
-  size_t start_idx = original_route.segments.size();
-  for (size_t i = 0; i < original_route.segments.size(); ++i) {
-    const auto & original_primitives = original_route.segments.at(i).primitives;
-    if (hasSamePrimitives(original_primitives, target_front_primitives)) {
-      start_idx = i;
-      break;
+  // find idx of original primitives that matches the target primitives
+  const auto start_idx_opt = std::invoke([&]() -> std::optional<size_t> {
+    /*
+     * find the index of the original route that has same idx with the front segment of the new
+     * route
+     *
+     *                          start_idx
+     * +-----------+-----------+-----------+-----------+-----------+
+     * |           |           |           |           |           |
+     * +-----------+-----------+-----------+-----------+-----------+
+     * |           |           |           |           |           |
+     * +-----------+-----------+-----------+-----------+-----------+
+     *  original    original    original    original    original
+     *                          target      target      target
+     */
+    const auto target_front_primitives = target_route.segments.front().primitives;
+    for (size_t i = 0; i < original_route.segments.size(); ++i) {
+      const auto & original_primitives = original_route.segments.at(i).primitives;
+      if (hasSamePrimitives(original_primitives, target_front_primitives)) {
+        return i;
+      }
+    }
+
+    /*
+     * find the target route that has same idx with the front segment of the original route
+     *
+     *                          start_idx
+     * +-----------+-----------+-----------+-----------+-----------+
+     * |           |           |           |           |           |
+     * +-----------+-----------+-----------+-----------+-----------+
+     * |           |           |           |           |           |
+     * +-----------+-----------+-----------+-----------+-----------+
+     * 　　　　　　　　　　　　　　　original    original    original
+     *  target      target      target      target      target
+     */
+    const auto original_front_primitives = original_route.segments.front().primitives;
+    for (size_t i = 0; i < target_route.segments.size(); ++i) {
+      const auto & target_primitives = target_route.segments.at(i).primitives;
+      if (hasSamePrimitives(target_primitives, original_front_primitives)) {
+        return 0;
+      }
     }
+
+    return std::nullopt;
+  });
+  if (!start_idx_opt.has_value()) {
+    return false;
   }
+  const size_t start_idx = start_idx_opt.value();
 
   // find last idx that matches the target primitives
   size_t end_idx = start_idx;