feat(crosswalk)!: update stop position caluculation

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/README.md

@@ -12,7 +12,6 @@ This module judges whether the ego should stop in front of the crosswalk in orde
 
 ```plantuml
 @startuml
-skinparam monochrome true
 
 title modifyPathVelocity
 start
@@ -38,8 +37,36 @@ group apply stop
     :planStop;
   endif
 end group
+stop
+@enduml
+```
 
+```plantuml
+@startuml
+
+title checkStopForCrosswalkUsers
+start
+group calculate the candidate stop
+  :pick the closest stop point against the pedestrians and stop point on map as the preferred stop;
+  if (the weak brake distance is closer than the preferred stop?) then (yes)
+    :plan to stop at the preferred stop;
+  else (no)
+    if (the weak brake distance is closer than the limit stop position against the nearest pedestrian?) then (yes)
+      :plan to stop by the weak brake distance;
+    else (no)
+      :plan to stop at the limit stop position against the nearest pedestrian;
+    endif
+  endif
+end group
+group check if the candidate stop pose is acceptable for braking distance
+  if (the stop pose candidate is closer than the acceptable stop dist?) then (yes)
+    :abort to stop.;
+  else (no)
+    :plan to stop at the candidate stop pose;
+  endif
+end group
 stop
+
 @enduml
 ```
 

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml

@@ -16,7 +16,10 @@
         # For the case where the crosswalk width is very wide
         far_object_threshold: 10.0 # [m] If objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object).
         # For the case where the stop position is determined according to the object position.
-        stop_distance_from_object: 3.0 # [m] the vehicle decelerates to be able to stop in front of object with margin
+        stop_distance_from_object_preferred: 3.0 # [m]
+        stop_distance_from_object_limit: 3.0 # [m]
+        min_acc_preferred: -1.0 # min acceleration [m/ss]
+        min_jerk_preferred: -1.0 # min jerk [m/sss]
 
       # params for ego's slow down velocity. These params are not used for the case of "enable_rtc: false".
       slow_down:
@@ -45,11 +48,10 @@
         ego_pass_later_additional_margin: 0.5 # [s] additional time margin for object pass first situation to suppress chattering
         ego_min_assumed_speed: 2.0 # [m/s] assumed speed to calculate the time to collision point
 
-        no_stop_decision: # parameters to determine if it is safe to attempt stopping before the crosswalk
-          max_offset_to_crosswalk_for_yield: 0.0 # [m] maximum offset from ego's front to crosswalk for yield. Positive value means in front of the crosswalk.
-          min_acc: -1.0 # min acceleration [m/ss]
-          min_jerk: -1.0 # min jerk [m/sss]
-          max_jerk: 1.0 # max jerk [m/sss]
+        no_stop_decision: # parameters to determine stop cancel. {-$overrun_threshold_length + f($min_acc, $min_jerk)} is compared against distance to stop pose.
+          min_acc: -1.5 # min acceleration [m/ss]
+          min_jerk: -1.5 # min jerk [m/sss]
+          overrun_threshold_length: 1.0 # [m] required to avoid giving up against backward movement of the stop position due to approaching pedestrians, etc.
 
         stop_object_velocity_threshold: 0.25 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph)
         min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph)

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp

@@ -47,12 +47,18 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
   // param for stop position
   cp.stop_distance_from_crosswalk =
     getOrDeclareParameter<double>(node, ns + ".stop_position.stop_distance_from_crosswalk");
-  cp.stop_distance_from_object =
-    getOrDeclareParameter<double>(node, ns + ".stop_position.stop_distance_from_object");
+  cp.stop_distance_from_object_preferred =
+    getOrDeclareParameter<double>(node, ns + ".stop_position.stop_distance_from_object_preferred");
+  cp.stop_distance_from_object_limit =
+    getOrDeclareParameter<double>(node, ns + ".stop_position.stop_distance_from_object_limit");
   cp.far_object_threshold =
     getOrDeclareParameter<double>(node, ns + ".stop_position.far_object_threshold");
   cp.stop_position_threshold =
     getOrDeclareParameter<double>(node, ns + ".stop_position.stop_position_threshold");
+  cp.min_acc_preferred =
+    getOrDeclareParameter<double>(node, ns + ".stop_position.min_acc_preferred");
+  cp.min_jerk_preferred =
+    getOrDeclareParameter<double>(node, ns + ".stop_position.min_jerk_preferred");
 
   // param for restart suppression
   cp.min_dist_to_stop_for_restart_suppression =
