From eb4733fcfd56c969be089df552c760ff6c92dbca Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 2 Oct 2023 10:55:00 +0900 Subject: [PATCH] feat(lane_change): expand target lanes for object filtering (#5167) * feat(lane_change): expand target lanes for object filtering Signed-off-by: Muhammad Zulfaqar Azmi * use expanded target lanes in collision check Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi --- .../config/lane_change/lane_change.param.yaml | 5 +++++ .../lane_change/lane_change_module_data.hpp | 2 ++ .../utils/lane_change/utils.hpp | 19 +++++++++++++++++++ .../src/scene_module/lane_change/manager.cpp | 5 ++++- .../src/scene_module/lane_change/normal.cpp | 16 ++++++++++++---- .../src/utils/lane_change/utils.cpp | 9 +++++++++ 6 files changed, 51 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 583bdeaaf8dc5..fda7390abdb9e 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -38,6 +38,11 @@ longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.8 + # lane expansion for object filtering + lane_expansion: + left_offset: 0.0 # [m] + right_offset: 0.0 # [m] + # lateral acceleration map lateral_acceleration: velocity: [0.0, 4.0, 10.0] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index d3555eae83539..1b80a69250a2d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -65,6 +65,8 @@ struct LaneChangeParameters bool check_objects_on_current_lanes{true}; bool check_objects_on_other_lanes{true}; bool use_all_predicted_path{false}; + double lane_expansion_left_offset{0.0}; + double lane_expansion_right_offset{0.0}; // regulatory elements bool regulate_on_crosswalk{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 3d7a7853ec6af..70ea047f2a19f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -191,5 +191,24 @@ ExtendedPredictedObject transform( bool isCollidedPolygonsInLanelet( const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes); + +/** + * @brief Generates expanded lanelets based on the given direction and offsets. + * + * Expands the provided lanelets in either the left or right direction based on + * the specified direction. If the direction is 'LEFT', the lanelets are expanded + * using the left_offset; if 'RIGHT', they are expanded using the right_offset. + * Otherwise, no expansion occurs. + * + * @param lanes The lanelets to be expanded. + * @param direction The direction of expansion: either LEFT or RIGHT. + * @param left_offset The offset value for left expansion. + * @param right_offset The offset value for right expansion. + * @return lanelet::ConstLanelets A collection of expanded lanelets. + */ +lanelet::ConstLanelets generateExpandedLanelets( + const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset, + const double right_offset); + } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 8d90a0a13179c..f273c9e2546d1 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -74,7 +74,10 @@ LaneChangeModuleManager::LaneChangeModuleManager( getOrDeclareParameter(*node, parameter("check_objects_on_other_lanes")); p.use_all_predicted_path = getOrDeclareParameter(*node, parameter("use_all_predicted_path")); - + p.lane_expansion_left_offset = + getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.left_offset")); + p.lane_expansion_right_offset = + getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.right_offset")); // lane change regulations p.regulate_on_crosswalk = getOrDeclareParameter(*node, parameter("regulation.crosswalk")); p.regulate_on_intersection = diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index c1235710a5e1d..d5d7e77b13d5f 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -670,8 +670,11 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto current_polygon = utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); - const auto target_polygon = - utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); + const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( + target_lanes, direction_, lane_change_parameters_->lane_expansion_left_offset, + lane_change_parameters_->lane_expansion_right_offset); + const auto target_polygon = utils::lane_change::createPolygon( + expanded_target_lanes, 0.0, std::numeric_limits::max()); const auto dist_ego_to_current_lanes_center = lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); std::vector> target_backward_polygons; @@ -1443,6 +1446,11 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( target_objects.other_lane.end()); } + const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( + lane_change_path.info.target_lanes, direction_, + lane_change_parameters_->lane_expansion_left_offset, + lane_change_parameters_->lane_expansion_right_offset); + for (const auto & obj : collision_check_objects) { auto current_debug_data = marker_utils::createObjectDebug(obj); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( @@ -1458,8 +1466,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto collision_in_current_lanes = utils::lane_change::isCollidedPolygonsInLanelet( collided_polygons, lane_change_path.info.current_lanes); - const auto collision_in_target_lanes = utils::lane_change::isCollidedPolygonsInLanelet( - collided_polygons, lane_change_path.info.target_lanes); + const auto collision_in_target_lanes = + utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_lanes); if (!collision_in_current_lanes && !collision_in_target_lanes) { continue; diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index fd7d8406af6c8..269eb194988e1 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -1123,4 +1123,13 @@ bool isCollidedPolygonsInLanelet( return std::any_of(collided_polygons.begin(), collided_polygons.end(), is_in_lanes); } + +lanelet::ConstLanelets generateExpandedLanelets( + const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset, + const double right_offset) +{ + const auto left_extend_offset = (direction == Direction::LEFT) ? left_offset : 0.0; + const auto right_extend_offset = (direction == Direction::RIGHT) ? -right_offset : 0.0; + return lanelet::utils::getExpandedLanelets(lanes, left_extend_offset, right_extend_offset); +} } // namespace behavior_path_planner::utils::lane_change