fix(behavior_path_planner): crash when drivable area expanding (autow…

…arefoundation#365)

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>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

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());
+          }
         }
       }
     }