feat(out_of_lane): add option to ignore overlaps in lane changes (aut…

…owarefoundation#6991) Signed-off-by: Maxime CLEMENT <[email protected]>
tier4 · May 13, 2024 · f5b95df · f5b95df
1 parent e521734
commit f5b95df
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 0 deletions.
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
@@ -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
@@ -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
@@ -38,6 +38,8 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node)
   pp.mode = getOrDeclareParameter<std::string>(node, ns + ".mode");
   pp.skip_if_already_overlapping =
     getOrDeclareParameter<bool>(node, ns + ".skip_if_already_overlapping");
+  pp.ignore_overlaps_over_lane_changeable_lanelets =
+    getOrDeclareParameter<bool>(node, ns + ".ignore_overlaps_over_lane_changeable_lanelets");
 
   pp.time_threshold = getOrDeclareParameter<double>(node, ns + ".threshold.time_threshold");
   pp.intervals_ego_buffer = getOrDeclareParameter<double>(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
@@ -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