feat(lane_change): enable lane change in crosswalk/intersection if eg…

…o vehicle gets stuck (autowarefoundation#5105)

* feat(lane_change): enalbe lane change in crosswalk/intersection if ego vehicle stucks

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* docs(lane_change): fix document

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* docs(lane_change): fix words in document

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* refactor(lane_change): refactor initializing

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

---------

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

planning/behavior_path_planner/config/lane_change/lane_change.param.yaml

@@ -60,6 +60,11 @@
         crosswalk: false
         intersection: false
 
+      # ego vehicle stuck detection
+      stuck_detection:
+        velocity: 0.1 # [m/s]
+        stop_time: 3.0 # [s]
+
       # collision check
       enable_prepare_segment_collision_check: true
       prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s]

planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md

@@ -190,6 +190,9 @@ The following figure illustrates when the lane is blocked in multiple lane chang
 
 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`.
+If the ego vehicle gets stuck, to avoid stuck, it enables lane change in crosswalk/intersection.
+If the ego vehicle stops more than `stuck_detection.stop_time` seconds, it is regarded as a stuck.
+If the ego vehicle velocity is smaller than `stuck_detection.velocity`, it is regarded as stopping.
 
 ### Aborting lane change
 
@@ -307,6 +310,13 @@ The following parameters are configurable in `lane_change.param.yaml`.
 | `regulation.crosswalk`    | [-]  | boolean | Regulate lane change on crosswalks    | false         |
 | `regulation.intersection` | [-]  | boolean | Regulate lane change on intersections | false         |
 
+### Ego vehicle stuck detection
+
+| Name                        | Unit  | Type   | Description                                         | Default value |
+| :-------------------------- | ----- | ------ | --------------------------------------------------- | ------------- |
+| `stuck_detection.velocity`  | [m/s] | double | Velocity threshold for ego vehicle stuck detection  | 0.1           |
+| `stuck_detection.stop_time` | [s]   | double | Stop time threshold for ego vehicle stuck detection | 3.0           |
+
 ### Collision checks during lane change
 
 The following parameters are configurable in `behavior_path_planner.param.yaml` and `lane_change.param.yaml`.

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp

@@ -155,7 +155,12 @@ class NormalLaneChange : public LaneChangeBase
 
   void setStopPose(const Pose & stop_pose);
 
+  void updateStopTime();
+
+  double getStopTime() const { return stop_time_; }
+
   rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr());
+  double stop_time_{0.0};
 };
 }  // namespace behavior_path_planner
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp

@@ -70,6 +70,10 @@ struct LaneChangeParameters
   bool regulate_on_crosswalk{false};
   bool regulate_on_intersection{false};
 
+  // ego vehicle stuck detection
+  double stop_velocity_threshold{0.1};
+  double stop_time_threshold{3.0};
+
   // true by default for all objects
   utils::path_safety_checker::ObjectTypesToCheck object_types_to_check;
 

planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp

@@ -80,6 +80,12 @@ LaneChangeModuleManager::LaneChangeModuleManager(
   p.regulate_on_intersection =
     getOrDeclareParameter<bool>(*node, parameter("regulation.intersection"));
 
+  // ego vehicle stuck detection
+  p.stop_velocity_threshold =
+    getOrDeclareParameter<double>(*node, parameter("stuck_detection.velocity"));
+  p.stop_time_threshold =
+    getOrDeclareParameter<double>(*node, parameter("stuck_detection.stop_time"));
+
   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>(

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -39,10 +39,12 @@ NormalLaneChange::NormalLaneChange(
 : LaneChangeBase(parameters, type, direction)
 {
   stop_watch_.tic(getModuleTypeStr());
+  stop_watch_.tic("stop_time");
 }
 
 void NormalLaneChange::updateLaneChangeStatus()
 {
+  updateStopTime();
   const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path);
 
   // Update status
@@ -822,29 +824,6 @@ bool NormalLaneChange::hasEnoughLength(
     return false;
   }
 
-  if (lane_change_parameters_->regulate_on_crosswalk) {
-    const double dist_to_crosswalk_from_lane_change_start_pose =
-      utils::getDistanceToCrosswalk(current_pose, current_lanes, *overall_graphs_ptr) -
-      path.info.length.prepare;
-    // Check lane changing section includes crosswalk
-    if (
-      dist_to_crosswalk_from_lane_change_start_pose > 0.0 &&
-      dist_to_crosswalk_from_lane_change_start_pose < path.info.length.lane_changing) {
-      return false;
-    }
-  }
-
-  if (lane_change_parameters_->regulate_on_intersection) {
-    const double dist_to_intersection_from_lane_change_start_pose =
-      utils::getDistanceToNextIntersection(current_pose, current_lanes) - path.info.length.prepare;
-    // Check lane changing section includes intersection
-    if (
-      dist_to_intersection_from_lane_change_start_pose > 0.0 &&
-      dist_to_intersection_from_lane_change_start_pose < path.info.length.lane_changing) {
-      return false;
-    }
-  }
-
   return true;
 }
 
@@ -1096,14 +1075,22 @@ bool NormalLaneChange::getLaneChangePaths(
           lane_change_parameters_->regulate_on_crosswalk &&
           !hasEnoughLengthToCrosswalk(*candidate_path, current_lanes)) {
           RCLCPP_DEBUG(logger_, "Including crosswalk!!");
-          continue;
+          if (getStopTime() < lane_change_parameters_->stop_time_threshold) {
+            continue;
+          }
+          RCLCPP_WARN_STREAM(
+            logger_, "Stop time is over threshold. Allow lane change in crosswalk.");
         }
 
         if (
           lane_change_parameters_->regulate_on_intersection &&
           !hasEnoughLengthToIntersection(*candidate_path, current_lanes)) {
           RCLCPP_DEBUG(logger_, "Including intersection!!");
-          continue;
+          if (getStopTime() < lane_change_parameters_->stop_time_threshold) {
+            continue;
+          }
+          RCLCPP_WARN_STREAM(
+            logger_, "Stop time is over threshold. Allow lane change in intersection.");
         }
 
         if (utils::lane_change::passParkedObject(
@@ -1463,4 +1450,24 @@ void NormalLaneChange::setStopPose(const Pose & stop_pose)
   lane_change_stop_pose_ = stop_pose;
 }
 
+void NormalLaneChange::updateStopTime()
+{
+  const auto current_vel = getEgoVelocity();
+
+  if (std::abs(current_vel) > lane_change_parameters_->stop_velocity_threshold) {
+    stop_time_ = 0.0;
+  } else {
+    const double duration = stop_watch_.toc("stop_time");
+    // clip stop time
+    if (stop_time_ + duration * 0.001 > lane_change_parameters_->stop_time_threshold) {
+      constexpr double eps = 0.1;
+      stop_time_ = lane_change_parameters_->stop_time_threshold + eps;
+    } else {
+      stop_time_ += duration * 0.001;
+    }
+  }
+
+  stop_watch_.tic("stop_time");
+}
+
 }  // namespace behavior_path_planner