fix(behavior_path_planner): crash when drivable area expanding

This is to fix the behavior path planner crash when the drivable area is expanding towards opposite direction lane Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
autowarefoundation · Feb 9, 2022 · 89f2abb · 89f2abb
1 parent a9c47ef
commit 89f2abb
Showing 1 changed file with 13 additions and 9 deletions.
diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -1682,22 +1682,26 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c
           extended_lanelets.push_back(lanelet_at_left.get());
           lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get());
         }
-        auto lanelet_at_right =
-          planner_data_->route_handler->getRightLanelet(lanelet_at_left.get());
-        while (lanelet_at_right) {
-          extended_lanelets.push_back(lanelet_at_right.get());
-          lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get());
+        if (lanelet_at_left) {
+          auto lanelet_at_right =
+            planner_data_->route_handler->getRightLanelet(lanelet_at_left.get());
+          while (lanelet_at_right) {
+            extended_lanelets.push_back(lanelet_at_right.get());
+            lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get());
+          }
         }
       } else {
         auto lanelet_at_right = route_handler->getRightLanelet(object_lanelet);
         while (lanelet_at_right) {
           extended_lanelets.push_back(lanelet_at_right.get());
           lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get());
         }
-        auto lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_right.get());
-        while (lanelet_at_left) {
-          extended_lanelets.push_back(lanelet_at_left.get());
-          lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get());
+        if (lanelet_at_right) {
+          auto lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_right.get());
+          while (lanelet_at_left) {
+            extended_lanelets.push_back(lanelet_at_left.get());
+            lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get());
+          }
         }
       }
     }