@@ -98,14 +104,12 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<double>(node, ns + ".pass_judge.ego_pass_later_additional_margin");
   cp.ego_min_assumed_speed =
     getOrDeclareParameter<double>(node, ns + ".pass_judge.ego_min_assumed_speed");
-  cp.max_offset_to_crosswalk_for_yield = getOrDeclareParameter<double>(
-    node, ns + ".pass_judge.no_stop_decision.max_offset_to_crosswalk_for_yield");
   cp.min_acc_for_no_stop_decision =
     getOrDeclareParameter<double>(node, ns + ".pass_judge.no_stop_decision.min_acc");
-  cp.max_jerk_for_no_stop_decision =
-    getOrDeclareParameter<double>(node, ns + ".pass_judge.no_stop_decision.max_jerk");
   cp.min_jerk_for_no_stop_decision =
     getOrDeclareParameter<double>(node, ns + ".pass_judge.no_stop_decision.min_jerk");
+  cp.overrun_threshold_length_for_no_stop_decision = getOrDeclareParameter<double>(
+    node, ns + ".pass_judge.no_stop_decision.overrun_threshold_length");
   cp.stop_object_velocity =
     getOrDeclareParameter<double>(node, ns + ".pass_judge.stop_object_velocity_threshold");
   cp.min_object_velocity =

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -1,4 +1,4 @@
 // Copyright 2020-2023 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -316,104 +316,177 @@
   const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path,
   const geometry_msgs::msg::Point & first_path_point_on_crosswalk,
   const geometry_msgs::msg::Point & last_path_point_on_crosswalk,
-  const std::optional<geometry_msgs::msg::Pose> & default_stop_pose)
+  const std::optional<geometry_msgs::msg::Pose> & default_stop_pose_opt)
 {
   const auto & ego_pos = planner_data_->current_odometry->pose.position;
-  const double ego_vel = planner_data_->current_velocity->twist.linear.x;
-  const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x;
-
-  const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
-  const auto dist_ego_to_stop =
-    calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose->position);
 
   // Calculate attention range for crosswalk
   const auto crosswalk_attention_range = getAttentionRange(
     sparse_resample_path, first_path_point_on_crosswalk, last_path_point_on_crosswalk);
 
   // Get attention area, which is ego's footprints on the crosswalk
   const auto attention_area = getAttentionArea(sparse_resample_path, crosswalk_attention_range);
 
   // Update object state
+  // This exceptional handling should be done in update(), but is compromised by history
+  const double dist_default_stop =
+    default_stop_pose_opt.has_value()
+      ? calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose_opt->position)
+      : std::numeric_limits<double>::max();
   updateObjectState(
-    dist_ego_to_stop, sparse_resample_path, crosswalk_attention_range, attention_area);
-
-  // Check if ego moves forward enough to ignore yield.
-  const auto & p = planner_param_;
-  const double dist_ego2crosswalk =
-    calcSignedArcLength(ego_path.points, ego_pos, first_path_point_on_crosswalk);
-  const auto braking_distance_opt = autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints(
-    ego_vel, 0.0, ego_acc, p.min_acc_for_no_stop_decision, p.max_jerk_for_no_stop_decision,
-    p.min_jerk_for_no_stop_decision);
-  const double braking_distance = braking_distance_opt ? *braking_distance_opt : 0.0;
-  if (
-    dist_ego2crosswalk - base_link2front - braking_distance < p.max_offset_to_crosswalk_for_yield) {
-    return {};
-  }
+    dist_default_stop, sparse_resample_path, crosswalk_attention_range, attention_area);
 
   // Check pedestrian for stop
   // NOTE: first stop point and its minimum distance from ego to stop
   auto isVehicleType = [](const uint8_t label) {
     return label == ObjectClassification::MOTORCYCLE || label == ObjectClassification::BICYCLE;
   };
