feat(crosswalk): add new stop position

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

@@ -316,15 +316,9 @@
   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(
@@ -334,39 +328,36 @@
   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();
+    if (!object.collision_point || object.collision_state != CollisionState::YIELD) {
+      if (collision_point_opt) {
+        continue;
+        const auto & collision_point = collision_point_opt.value();
+      }
       const auto & collision_state = object.collision_state;
+      const auto & collision_point = object.collision_point.value();
       if (collision_state != CollisionState::YIELD) {
         continue;
       }
+
       if (
         isVehicleType(object.classification) && ego_crosswalk_passage_direction &&
         collision_point.crosswalk_passage_direction) {
@@ -381,39 +372,125 @@
       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);
+}
+
+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_nega = std::max(0.0, planner_data_->current_velocity->twist.linear.x);
+  const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x;
 
-  // 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;
+  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};
+  }();
 
-  if (stop_at_stop_line) {
-    // Stop at the stop line
-    if (default_stop_pose) {
-      return createStopFactor(*default_stop_pose, stop_factor_points);
+  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 agaist 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_nega, 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_nega, 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,