From f5b95dfba71178a985ae57f4abbfa3a6e68ff378 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 13 May 2024 20:59:20 +0900 Subject: [PATCH] feat(out_of_lane): add option to ignore overlaps in lane changes (#6991) Signed-off-by: Maxime CLEMENT --- .../config/out_of_lane.param.yaml | 1 + .../src/lanelets_selection.cpp | 11 +++++++++++ .../src/manager.cpp | 2 ++ .../src/types.hpp | 2 ++ 4 files changed, 16 insertions(+) diff --git a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml index 1b483b95510ed..e4d08367ee4f7 100644 --- a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -3,6 +3,7 @@ out_of_lane: # module to stop or slowdown before overlapping another lane with other objects mode: ttc # mode used to consider a conflict with an object. "threshold", "intervals", or "ttc" skip_if_already_overlapping: true # do not run this module when ego already overlaps another lane + ignore_overlaps_over_lane_changeable_lanelets: true # if true, overlaps on lane changeable lanelets are ignored threshold: time_threshold: 5.0 # [s] consider objects that will reach an overlap within this time diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp index 738ea22106b29..67d8a79a63f03 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -78,6 +78,15 @@ lanelet::ConstLanelets calculate_path_lanelets( return path_lanelets; } +void add_lane_changeable_lanelets( + lanelet::ConstLanelets & lanelets_to_ignore, const lanelet::ConstLanelets & path_lanelets, + const route_handler::RouteHandler & route_handler) +{ + for (const auto & path_lanelet : path_lanelets) + for (const auto & ll : route_handler.getLaneChangeableNeighbors(path_lanelet)) + if (!contains_lanelet(lanelets_to_ignore, ll.id())) lanelets_to_ignore.push_back(ll); +} + lanelet::ConstLanelets calculate_ignored_lanelets( const EgoData & ego_data, const lanelet::ConstLanelets & path_lanelets, const route_handler::RouteHandler & route_handler, const PlannerParam & params) @@ -93,6 +102,8 @@ lanelet::ConstLanelets calculate_ignored_lanelets( const auto is_path_lanelet = contains_lanelet(path_lanelets, l.second.id()); if (!is_path_lanelet) ignored_lanelets.push_back(l.second); } + if (params.ignore_overlaps_over_lane_changeable_lanelets) + add_lane_changeable_lanelets(ignored_lanelets, path_lanelets, route_handler); return ignored_lanelets; } diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 8055f5c9106ef..7d764722405af 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -38,6 +38,8 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) pp.mode = getOrDeclareParameter(node, ns + ".mode"); pp.skip_if_already_overlapping = getOrDeclareParameter(node, ns + ".skip_if_already_overlapping"); + pp.ignore_overlaps_over_lane_changeable_lanelets = + getOrDeclareParameter(node, ns + ".ignore_overlaps_over_lane_changeable_lanelets"); pp.time_threshold = getOrDeclareParameter(node, ns + ".threshold.time_threshold"); pp.intervals_ego_buffer = getOrDeclareParameter(node, ns + ".intervals.ego_time_buffer"); diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp index d482870e5ce18..ff690699ee638 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -39,6 +39,8 @@ struct PlannerParam std::string mode; // mode used to consider a conflict with an object bool skip_if_already_overlapping; // if true, do not run the module when ego already overlaps // another lane + bool ignore_overlaps_over_lane_changeable_lanelets; // if true, overlaps on lane changeable + // lanelets are ignored double time_threshold; // [s](mode="threshold") objects time threshold double intervals_ego_buffer; // [s](mode="intervals") buffer to extend the ego time range