feat(lane_change): enable lane change in crosswalk/intersection if ego vehicle gets stuck

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/base_class.hpp

@@ -209,6 +209,10 @@ class LaneChangeBase
     return direction_;
   }
 
+  boost::optional<Pose> getStopPose() const { return lane_change_stop_pose_; }
+
+  void resetStopPose() { lane_change_stop_pose_ = boost::none; }
+
 protected:
   virtual lanelet::ConstLanelets getCurrentLanes() const = 0;
 
@@ -244,6 +248,7 @@ class LaneChangeBase
   PathWithLaneId prev_module_path_{};
   DrivableAreaInfo prev_drivable_area_info_{};
   TurnSignalInfo prev_turn_signal_info_{};
+  boost::optional<Pose> lane_change_stop_pose_{boost::none};
 
   PathWithLaneId prev_approved_path_{};
 

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

@@ -153,7 +153,14 @@ class NormalLaneChange : public LaneChangeBase
     const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
     const lanelet::ConstLanelets & target_backward_lanes) const;
 
+  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/interface.cpp

@@ -203,6 +203,7 @@ void LaneChangeInterface::updateData()
   module_type_->setPreviousModulePaths(
     getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path);
   module_type_->updateSpecialData();
+  module_type_->resetStopPose();
 }
 
 BehaviorModuleOutput LaneChangeInterface::plan()
@@ -221,6 +222,8 @@ BehaviorModuleOutput LaneChangeInterface::plan()
   *prev_approved_path_ = *getPreviousModuleOutput().path;
   module_type_->insertStopPoint(module_type_->getLaneChangeStatus().target_lanes, *output.path);
 
+  stop_pose_ = module_type_->getStopPose();
+
   updateSteeringFactorPtr(output);
   clearWaitingApproval();
 
@@ -249,6 +252,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
 
   path_reference_ = getPreviousModuleOutput().reference_path;
 
+  stop_pose_ = module_type_->getStopPose();
+
   if (!module_type_->isValidPath()) {
     removeRTCStatus();
     path_candidate_ = std::make_shared<PathWithLaneId>();
@@ -286,6 +291,7 @@ void LaneChangeInterface::updateModuleParams(const std::any & parameters)
 
 void LaneChangeInterface::setData(const std::shared_ptr<const PlannerData> & data)
 {
+  planner_data_ = data;
   module_type_->setData(data);
 }
 

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
@@ -138,6 +140,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput()
     const auto stop_dist =
       -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc));
     const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path);
+    setStopPose(stop_point.point.pose);
   }
 
   const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points);
@@ -198,6 +201,7 @@ void NormalLaneChange::insertStopPoint(
   const double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer;
   if (stopping_distance > 0.0) {
     const auto stop_point = utils::insertStopPoint(stopping_distance, path);
+    setStopPose(stop_point.point.pose);
   }
 }
 
@@ -649,7 +653,6 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
   const lanelet::ConstLanelets & target_backward_lanes) const
 {
   const auto current_pose = getEgoPose();
-  const auto current_vel = getEgoVelocity();
   const auto & route_handler = *getRouteHandler();
   const auto & common_parameters = planner_data_->parameters;
   const auto & objects = *planner_data_->dynamic_object;
@@ -672,12 +675,6 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
   const auto target_backward_polygon = utils::lane_change::createPolygon(
     target_backward_lanes, 0.0, std::numeric_limits<double>::max());
 
-  // minimum distance to lane changing start point
-  const double t_prepare = common_parameters.lane_change_prepare_duration;
-  const double a_min = lane_change_parameters_->min_longitudinal_acc;
-  const double min_dist_to_lane_changing_start = std::max(
-    current_vel * t_prepare, current_vel * t_prepare + 0.5 * a_min * t_prepare * t_prepare);
-
   auto filtered_objects = objects;
 
   utils::path_safety_checker::filterObjectsByClass(
@@ -712,12 +709,8 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
 
     // check if the object intersects with target lanes
     if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) {
-      // ignore static parked object that are in front of the ego vehicle in target lanes
-      bool is_parked_object =
-        utils::lane_change::isParkedObject(target_path, route_handler, extended_object, 0.0, 0.0);
-      if (is_parked_object && min_dist_ego_to_obj > min_dist_to_lane_changing_start) {
-        continue;
-      }
+      // TODO(watanabe): ignore static parked object that are in front of the ego vehicle in target
+      // lanes
 
       filtered_obj_indices.target_lane.push_back(i);
       continue;
@@ -831,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;
 }
 
@@ -1105,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(
@@ -1466,4 +1444,30 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
 
   return path_safety_status;
 }
+
+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