refactor(lane_change): revert "remove std::optional from lanes polygon" (#9272)

Revert "refactor(lane_change): remove std::optional from lanes polygon (#9267)"

This reverts commit 0c70ea8.

Author: youtalk

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp

@@ -311,9 +311,9 @@ struct PathSafetyStatus
 
 struct LanesPolygon
 {
-  lanelet::BasicPolygon2d current;
-  lanelet::BasicPolygon2d target;
-  lanelet::BasicPolygon2d expanded_target;
+  std::optional<lanelet::BasicPolygon2d> current;
+  std::optional<lanelet::BasicPolygon2d> target;
+  std::optional<lanelet::BasicPolygon2d> expanded_target;
   lanelet::BasicPolygon2d target_neighbor;
   std::vector<lanelet::BasicPolygon2d> preceding_target;
 };

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp

@@ -140,15 +140,16 @@ std::optional<size_t> getLeadingStaticObjectIdx(
   const std::vector<ExtendedPredictedObject> & objects,
   const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold);
 
-lanelet::BasicPolygon2d create_polygon(
+std::optional<lanelet::BasicPolygon2d> createPolygon(
   const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist);
 
 ExtendedPredictedObject transform(
   const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters,
   const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase);
 
-bool is_collided_polygons_in_lanelet(
-  const std::vector<Polygon2d> & collided_polygons, const lanelet::BasicPolygon2d & lanes_polygon);
+bool isCollidedPolygonsInLanelet(
+  const std::vector<Polygon2d> & collided_polygons,
+  const std::optional<lanelet::BasicPolygon2d> & lanes_polygon);
 
 /**
  * @brief Generates expanded lanelets based on the given direction and offsets.

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -512,7 +512,7 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path)
     dist_to_terminal - min_dist_buffer - calculation::calc_stopping_distance(lc_param_ptr);
 
   const auto distance_to_last_fit_width = std::invoke([&]() -> double {
-    const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current;
+    const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value();
     if (utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), *bpp_param_ptr)) {
       return utils::lane_change::calculation::calc_dist_to_last_fit_width(
         lanes_ptr->current, path.points.front().point.pose, *bpp_param_ptr);
@@ -740,7 +740,7 @@ bool NormalLaneChange::hasFinishedLaneChange() const
   if (has_passed_end_pose) {
     const auto & lanes_polygon = common_data_ptr_->lanes_polygon_ptr->target;
     return !boost::geometry::disjoint(
-      lanes_polygon,
+      lanes_polygon.value(),
       lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(current_pose.position)));
   }
 
@@ -767,7 +767,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const
     return false;
   }
 
-  const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current;
+  const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value();
   if (!utils::isEgoWithinOriginalLane(
         curr_lanes_poly, getEgoPose(), planner_data_->parameters,
         lane_change_parameters_->cancel.overhang_tolerance)) {
@@ -843,7 +843,7 @@ bool NormalLaneChange::isAbleToStopSafely() const
   const auto stop_dist =
     -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc));
 
-  const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current;
+  const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value();
   double dist = 0.0;
   for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) {
     dist += calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1);
@@ -1076,7 +1076,7 @@ FilteredByLanesObjects NormalLaneChange::filterObjectsByLanelets(
   const auto & route_handler = getRouteHandler();
   const auto & common_parameters = planner_data_->parameters;
   const auto check_optional_polygon = [](const auto & object, const auto & polygon) {
-    return !polygon.empty() && isPolygonOverlapLanelet(object, polygon);
+    return polygon && isPolygonOverlapLanelet(object, *polygon);
   };
 
   // get backward lanes
@@ -1963,7 +1963,7 @@ bool NormalLaneChange::is_collided(
   const auto & current_polygon = lanes_polygon_ptr->current;
   const auto & expanded_target_polygon = lanes_polygon_ptr->target;
 
-  if (current_polygon.empty() || expanded_target_polygon.empty()) {
+  if (!current_polygon.has_value() || !expanded_target_polygon.has_value()) {
     return !is_collided;
   }
 
@@ -1989,9 +1989,9 @@ bool NormalLaneChange::is_collided(
     }
 
     const auto collision_in_current_lanes =
-      utils::lane_change::is_collided_polygons_in_lanelet(collided_polygons, current_polygon);
-    const auto collision_in_target_lanes = utils::lane_change::is_collided_polygons_in_lanelet(
-      collided_polygons, expanded_target_polygon);
+      utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, current_polygon);
+    const auto collision_in_target_lanes =
+      utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_polygon);
 
     if (!collision_in_current_lanes && !collision_in_target_lanes) {
       utils::path_safety_checker::updateCollisionCheckDebugMap(
@@ -2077,7 +2077,7 @@ bool NormalLaneChange::is_valid_start_point(
   const lanelet::BasicPoint2d lc_start_point(pose.position.x, pose.position.y);
 
   const auto & target_neighbor_poly = common_data_ptr->lanes_polygon_ptr->target_neighbor;
-  const auto & target_lane_poly = common_data_ptr_->lanes_polygon_ptr->target;
+  const auto & target_lane_poly = common_data_ptr_->lanes_polygon_ptr->target.value();
 
   return boost::geometry::covered_by(lc_start_point, target_neighbor_poly) ||
          boost::geometry::covered_by(lc_start_point, target_lane_poly);

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp

@@ -900,13 +900,12 @@ std::optional<size_t> getLeadingStaticObjectIdx(
   return leading_obj_idx;
 }
 
-lanelet::BasicPolygon2d create_polygon(
+std::optional<lanelet::BasicPolygon2d> createPolygon(
   const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist)
 {
   if (lanes.empty()) {
     return {};
   }
-
   const auto polygon_3d = lanelet::utils::getPolygonFromArcLength(lanes, start_dist, end_dist);
   return lanelet::utils::to2D(polygon_3d).basicPolygon();
 }
@@ -950,11 +949,12 @@ ExtendedPredictedObject transform(
   return extended_object;
 }
 
-bool is_collided_polygons_in_lanelet(
-  const std::vector<Polygon2d> & collided_polygons, const lanelet::BasicPolygon2d & lanes_polygon)
+bool isCollidedPolygonsInLanelet(
+  const std::vector<Polygon2d> & collided_polygons,
+  const std::optional<lanelet::BasicPolygon2d> & lanes_polygon)
 {
   const auto is_in_lanes = [&](const auto & collided_polygon) {
-    return !lanes_polygon.empty() && !boost::geometry::disjoint(collided_polygon, lanes_polygon);
+    return lanes_polygon && boost::geometry::intersects(lanes_polygon.value(), collided_polygon);
   };
 
   return std::any_of(collided_polygons.begin(), collided_polygons.end(), is_in_lanes);
@@ -1033,28 +1033,28 @@ LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr)
   LanesPolygon lanes_polygon;
 
   lanes_polygon.current =
-    utils::lane_change::create_polygon(lanes->current, 0.0, std::numeric_limits<double>::max());
+    utils::lane_change::createPolygon(lanes->current, 0.0, std::numeric_limits<double>::max());
 
   lanes_polygon.target =
-    utils::lane_change::create_polygon(lanes->target, 0.0, std::numeric_limits<double>::max());
+    utils::lane_change::createPolygon(lanes->target, 0.0, std::numeric_limits<double>::max());
 
   const auto & lc_param_ptr = common_data_ptr->lc_param_ptr;
   const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets(
     lanes->target, common_data_ptr->direction, lc_param_ptr->lane_expansion_left_offset,
     lc_param_ptr->lane_expansion_right_offset);
-  lanes_polygon.expanded_target = utils::lane_change::create_polygon(
+  lanes_polygon.expanded_target = utils::lane_change::createPolygon(
     expanded_target_lanes, 0.0, std::numeric_limits<double>::max());
 
-  lanes_polygon.target_neighbor = utils::lane_change::create_polygon(
+  lanes_polygon.target_neighbor = *utils::lane_change::createPolygon(
     lanes->target_neighbor, 0.0, std::numeric_limits<double>::max());
 
   lanes_polygon.preceding_target.reserve(lanes->preceding_target.size());
   for (const auto & preceding_lane : lanes->preceding_target) {
     auto lane_polygon =
-      utils::lane_change::create_polygon(preceding_lane, 0.0, std::numeric_limits<double>::max());
+      utils::lane_change::createPolygon(preceding_lane, 0.0, std::numeric_limits<double>::max());
 
-    if (!lane_polygon.empty()) {
-      lanes_polygon.preceding_target.push_back(lane_polygon);
+    if (lane_polygon) {
+      lanes_polygon.preceding_target.push_back(*lane_polygon);
     }
   }
   return lanes_polygon;
@@ -1260,7 +1260,7 @@ bool has_blocking_target_object(
 
       // filtered_objects includes objects out of target lanes, so filter them out
       if (boost::geometry::disjoint(
-            object.initial_polygon, common_data_ptr->lanes_polygon_ptr->target)) {
+            object.initial_polygon, common_data_ptr->lanes_polygon_ptr->target.value())) {
         return false;
       }