From 865296357907c426a441ca73aab22362af34e5ae Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 12 Nov 2024 13:17:04 +0900 Subject: [PATCH] refactor(lane_change): remove std::optional from lanes polygon (#9288) Signed-off-by: Zulfaqar Azmi --- .../utils/data_structs.hpp | 6 ++-- .../utils/utils.hpp | 7 ++--- .../src/scene.cpp | 20 ++++++------- .../src/utils/utils.cpp | 30 +++++++++---------- 4 files changed, 31 insertions(+), 32 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 d970976d80a8d..16b2b69a81af8 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 { - std::optional current; - std::optional target; - std::optional expanded_target; + lanelet::BasicPolygon2d current; + lanelet::BasicPolygon2d target; + lanelet::BasicPolygon2d 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 4bd5d1e53ed6c..747a991b038e9 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,16 +140,15 @@ std::optional getLeadingStaticObjectIdx( const std::vector & objects, const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold); -std::optional createPolygon( +lanelet::BasicPolygon2d create_polygon( 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 isCollidedPolygonsInLanelet( - const std::vector & collided_polygons, - const std::optional & lanes_polygon); +bool is_collided_polygons_in_lanelet( + const std::vector & collided_polygons, const lanelet::BasicPolygon2d & 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 96f842e7dada1..1e102a8b7542d 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.value(); + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current; 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); @@ -730,7 +730,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.value(), + lanes_polygon, lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(current_pose.position))); } @@ -757,7 +757,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const return false; } - const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current; if (!utils::isEgoWithinOriginalLane( curr_lanes_poly, getEgoPose(), planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance)) { @@ -833,7 +833,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.value(); + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current; 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); @@ -1066,7 +1066,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 && isPolygonOverlapLanelet(object, *polygon); + return !polygon.empty() && isPolygonOverlapLanelet(object, polygon); }; // get backward lanes @@ -1948,7 +1948,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.has_value() || !expanded_target_polygon.has_value()) { + if (current_polygon.empty() || expanded_target_polygon.empty()) { return !is_collided; } @@ -1974,9 +1974,9 @@ bool NormalLaneChange::is_collided( } const auto collision_in_current_lanes = - utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, current_polygon); - const auto collision_in_target_lanes = - utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_polygon); + 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); if (!collision_in_current_lanes && !collision_in_target_lanes) { utils::path_safety_checker::updateCollisionCheckDebugMap( @@ -2062,7 +2062,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.value(); + const auto & target_lane_poly = common_data_ptr_->lanes_polygon_ptr->target; 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 fb7b201c7cd55..4d92b7440d1ec 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,12 +900,13 @@ std::optional getLeadingStaticObjectIdx( return leading_obj_idx; } -std::optional createPolygon( +lanelet::BasicPolygon2d create_polygon( 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(); } @@ -949,12 +950,11 @@ ExtendedPredictedObject transform( return extended_object; } -bool isCollidedPolygonsInLanelet( - const std::vector & collided_polygons, - const std::optional & lanes_polygon) +bool is_collided_polygons_in_lanelet( + const std::vector & collided_polygons, const lanelet::BasicPolygon2d & lanes_polygon) { const auto is_in_lanes = [&](const auto & collided_polygon) { - return lanes_polygon && boost::geometry::intersects(lanes_polygon.value(), collided_polygon); + return !lanes_polygon.empty() && !boost::geometry::disjoint(collided_polygon, lanes_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::createPolygon(lanes->current, 0.0, std::numeric_limits::max()); + utils::lane_change::create_polygon(lanes->current, 0.0, std::numeric_limits::max()); lanes_polygon.target = - utils::lane_change::createPolygon(lanes->target, 0.0, std::numeric_limits::max()); + utils::lane_change::create_polygon(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::createPolygon( + lanes_polygon.expanded_target = utils::lane_change::create_polygon( expanded_target_lanes, 0.0, std::numeric_limits::max()); - lanes_polygon.target_neighbor = *utils::lane_change::createPolygon( + lanes_polygon.target_neighbor = utils::lane_change::create_polygon( 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::createPolygon(preceding_lane, 0.0, std::numeric_limits::max()); + utils::lane_change::create_polygon(preceding_lane, 0.0, std::numeric_limits::max()); - if (lane_polygon) { - lanes_polygon.preceding_target.push_back(*lane_polygon); + if (!lane_polygon.empty()) { + 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.value())) { + object.initial_polygon, common_data_ptr->lanes_polygon_ptr->target)) { return false; } @@ -1312,14 +1312,14 @@ bool has_overtaking_turn_lane_object( } const auto is_path_overlap_with_target = [&](const LineString2d & path) { - return !boost::geometry::disjoint(path, common_data_ptr->lanes_polygon_ptr->target.value()); + return !boost::geometry::disjoint(path, common_data_ptr->lanes_polygon_ptr->target); }; const auto is_object_overlap_with_target = [&](const auto & object) { // to compensate for perception issue, or if object is from behind ego, and tries to overtake, // but stop all of sudden if (!boost::geometry::disjoint( - object.initial_polygon, common_data_ptr->lanes_polygon_ptr->current.value())) { + object.initial_polygon, common_data_ptr->lanes_polygon_ptr->current)) { return true; }