-  std::optional<std::pair<geometry_msgs::msg::Point, double>> nearest_stop_info;
+  std::optional<double> dist_nearest_cp;
   std::vector<geometry_msgs::msg::Point> stop_factor_points;
   const std::optional<double> ego_crosswalk_passage_direction =
     findEgoPassageDirectionAlongPath(sparse_resample_path);
   for (const auto & object : object_info_manager_.getObject()) {
     const auto & collision_point_opt = object.collision_point;
     if (collision_point_opt) {
       const auto & collision_point = collision_point_opt.value();
       const auto & collision_state = object.collision_state;
       if (collision_state != CollisionState::YIELD) {
         continue;
       }
       if (
         isVehicleType(object.classification) && ego_crosswalk_passage_direction &&
         collision_point.crosswalk_passage_direction) {
         const double direction_diff = autoware::universe_utils::normalizeRadian(
           collision_point.crosswalk_passage_direction.value() -
           ego_crosswalk_passage_direction.value());
         if (std::fabs(direction_diff) < planner_param_.vehicle_object_cross_angle_threshold) {
           continue;
         }
       }
 
       stop_factor_points.push_back(object.position);
 
       const auto dist_ego2cp =
-        calcSignedArcLength(sparse_resample_path.points, ego_pos, collision_point.collision_point) -
-        planner_param_.stop_distance_from_object;
-      if (!nearest_stop_info || dist_ego2cp - base_link2front < nearest_stop_info->second) {
-        nearest_stop_info =
-          std::make_pair(collision_point.collision_point, dist_ego2cp - base_link2front);
+        calcSignedArcLength(sparse_resample_path.points, ego_pos, collision_point.collision_point);
+      if (!dist_nearest_cp || dist_ego2cp < dist_nearest_cp) {
+        dist_nearest_cp = dist_ego2cp;
       }
     }
   }
+  if (!dist_nearest_cp) {
+    return {};
+  }
 
-  // Check if stop is required
-  if (!nearest_stop_info) {
+  const auto decided_stop_pose_opt =
+    calcStopPose(ego_path, dist_nearest_cp.value(), default_stop_pose_opt);
+  if (!decided_stop_pose_opt.has_value()) {
     return {};
   }
+  return createStopFactor(decided_stop_pose_opt.value(), stop_factor_points);
+}
 
-  // Check if the ego should stop at the stop line or the other points.
-  const bool stop_at_stop_line =
-    dist_ego_to_stop < nearest_stop_info->second &&
-    nearest_stop_info->second < dist_ego_to_stop + planner_param_.far_object_threshold;
+std::optional<geometry_msgs::msg::Pose> CrosswalkModule::calcStopPose(
+  const PathWithLaneId & ego_path, double dist_nearest_cp,
+  const std::optional<geometry_msgs::msg::Pose> & default_stop_pose_opt)
+{
+  struct StopCandidate
+  {
+    geometry_msgs::msg::Pose pose;
+    double dist;
+  };
+
+  const auto & p = planner_param_;
+  const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
+  const auto & ego_pos = planner_data_->current_odometry->pose.position;
+  const double ego_vel_non_negative =
+    std::max(0.0, planner_data_->current_velocity->twist.linear.x);
+  const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x;
 
-  if (stop_at_stop_line) {
-    // Stop at the stop line
-    if (default_stop_pose) {
-      return createStopFactor(*default_stop_pose, stop_factor_points);
+  const auto default_stop_opt = [&]() -> std::optional<StopCandidate> {
+    if (!default_stop_pose_opt.has_value()) return std::nullopt;
+    return StopCandidate{
+      default_stop_pose_opt.value(),
+      calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose_opt->position)};
+  }();
+
+  const auto ped_stop_pref_opt = [&]() -> std::optional<StopCandidate> {
+    const double dist =
+      dist_nearest_cp - base_link2front - planner_param_.stop_distance_from_object_preferred;
+    const auto pose_opt = calcLongitudinalOffsetPose(ego_path.points, ego_pos, dist);
+    if (!pose_opt.has_value()) return std::nullopt;
+    return StopCandidate{pose_opt.value(), dist};
+  }();
+
+  const auto without_acc_pref_stop_opt = [&]() -> std::optional<StopCandidate> {
+    if (!ped_stop_pref_opt.has_value()) {
+      RCLCPP_INFO(logger_, "Failure to calculate pref_stop.");
+      return std::nullopt;
+    } else if (
+      default_stop_opt.has_value() && ped_stop_pref_opt->dist > default_stop_opt->dist &&
+      ped_stop_pref_opt->dist < default_stop_opt->dist + planner_param_.far_object_threshold) {
+      return default_stop_opt;
+    } else {
+      return ped_stop_pref_opt;
     }
-  } else {
-    const auto stop_pose = calcLongitudinalOffsetPose(
-      ego_path.points, nearest_stop_info->first,
-      -base_link2front - planner_param_.stop_distance_from_object);
-    if (stop_pose) {
-      return createStopFactor(*stop_pose, stop_factor_points);
+  }();
+
+  const auto ped_stop_limit = [&]() -> std::optional<StopCandidate> {
+    const double dist =
+      dist_nearest_cp - base_link2front - planner_param_.stop_distance_from_object_limit;
+    const auto pose_opt = calcLongitudinalOffsetPose(ego_path.points, ego_pos, dist);
+    if (!pose_opt.has_value()) return std::nullopt;
+    return StopCandidate{pose_opt.value(), dist};
+  }();
+  if (!ped_stop_limit.has_value()) {
+    RCLCPP_WARN(
+      logger_,
+      "Stop is canceled. "
+      "Failure to calculate stop_pose against the nearest pedestrian with a limit margin.");
+    return std::nullopt;
+  }
+
+  const auto weak_brk_stop = [&]() -> std::optional<StopCandidate> {
+    const auto dist_opt = autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints(
+      ego_vel_non_negative, 0.0, ego_acc, p.min_acc_preferred, 10.0, p.min_jerk_preferred);
+    if (!dist_opt.has_value()) return std::nullopt;
+    const auto pose_opt = calcLongitudinalOffsetPose(ego_path.points, ego_pos, dist_opt.value());
+    if (!pose_opt.has_value()) return std::nullopt;
+    return StopCandidate{pose_opt.value(), dist_opt.value()};
+  }();
+  if (!weak_brk_stop.has_value()) {
+    RCLCPP_ERROR(logger_, "Failure to calculate braking distance. Stop will be canceled.");
+    return std::nullopt;
+  }
+
+  const auto selected_stop = [&]() {
+    if (
+      without_acc_pref_stop_opt.has_value() &&
+      weak_brk_stop->dist < without_acc_pref_stop_opt->dist) {
+      return without_acc_pref_stop_opt.value();
+    } else if (weak_brk_stop->dist < ped_stop_limit->dist) {
+      return weak_brk_stop.value();
+    } else {
+      return ped_stop_limit.value();
     }
+  }();
+
+  const double strong_brk_dist = [&]() {
+    const auto strong_brk_dist_opt = autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints(
+      ego_vel_non_negative, 0.0, ego_acc, p.min_acc_for_no_stop_decision, 10.0,
+      p.min_jerk_for_no_stop_decision);
+    return strong_brk_dist_opt ? strong_brk_dist_opt.value() : 0.0;
+  }();
+  if (selected_stop.dist < strong_brk_dist - p.overrun_threshold_length_for_no_stop_decision) {
+    RCLCPP_INFO(
+      logger_,
+      "Abandon to stop. "
+      "Can not stop against the nearest pedestrian with a specified deceleration. "
+      "dist to stop: %f, braking distance: %f",
+      selected_stop.dist, strong_brk_dist);
+    return std::nullopt;
   }
-  return {};
+
+  return std::make_optional(selected_stop.pose);
 }
 
 std::pair<double, double> CrosswalkModule::getAttentionRange(

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -112,10 +112,13 @@ class CrosswalkModule : public SceneModuleInterface
   {
     bool show_processing_time;
     // param for stop position
-    double stop_distance_from_object;
+    double stop_distance_from_object_preferred;
+    double stop_distance_from_object_limit;
     double stop_distance_from_crosswalk;
     double far_object_threshold;
     double stop_position_threshold;
+    double min_acc_preferred;
+    double min_jerk_preferred;
     // param for restart suppression
     double min_dist_to_stop_for_restart_suppression;
     double max_dist_to_stop_for_restart_suppression;
@@ -140,10 +143,9 @@ class CrosswalkModule : public SceneModuleInterface
     std::vector<double> ego_pass_later_margin_y;
     double ego_pass_later_additional_margin;
     double ego_min_assumed_speed;
-    double max_offset_to_crosswalk_for_yield;
     double min_acc_for_no_stop_decision;
-    double max_jerk_for_no_stop_decision;
     double min_jerk_for_no_stop_decision;
+    double overrun_threshold_length_for_no_stop_decision;
     double stop_object_velocity;
     double min_object_velocity;
     bool disable_yield_for_new_stopped_object;
@@ -352,6 +354,10 @@ class CrosswalkModule : public SceneModuleInterface
     const PathWithLaneId & ego_path,
     const geometry_msgs::msg::Point & first_path_point_on_crosswalk) const;
 
+  std::optional<geometry_msgs::msg::Pose> calcStopPose(
+    const PathWithLaneId & ego_path, double dist_nearest_cp,
+    const std::optional<geometry_msgs::msg::Pose> & default_stop_pose_opt);
+
   std::optional<StopFactor> checkStopForCrosswalkUsers(
     const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path,
     const geometry_msgs::msg::Point & first_path_point_on_crosswalk,