feat(lane_change): regulate lane change on crosswalk and intersection (…

…autowarefoundation#5068) * fix(lane_change): fix hasEnoughDistance() Signed-off-by: Fumiya Watanabe <[email protected]> * feat(lane_change): regulate lane change on crosswalk and intersection Signed-off-by: Fumiya Watanabe <[email protected]> * fix(lane_change): remove unnecessary value Signed-off-by: Fumiya Watanabe <[email protected]> * Update planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp Co-authored-by: Kyoichi Sugahara <[email protected]> * style(pre-commit): autofix * Update planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp Co-authored-by: Zulfaqar Azmi <[email protected]> * style(pre-commit): autofix * fix(lane_change): separate functions and fix Signed-off-by: Fumiya Watanabe <[email protected]> --------- Signed-off-by: Fumiya Watanabe <[email protected]> Co-authored-by: Kyoichi Sugahara <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Zulfaqar Azmi <[email protected]>
tier4 · Sep 26, 2023 · 76087bc · 76087bc
1 parent 0ab8768
commit 76087bc
Show file tree

Hide file tree

Showing 7 changed files with 276 additions and 160 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
@@ -55,6 +55,11 @@
         motorcycle: true
         pedestrian: true
 
+      # lane change regulations
+      regulation:
+        crosswalk: false
+        intersection: false
+
       # collision check
       enable_prepare_segment_collision_check: true
       prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s]

diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md
@@ -186,6 +186,11 @@ The following figure illustrates when the lane is blocked in multiple lane chang
 
 ![multiple-lane-changes](../image/lane_change/lane_change-when_cannot_change_lanes.png)
 
+### Lane change regulations
+
+If you want to regulate lane change on crosswalks or intersections, the lane change module finds a lane change path excluding it includes crosswalks or intersections.
+To regulate lane change on crosswalks or intersections, change `regulation.crosswalk` or `regulation.intersection` to `true`.
+
 ### Aborting lane change
 
 The abort process may result in three different outcome; Cancel, Abort and Stop/Cruise.
@@ -295,6 +300,13 @@ The following parameters are configurable in `lane_change.param.yaml`.
 | `target_object.motorcycle`                  | [-]    | boolean | Include motorcycle objects for safety check                                                                     | true               |
 | `target_object.pedestrian`                  | [-]    | boolean | Include pedestrian objects for safety check                                                                     | true               |
 
+### Lane change regulations
+
+| Name                      | Unit | Type    | Description                           | Default value |
+| :------------------------ | ---- | ------- | ------------------------------------- | ------------- |
+| `regulation.crosswalk`    | [-]  | boolean | Regulate lane change on crosswalks    | false         |
+| `regulation.intersection` | [-]  | boolean | Regulate lane change on intersections | false         |
+
 ### Collision checks during lane change
 
 The following parameters are configurable in `behavior_path_planner.param.yaml` and `lane_change.param.yaml`.

diff --git a/...g/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/...g/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
@@ -108,7 +108,7 @@ class NormalLaneChange : public LaneChangeBase
     const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes) const;
 
-  double calcPrepareDuration(
+  std::vector<double> calcPrepareDuration(
     const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes) const;
 
@@ -129,6 +129,12 @@ class NormalLaneChange : public LaneChangeBase
     const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const;
 
+  bool hasEnoughLengthToCrosswalk(
+    const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const;
+
+  bool hasEnoughLengthToIntersection(
+    const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const;
+
   bool getLaneChangePaths(
     const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
     Direction direction, LaneChangePaths * candidate_paths,

diff --git a/..._path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/..._path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp
@@ -66,6 +66,10 @@ struct LaneChangeParameters
   bool check_objects_on_other_lanes{true};
   bool use_all_predicted_path{false};
 
+  // regulatory elements
+  bool regulate_on_crosswalk{false};
+  bool regulate_on_intersection{false};
+
   // true by default for all objects
   utils::path_safety_checker::ObjectTypesToCheck object_types_to_check;
 

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
@@ -75,6 +75,11 @@ LaneChangeModuleManager::LaneChangeModuleManager(
   p.use_all_predicted_path =
     getOrDeclareParameter<bool>(*node, parameter("use_all_predicted_path"));
 
+  // lane change regulations
+  p.regulate_on_crosswalk = getOrDeclareParameter<bool>(*node, parameter("regulation.crosswalk"));
+  p.regulate_on_intersection =
+    getOrDeclareParameter<bool>(*node, parameter("regulation.intersection"));
+
   p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.longitudinal_distance_min_threshold"));
   p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(