From 1ea45e6d916037ca5055776790e6b9096695b8fe Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Fri, 8 Nov 2024 22:14:22 +0900 Subject: [PATCH] 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 0c70ea8793985c6aae90f851eeffdd2561fe04b3. --- .../utils/data_structs.hpp | 6 ++--- .../utils/utils.hpp | 7 ++--- .../src/scene.cpp | 20 +++++++------- .../src/utils/utils.cpp | 26 +++++++++---------- 4 files changed, 30 insertions(+), 29 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 16b2b69a81af8..d970976d80a8d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/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 current; + std::optional target; + std::optional expanded_target; lanelet::BasicPolygon2d target_neighbor; std::vector preceding_target; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 747a991b038e9..4bd5d1e53ed6c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/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 getLeadingStaticObjectIdx( const std::vector & objects, const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold); -lanelet::BasicPolygon2d create_polygon( +std::optional 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 & collided_polygons, const lanelet::BasicPolygon2d & lanes_polygon); +bool isCollidedPolygonsInLanelet( + const std::vector & collided_polygons, + const std::optional & lanes_polygon); /** * @brief Generates expanded lanelets based on the given direction and offsets. diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 4c4f35f7dbddc..aea6cda6c6807 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/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); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 1711bf1ddfacc..fb7b201c7cd55 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -900,13 +900,12 @@ std::optional getLeadingStaticObjectIdx( return leading_obj_idx; } -lanelet::BasicPolygon2d create_polygon( +std::optional 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 & collided_polygons, const lanelet::BasicPolygon2d & lanes_polygon) +bool isCollidedPolygonsInLanelet( + const std::vector & collided_polygons, + const std::optional & 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::max()); + utils::lane_change::createPolygon(lanes->current, 0.0, std::numeric_limits::max()); lanes_polygon.target = - utils::lane_change::create_polygon(lanes->target, 0.0, std::numeric_limits::max()); + utils::lane_change::createPolygon(lanes->target, 0.0, std::numeric_limits::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::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::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::max()); + utils::lane_change::createPolygon(preceding_lane, 0.0, std::numeric_limits::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; }