From 81bb9d02d6bb5b30ab1df7c1f5acfcde1a795013 Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Fri, 6 Sep 2024 16:07:11 +0900
Subject: [PATCH 01/12] add optional handling

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 .../src/scene_crosswalk.cpp                   | 22 ++++++++++++-------
 .../src/scene_crosswalk.hpp                   |  2 +-
 2 files changed, 15 insertions(+), 9 deletions(-)

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
index 639ee214bca9c..75be29779745f 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -323,8 +323,10 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
   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);
+  const auto dist_ego_to_default_stop_opt =
+    default_stop_pose.has_value() ? std::optional<double>(calcSignedArcLength(
+                                      ego_path.points, ego_pos, default_stop_pose->position))
+                                  : std::nullopt;
 
   // Calculate attention range for crosswalk
   const auto crosswalk_attention_range = getAttentionRange(
@@ -335,7 +337,7 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
 
   // Update object state
   updateObjectState(
-    dist_ego_to_stop, sparse_resample_path, crosswalk_attention_range, attention_area);
+    dist_ego_to_default_stop_opt, sparse_resample_path, crosswalk_attention_range, attention_area);
 
   // Check if ego moves forward enough to ignore yield.
   const auto & p = planner_param_;
@@ -396,9 +398,10 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
   }
 
   // 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 bool stop_at_stop_line = dist_ego_to_default_stop_opt.has_value() &&
+                                 dist_ego_to_default_stop_opt.value() < nearest_stop_info->second &&
+                                 nearest_stop_info->second < dist_ego_to_default_stop_opt.value() +
+                                                               planner_param_.far_object_threshold;
 
   if (stop_at_stop_line) {
     // Stop at the stop line
@@ -1055,7 +1058,8 @@ std::optional<StopFactor> CrosswalkModule::getNearestStopFactor(
 }
 
 void CrosswalkModule::updateObjectState(
-  const double dist_ego_to_stop, const PathWithLaneId & sparse_resample_path,
+  const std::optional<double> dist_ego_to_default_stop_opt,
+  const PathWithLaneId & sparse_resample_path,
   const std::pair<double, double> & crosswalk_attention_range, const Polygon2d & attention_area)
 {
   const auto & objects_ptr = planner_data_->predicted_objects;
@@ -1066,7 +1070,9 @@ void CrosswalkModule::updateObjectState(
 
   // Check if ego is yielding
   const bool is_ego_yielding = [&]() {
-    const auto has_reached_stop_point = dist_ego_to_stop < planner_param_.stop_position_threshold;
+    const auto has_reached_stop_point =
+      dist_ego_to_default_stop_opt.has_value() &&
+      dist_ego_to_default_stop_opt.has_value() < planner_param_.stop_position_threshold;
 
     return planner_data_->isVehicleStopped(planner_param_.timeout_ego_stop_for_yield) &&
            has_reached_stop_point;
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
index be2a7d199c666..5dfca7e79b542 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
@@ -417,7 +417,7 @@ class CrosswalkModule : public SceneModuleInterface
     const std::pair<double, double> & crosswalk_attention_range) const;
 
   void updateObjectState(
-    const double dist_ego_to_stop, const PathWithLaneId & sparse_resample_path,
+    const std::optional<double> dist_ego_to_stop_opt, const PathWithLaneId & sparse_resample_path,
     const std::pair<double, double> & crosswalk_attention_range, const Polygon2d & attention_area);
 
   bool isRedSignalForPedestrians() const;

From 8d4663fda9fff07ae3f10a7f1d0a1b1aba524c98 Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Thu, 5 Sep 2024 21:05:16 +0900
Subject: [PATCH 02/12] add new stop pos

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 .../src/scene_crosswalk.cpp                   | 113 ++++++++++--------
 1 file changed, 66 insertions(+), 47 deletions(-)

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
index 75be29779745f..d1e04016a2ab7 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -343,14 +343,6 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
   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 {};
-  }
 
   // Check pedestrian for stop
   // NOTE: first stop point and its minimum distance from ego to stop
@@ -362,60 +354,87 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
   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) {
+    if (!object.collision_point || object.collision_state != CollisionState::YIELD) {
+      continue;
+    }
+    const auto & collision_point = object.collision_point.value();
+
+    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;
       }
-      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);
+    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);
-      }
+    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);
     }
   }
 
-  // Check if stop is required
   if (!nearest_stop_info) {
     return {};
   }
 
-  // Check if the ego should stop at the stop line or the other points.
-  const bool stop_at_stop_line = dist_ego_to_default_stop_opt.has_value() &&
-                                 dist_ego_to_default_stop_opt.value() < nearest_stop_info->second &&
-                                 nearest_stop_info->second < dist_ego_to_default_stop_opt.value() +
-                                                               planner_param_.far_object_threshold;
+  // decide stop position including cancel
+  const auto stop_pose_against_nearest_ped_opt = calcLongitudinalOffsetPose(
+    ego_path.points, nearest_stop_info->first,
+    -base_link2front - planner_param_.stop_distance_from_object);
+  if (!stop_pose_against_nearest_ped_opt) {
+    RCLCPP_DEBUG(logger_, "Failure to calculate stop_pose agaist a pedestrian with margin.");
+    return {};
+  }
+
+  const auto braking_dist_strong = [&]() {
+    const auto braking_dist_strong_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);
+    return braking_dist_strong_opt ? *braking_dist_strong_opt : 0.0;
+  }();
+  const auto braking_dist_weak = [&]() {
+    const auto braking_dist_weak_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);
+    return braking_dist_weak_opt ? *braking_dist_weak_opt : 0.0;
+  }();
 
-  if (stop_at_stop_line) {
-    // Stop at the stop line
-    if (default_stop_pose) {
-      return createStopFactor(*default_stop_pose, stop_factor_points);
+  const auto stop_pose_without_acc_constraint = [&]() {
+    if (
+      !default_stop_pose.has_value() ||
+      nearest_stop_info->second < dist_ego_to_default_stop_opt.value() ||
+      dist_ego_to_default_stop_opt.value() + planner_param_.far_object_threshold <
+        nearest_stop_info->second) {
+      return stop_pose_against_nearest_ped_opt.value();
+    } else {
+      return default_stop_pose.value();
     }
-  } 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);
+  }();
+
+  if (
+    braking_dist_weak <
+    calcSignedArcLength(ego_path.points, ego_pos, stop_pose_without_acc_constraint.position)) {
+    return createStopFactor(stop_pose_without_acc_constraint, stop_factor_points);
+  } else if (braking_dist_weak < nearest_stop_info->second) {
+    const auto stop_pose_with_weak_brake =
+      calcLongitudinalOffsetPose(ego_path.points, ego_pos, braking_dist_weak);
+    if (stop_pose_with_weak_brake.has_value()) {
+      return createStopFactor(stop_pose_with_weak_brake.value(), stop_factor_points);
     }
+  } else if (braking_dist_strong < nearest_stop_info->second) {
+    return createStopFactor(stop_pose_against_nearest_ped_opt.value(), stop_factor_points);
   }
+  // abort stop
   return {};
 }
 

From ed67cf3d11ecf665a7d30d2d6ddb956b396ff0c6 Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Fri, 6 Sep 2024 20:17:35 +0900
Subject: [PATCH 03/12] independ stop cancel

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 .../src/scene_crosswalk.cpp                   | 143 +++++++++++-------
 1 file changed, 88 insertions(+), 55 deletions(-)

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
index d1e04016a2ab7..a6caa938835f5 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -316,17 +316,28 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
   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)
 {
+  struct StopCandidate
+  {
+    geometry_msgs::msg::Pose pose;
+    double dist;
+  };
+
   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_default_stop_opt =
-    default_stop_pose.has_value() ? std::optional<double>(calcSignedArcLength(
-                                      ego_path.points, ego_pos, default_stop_pose->position))
-                                  : std::nullopt;
+
+  const auto default_stop_opt = [&]() -> std::optional<StopCandidate> {
+    if (!default_stop_pose_opt.has_value()) {
+      return std::nullopt;
+    }
+    const double dist =
+      calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose_opt->position);
+    return StopCandidate{default_stop_pose_opt.value(), dist};
+  }();
 
   // Calculate attention range for crosswalk
   const auto crosswalk_attention_range = getAttentionRange(
@@ -337,7 +348,8 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
 
   // Update object state
   updateObjectState(
-    dist_ego_to_default_stop_opt, sparse_resample_path, crosswalk_attention_range, attention_area);
+    default_stop_opt.has_value() ? std::optional<double>(default_stop_opt->dist) : std::nullopt,
+    sparse_resample_path, crosswalk_attention_range, attention_area);
 
   // Check if ego moves forward enough to ignore yield.
   const auto & p = planner_param_;
@@ -349,7 +361,7 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
   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);
@@ -373,69 +385,90 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
     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 (!nearest_stop_info) {
+  if (!dist_nearest_cp) {
     return {};
   }
 
-  // decide stop position including cancel
-  const auto stop_pose_against_nearest_ped_opt = calcLongitudinalOffsetPose(
-    ego_path.points, nearest_stop_info->first,
-    -base_link2front - planner_param_.stop_distance_from_object);
-  if (!stop_pose_against_nearest_ped_opt) {
-    RCLCPP_DEBUG(logger_, "Failure to calculate stop_pose agaist a pedestrian with margin.");
+  const auto ped_stop_limit = [&]() -> std::optional<StopCandidate> {
+    const double dist =
+      dist_nearest_cp.value() - base_link2front - planner_param_.stop_distance_from_object;
+    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()) {
     return {};
   }
 
-  const auto braking_dist_strong = [&]() {
-    const auto braking_dist_strong_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);
-    return braking_dist_strong_opt ? *braking_dist_strong_opt : 0.0;
-  }();
-  const auto braking_dist_weak = [&]() {
-    const auto braking_dist_weak_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);
-    return braking_dist_weak_opt ? *braking_dist_weak_opt : 0.0;
+  const auto without_acc_stop = [&]() -> std::optional<StopCandidate> {
+    const auto ped_stop_pref_opt = [&]() -> std::optional<StopCandidate> {
+      const double dist =
+        dist_nearest_cp.value() - base_link2front - planner_param_.stop_distance_from_object;
+      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_pref_opt.has_value()) {
+      // RCLCPP_DEBUG(logger_, "Failure to calculate stop_pose agaist a pedestrian with margin.");
+      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;
+    }
   }();
+  if (!without_acc_stop.has_value()) {
+    return {};
+  }
 
-  const auto stop_pose_without_acc_constraint = [&]() {
-    if (
-      !default_stop_pose.has_value() ||
-      nearest_stop_info->second < dist_ego_to_default_stop_opt.value() ||
-      dist_ego_to_default_stop_opt.value() + planner_param_.far_object_threshold <
-        nearest_stop_info->second) {
-      return stop_pose_against_nearest_ped_opt.value();
+  const auto weak_brk_stop = [&]() -> std::optional<StopCandidate> {
+    const auto dist_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);
+    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()) {
+    return {};
+  }
+ 
+  const auto decided_stop_opt = [&]() {
+    if (weak_brk_stop->dist < without_acc_stop->dist) {
+      return without_acc_stop;
+    } else if (weak_brk_stop->dist < ped_stop_limit->dist) {
+      return weak_brk_stop;
     } else {
-      return default_stop_pose.value();
+      return ped_stop_limit;
     }
   }();
 
-  if (
-    braking_dist_weak <
-    calcSignedArcLength(ego_path.points, ego_pos, stop_pose_without_acc_constraint.position)) {
-    return createStopFactor(stop_pose_without_acc_constraint, stop_factor_points);
-  } else if (braking_dist_weak < nearest_stop_info->second) {
-    const auto stop_pose_with_weak_brake =
-      calcLongitudinalOffsetPose(ego_path.points, ego_pos, braking_dist_weak);
-    if (stop_pose_with_weak_brake.has_value()) {
-      return createStopFactor(stop_pose_with_weak_brake.value(), stop_factor_points);
-    }
-  } else if (braking_dist_strong < nearest_stop_info->second) {
-    return createStopFactor(stop_pose_against_nearest_ped_opt.value(), stop_factor_points);
+  const double strong_brk_dist = [&]() {
+    const auto strong_brk_dist_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);
+    return strong_brk_dist_opt ? strong_brk_dist_opt.value() : 0.0;
+  }();
+  if (!decided_stop_opt.has_value() || decided_stop_opt->dist < strong_brk_dist) {
+    return {};
   }
-  // abort stop
-  return {};
+  createStopFactor(decided_stop_opt->pose, stop_factor_points);
 }
 
 std::pair<double, double> CrosswalkModule::getAttentionRange(

From de2a756381f4878e564b2baddf429a8bc40b08be Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Fri, 6 Sep 2024 20:22:46 +0900
Subject: [PATCH 04/12] fix compare boll

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 .../src/scene_crosswalk.cpp                                     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
index a6caa938835f5..4594dae334b3c 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -1124,7 +1124,7 @@ void CrosswalkModule::updateObjectState(
   const bool is_ego_yielding = [&]() {
     const auto has_reached_stop_point =
       dist_ego_to_default_stop_opt.has_value() &&
-      dist_ego_to_default_stop_opt.has_value() < planner_param_.stop_position_threshold;
+      dist_ego_to_default_stop_opt.value() < planner_param_.stop_position_threshold;
 
     return planner_data_->isVehicleStopped(planner_param_.timeout_ego_stop_for_yield) &&
            has_reached_stop_point;

From 8b780445d6720bc22bba667e6717f8a518af6414 Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Fri, 6 Sep 2024 21:30:37 +0900
Subject: [PATCH 05/12] refactor

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 .../src/scene_crosswalk.cpp                   | 97 ++++++++++---------
 .../src/scene_crosswalk.hpp                   |  6 +-
 2 files changed, 58 insertions(+), 45 deletions(-)

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
index 4594dae334b3c..698f06cd3fb15 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -318,26 +318,7 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
   const geometry_msgs::msg::Point & last_path_point_on_crosswalk,
   const std::optional<geometry_msgs::msg::Pose> & default_stop_pose_opt)
 {
-  struct StopCandidate
-  {
-    geometry_msgs::msg::Pose pose;
-    double dist;
-  };
-
   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 default_stop_opt = [&]() -> std::optional<StopCandidate> {
-    if (!default_stop_pose_opt.has_value()) {
-      return std::nullopt;
-    }
-    const double dist =
-      calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose_opt->position);
-    return StopCandidate{default_stop_pose_opt.value(), dist};
-  }();
 
   // Calculate attention range for crosswalk
   const auto crosswalk_attention_range = getAttentionRange(
@@ -347,14 +328,12 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
   const auto attention_area = getAttentionArea(sparse_resample_path, crosswalk_attention_range);
 
   // Update object state
+  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(
-    default_stop_opt.has_value() ? std::optional<double>(default_stop_opt->dist) : std::nullopt,
-    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);
+    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
@@ -394,9 +373,42 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
     return {};
   }
 
+  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 = planner_data_->current_velocity->twist.linear.x;
+  const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x;
+
+  const auto default_stop_opt = [&]() -> std::optional<StopCandidate> {
+    if (!default_stop_pose_opt.has_value()) {
+      return std::nullopt;
+    }
+    const double dist =
+      calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose_opt->position);
+    return StopCandidate{default_stop_pose_opt.value(), dist};
+  }();
+
   const auto ped_stop_limit = [&]() -> std::optional<StopCandidate> {
     const double dist =
-      dist_nearest_cp.value() - base_link2front - planner_param_.stop_distance_from_object;
+      dist_nearest_cp - base_link2front - planner_param_.stop_distance_from_object;
     const auto pose_opt = calcLongitudinalOffsetPose(ego_path.points, ego_pos, dist);
     if (!pose_opt.has_value()) {
       return std::nullopt;
@@ -404,13 +416,13 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
     return StopCandidate{pose_opt.value(), dist};
   }();
   if (!ped_stop_limit.has_value()) {
-    return {};
+    return std::nullopt;
   }
 
   const auto without_acc_stop = [&]() -> std::optional<StopCandidate> {
     const auto ped_stop_pref_opt = [&]() -> std::optional<StopCandidate> {
       const double dist =
-        dist_nearest_cp.value() - base_link2front - planner_param_.stop_distance_from_object;
+        dist_nearest_cp - base_link2front - planner_param_.stop_distance_from_object;
       const auto pose_opt = calcLongitudinalOffsetPose(ego_path.points, ego_pos, dist);
       if (!pose_opt.has_value()) {
         return std::nullopt;
@@ -429,7 +441,7 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
     }
   }();
   if (!without_acc_stop.has_value()) {
-    return {};
+    return std::nullopt;
   }
 
   const auto weak_brk_stop = [&]() -> std::optional<StopCandidate> {
@@ -446,16 +458,15 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
     return StopCandidate{pose_opt.value(), dist_opt.value()};
   }();
   if (!weak_brk_stop.has_value()) {
-    return {};
+    return std::nullopt;
   }
- 
-  const auto decided_stop_opt = [&]() {
+  const auto decided_stop_pos = [&]() {
     if (weak_brk_stop->dist < without_acc_stop->dist) {
-      return without_acc_stop;
+      return without_acc_stop.value();
     } else if (weak_brk_stop->dist < ped_stop_limit->dist) {
-      return weak_brk_stop;
+      return weak_brk_stop.value();
     } else {
-      return ped_stop_limit;
+      return ped_stop_limit.value();
     }
   }();
 
@@ -465,10 +476,11 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
       p.min_jerk_for_no_stop_decision);
     return strong_brk_dist_opt ? strong_brk_dist_opt.value() : 0.0;
   }();
-  if (!decided_stop_opt.has_value() || decided_stop_opt->dist < strong_brk_dist) {
-    return {};
+  if (decided_stop_pos.dist < strong_brk_dist) {
+    return std::nullopt;
   }
-  createStopFactor(decided_stop_opt->pose, stop_factor_points);
+
+  return std::make_optional(decided_stop_pos.pose);
 }
 
 std::pair<double, double> CrosswalkModule::getAttentionRange(
@@ -1110,8 +1122,7 @@ std::optional<StopFactor> CrosswalkModule::getNearestStopFactor(
 }
 
 void CrosswalkModule::updateObjectState(
-  const std::optional<double> dist_ego_to_default_stop_opt,
-  const PathWithLaneId & sparse_resample_path,
+  const double dist_ego_to_stop, const PathWithLaneId & sparse_resample_path,
   const std::pair<double, double> & crosswalk_attention_range, const Polygon2d & attention_area)
 {
   const auto & objects_ptr = planner_data_->predicted_objects;
@@ -1122,9 +1133,7 @@ void CrosswalkModule::updateObjectState(
 
   // Check if ego is yielding
   const bool is_ego_yielding = [&]() {
-    const auto has_reached_stop_point =
-      dist_ego_to_default_stop_opt.has_value() &&
-      dist_ego_to_default_stop_opt.value() < planner_param_.stop_position_threshold;
+    const auto has_reached_stop_point = dist_ego_to_stop < planner_param_.stop_position_threshold;
 
     return planner_data_->isVehicleStopped(planner_param_.timeout_ego_stop_for_yield) &&
            has_reached_stop_point;
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
index 5dfca7e79b542..b5668a6c8d2d7 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
@@ -352,6 +352,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,
@@ -417,7 +421,7 @@ class CrosswalkModule : public SceneModuleInterface
     const std::pair<double, double> & crosswalk_attention_range) const;
 
   void updateObjectState(
-    const std::optional<double> dist_ego_to_stop_opt, const PathWithLaneId & sparse_resample_path,
+    const double dist_ego_to_stop, const PathWithLaneId & sparse_resample_path,
     const std::pair<double, double> & crosswalk_attention_range, const Polygon2d & attention_area);
 
   bool isRedSignalForPedestrians() const;

From ae270bb4e05d2980b4bd6e485f610391d543c56a Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Mon, 9 Sep 2024 18:37:56 +0900
Subject: [PATCH 06/12] add params

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 .../src/manager.cpp                           |  7 ++
 .../src/scene_crosswalk.cpp                   | 64 ++++++++-----------
 .../src/scene_crosswalk.hpp                   |  4 ++
 3 files changed, 37 insertions(+), 38 deletions(-)

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp
index 0bd708f7b1705..7c7ada7caa28d 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp
@@ -49,10 +49,17 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
     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_preferd =
+    getOrDeclareParameter<double>(node, ns + ".stop_position.stop_distance_from_object_preferd");
+  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_prefered = getOrDeclareParameter<double>(node, ns + ".stop_position.min_acc_preferd");
+  cp.min_jerk_prefered =
+    getOrDeclareParameter<double>(node, ns + ".stop_position.min_jerk_preferd");
 
   // param for restart suppression
   cp.min_dist_to_stop_for_restart_suppression =
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
index 698f06cd3fb15..9e4db5a0635fe 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -398,37 +398,29 @@ std::optional<geometry_msgs::msg::Pose> CrosswalkModule::calcStopPose(
   const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x;
 
   const auto default_stop_opt = [&]() -> std::optional<StopCandidate> {
-    if (!default_stop_pose_opt.has_value()) {
-      return std::nullopt;
-    }
+    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 =
-      calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose_opt->position);
-    return StopCandidate{default_stop_pose_opt.value(), dist};
+      dist_nearest_cp - base_link2front - planner_param_.stop_distance_from_object_preferd;
+    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 ped_stop_limit = [&]() -> std::optional<StopCandidate> {
+  const auto ped_stop_limit_opt = [&]() -> std::optional<StopCandidate> {
     const double dist =
-      dist_nearest_cp - base_link2front - planner_param_.stop_distance_from_object;
+      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;
-    }
+    if (!pose_opt.has_value()) return std::nullopt;
     return StopCandidate{pose_opt.value(), dist};
   }();
-  if (!ped_stop_limit.has_value()) {
-    return std::nullopt;
-  }
 
   const auto without_acc_stop = [&]() -> std::optional<StopCandidate> {
-    const auto ped_stop_pref_opt = [&]() -> std::optional<StopCandidate> {
-      const double dist =
-        dist_nearest_cp - base_link2front - planner_param_.stop_distance_from_object;
-      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_pref_opt.has_value()) {
       // RCLCPP_DEBUG(logger_, "Failure to calculate stop_pose agaist a pedestrian with margin.");
       return std::nullopt;
@@ -440,33 +432,28 @@ std::optional<geometry_msgs::msg::Pose> CrosswalkModule::calcStopPose(
       return ped_stop_pref_opt;
     }
   }();
-  if (!without_acc_stop.has_value()) {
-    return std::nullopt;
-  }
+  if (!without_acc_stop.has_value()) return std::nullopt;
 
   const auto weak_brk_stop = [&]() -> std::optional<StopCandidate> {
     const auto dist_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);
-    if (!dist_opt.has_value()) {
-      return std::nullopt;
-    }
+      ego_vel, 0.0, ego_acc, p.min_acc_prefered, 10.0, p.min_jerk_prefered);
+    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;
-    }
+    if (!pose_opt.has_value()) return std::nullopt;
     return StopCandidate{pose_opt.value(), dist_opt.value()};
   }();
   if (!weak_brk_stop.has_value()) {
+    // RCLCPP_DEBUG(logger_, "Failure to calculate stop_pose agaist a pedestrian with margin.");
     return std::nullopt;
   }
-  const auto decided_stop_pos = [&]() {
+
+  const auto selected_stop = [&]() {
     if (weak_brk_stop->dist < without_acc_stop->dist) {
       return without_acc_stop.value();
-    } else if (weak_brk_stop->dist < ped_stop_limit->dist) {
+    } else if (weak_brk_stop->dist < ped_stop_limit_opt->dist) {
       return weak_brk_stop.value();
     } else {
-      return ped_stop_limit.value();
+      return ped_stop_limit_opt.value();
     }
   }();
 
@@ -476,11 +463,12 @@ std::optional<geometry_msgs::msg::Pose> CrosswalkModule::calcStopPose(
       p.min_jerk_for_no_stop_decision);
     return strong_brk_dist_opt ? strong_brk_dist_opt.value() : 0.0;
   }();
-  if (decided_stop_pos.dist < strong_brk_dist) {
+  if (selected_stop.dist < strong_brk_dist) {
+    // RCLCPP_DEBUG(logger_, "Failure to calculate stop_pose agaist a pedestrian with margin.");
     return std::nullopt;
   }
 
-  return std::make_optional(decided_stop_pos.pose);
+  return std::make_optional(selected_stop.pose);
 }
 
 std::pair<double, double> CrosswalkModule::getAttentionRange(
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
index b5668a6c8d2d7..e2908d7189604 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
@@ -113,9 +113,13 @@ class CrosswalkModule : public SceneModuleInterface
     bool show_processing_time;
     // param for stop position
     double stop_distance_from_object;
+    double stop_distance_from_object_preferd;
+    double stop_distance_from_object_limit;
     double stop_distance_from_crosswalk;
     double far_object_threshold;
     double stop_position_threshold;
+    double min_acc_prefered;
+    double min_jerk_prefered;
     // param for restart suppression
     double min_dist_to_stop_for_restart_suppression;
     double max_dist_to_stop_for_restart_suppression;

From 9aab3600e3e703bef556c96148e20d77ef8a2d12 Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Mon, 9 Sep 2024 23:00:44 +0900
Subject: [PATCH 07/12] change logic

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 .../src/manager.cpp                           |  6 +-
 .../src/scene_crosswalk.cpp                   | 72 +++++++++++++------
 2 files changed, 56 insertions(+), 22 deletions(-)

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp
index 7c7ada7caa28d..d9a50b65fce76 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp
@@ -24,6 +24,8 @@
 #include <string>
 #include <vector>
 
+
+
 namespace autoware::behavior_velocity_planner
 {
 
@@ -57,9 +59,9 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
     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_prefered = getOrDeclareParameter<double>(node, ns + ".stop_position.min_acc_preferd");
+  cp.min_acc_prefered = getOrDeclareParameter<double>(node, ns + ".stop_position.min_acc_prefered");
   cp.min_jerk_prefered =
-    getOrDeclareParameter<double>(node, ns + ".stop_position.min_jerk_preferd");
+    getOrDeclareParameter<double>(node, ns + ".stop_position.min_jerk_prefered");
 
   // param for restart suppression
   cp.min_dist_to_stop_for_restart_suppression =
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
index 9e4db5a0635fe..5bac1dae5e4b3 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -38,6 +38,36 @@
 #include <tuple>
 #include <vector>
 
+#define debug(var)                                                      \
+  do {                                                                  \
+    std::cerr << __func__ << ": " << __LINE__ << ", " << #var << " : "; \
+    view(var);                                                          \
+  } while (0)
+template <typename T>
+void view(T e)
+{
+  std::cerr << e << std::endl;
+}
+template <typename T>
+void view(const std::vector<T> & v)
+{
+  for (const auto & e : v) {
+    std::cerr << e << " ";
+  }
+  std::cerr << std::endl;
+}
+template <typename T>
+void view(const std::vector<std::vector<T>> & vv)
+{
+  for (const auto & v : vv) {
+    view(v);
+  }
+}
+#define line()                                                                         \
+  {                                                                                    \
+    std::cerr << "(" << __FILE__ << ") " << __func__ << ": " << __LINE__ << std::endl; \
+  }
+
 namespace autoware::behavior_velocity_planner
 {
 namespace bg = boost::geometry;
@@ -412,18 +442,11 @@ std::optional<geometry_msgs::msg::Pose> CrosswalkModule::calcStopPose(
     return StopCandidate{pose_opt.value(), dist};
   }();
 
-  const auto ped_stop_limit_opt = [&]() -> 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};
-  }();
-
-  const auto without_acc_stop = [&]() -> std::optional<StopCandidate> {
+  const auto without_acc_stop_opt = [&]() -> std::optional<StopCandidate> {
     if (!ped_stop_pref_opt.has_value()) {
-      // RCLCPP_DEBUG(logger_, "Failure to calculate stop_pose agaist a pedestrian with margin.");
       return std::nullopt;
+      // RCLCPP_DEBUG(logger_, "Failure to calculate stop_pose agaist a pedestrian with margin.");
+      // return weak_brk_stop; だめ limitへのアプローチからオーバーランする
     } 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) {
@@ -432,28 +455,37 @@ std::optional<geometry_msgs::msg::Pose> CrosswalkModule::calcStopPose(
       return ped_stop_pref_opt;
     }
   }();
-  if (!without_acc_stop.has_value()) return std::nullopt;
+  // if (!without_acc_stop_opt.has_value()) return std::nullopt;
+
+  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()) return std::nullopt;
 
   const auto weak_brk_stop = [&]() -> std::optional<StopCandidate> {
     const auto dist_opt = autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints(
-      ego_vel, 0.0, ego_acc, p.min_acc_prefered, 10.0, p.min_jerk_prefered);
+      ego_vel, 0.0, ego_acc, p.min_acc_prefered, 1.0, p.min_jerk_prefered);
     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_DEBUG(logger_, "Failure to calculate stop_pose agaist a pedestrian with margin.");
-    return std::nullopt;
-  }
+  if (!weak_brk_stop.has_value()) return std::nullopt;
 
   const auto selected_stop = [&]() {
-    if (weak_brk_stop->dist < without_acc_stop->dist) {
-      return without_acc_stop.value();
-    } else if (weak_brk_stop->dist < ped_stop_limit_opt->dist) {
+    // debug(weak_brk_stop->dist);
+    // debug(without_acc_stop_opt->dist);
+    // debug(ped_stop_limit->dist);
+    if (without_acc_stop_opt.has_value() && weak_brk_stop->dist < without_acc_stop_opt->dist) {
+      return without_acc_stop_opt.value();
+    } else if (weak_brk_stop->dist < ped_stop_limit->dist) {
       return weak_brk_stop.value();
     } else {
-      return ped_stop_limit_opt.value();
+      return ped_stop_limit.value();
     }
   }();
 

From e5717f45f4db9df05aa1eb876cd0ae3ab6e76c28 Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Tue, 10 Sep 2024 18:11:25 +0900
Subject: [PATCH 08/12] add additiona param

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 .../src/manager.cpp                           |  2 +
 .../src/scene_crosswalk.cpp                   | 69 +++++++------------
 .../src/scene_crosswalk.hpp                   |  1 +
 3 files changed, 28 insertions(+), 44 deletions(-)

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp
index d9a50b65fce76..585f99c487500 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp
@@ -115,6 +115,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
     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.offset_length_for_no_stop_decision =
+    getOrDeclareParameter<double>(node, ns + ".pass_judge.no_stop_decision.offset_length");
   cp.stop_object_velocity =
     getOrDeclareParameter<double>(node, ns + ".pass_judge.stop_object_velocity_threshold");
   cp.min_object_velocity =
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
index 5bac1dae5e4b3..2ff184e84a590 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -38,36 +38,6 @@
 #include <tuple>
 #include <vector>
 
-#define debug(var)                                                      \
-  do {                                                                  \
-    std::cerr << __func__ << ": " << __LINE__ << ", " << #var << " : "; \
-    view(var);                                                          \
-  } while (0)
-template <typename T>
-void view(T e)
-{
-  std::cerr << e << std::endl;
-}
-template <typename T>
-void view(const std::vector<T> & v)
-{
-  for (const auto & e : v) {
-    std::cerr << e << " ";
-  }
-  std::cerr << std::endl;
-}
-template <typename T>
-void view(const std::vector<std::vector<T>> & vv)
-{
-  for (const auto & v : vv) {
-    view(v);
-  }
-}
-#define line()                                                                         \
-  {                                                                                    \
-    std::cerr << "(" << __FILE__ << ") " << __func__ << ": " << __LINE__ << std::endl; \
-  }
-
 namespace autoware::behavior_velocity_planner
 {
 namespace bg = boost::geometry;
@@ -424,7 +394,7 @@ std::optional<geometry_msgs::msg::Pose> CrosswalkModule::calcStopPose(
   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 = planner_data_->current_velocity->twist.linear.x;
+  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;
 
   const auto default_stop_opt = [&]() -> std::optional<StopCandidate> {
@@ -444,9 +414,10 @@ std::optional<geometry_msgs::msg::Pose> CrosswalkModule::calcStopPose(
 
   const auto without_acc_stop_opt = [&]() -> std::optional<StopCandidate> {
     if (!ped_stop_pref_opt.has_value()) {
+      RCLCPP_WARN(
+        logger_,
+        "Failure to calculate stop_pose agaist the nearest pedestrian with a prefered margin.");
       return std::nullopt;
-      // RCLCPP_DEBUG(logger_, "Failure to calculate stop_pose agaist a pedestrian with margin.");
-      // return weak_brk_stop; だめ limitへのアプローチからオーバーランする
     } 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) {
@@ -455,7 +426,6 @@ std::optional<geometry_msgs::msg::Pose> CrosswalkModule::calcStopPose(
       return ped_stop_pref_opt;
     }
   }();
-  // if (!without_acc_stop_opt.has_value()) return std::nullopt;
 
   const auto ped_stop_limit = [&]() -> std::optional<StopCandidate> {
     const double dist =
@@ -464,22 +434,28 @@ std::optional<geometry_msgs::msg::Pose> CrosswalkModule::calcStopPose(
     if (!pose_opt.has_value()) return std::nullopt;
     return StopCandidate{pose_opt.value(), dist};
   }();
-  if (!ped_stop_limit.has_value()) return std::nullopt;
+  if (!ped_stop_limit.has_value()) {
+    RCLCPP_WARN(
+      logger_,
+      "Failure to calculate stop_pose agaist the nearest pedestrian with a limit margin. "
+      "Stop will be canceled.");
+    return std::nullopt;
+  }
 
   const auto weak_brk_stop = [&]() -> std::optional<StopCandidate> {
     const auto dist_opt = autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints(
-      ego_vel, 0.0, ego_acc, p.min_acc_prefered, 1.0, p.min_jerk_prefered);
+      ego_vel_non_nega, 0.0, ego_acc, p.min_acc_prefered, 10.0, p.min_jerk_prefered);
     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()) return std::nullopt;
+  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 = [&]() {
-    // debug(weak_brk_stop->dist);
-    // debug(without_acc_stop_opt->dist);
-    // debug(ped_stop_limit->dist);
     if (without_acc_stop_opt.has_value() && weak_brk_stop->dist < without_acc_stop_opt->dist) {
       return without_acc_stop_opt.value();
     } else if (weak_brk_stop->dist < ped_stop_limit->dist) {
@@ -491,12 +467,17 @@ std::optional<geometry_msgs::msg::Pose> CrosswalkModule::calcStopPose(
 
   const double strong_brk_dist = [&]() {
     const auto strong_brk_dist_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);
+      ego_vel_non_nega, 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);
     return strong_brk_dist_opt ? strong_brk_dist_opt.value() : 0.0;
   }();
-  if (selected_stop.dist < strong_brk_dist) {
-    // RCLCPP_DEBUG(logger_, "Failure to calculate stop_pose agaist a pedestrian with margin.");
+  if (selected_stop.dist < strong_brk_dist + p.offset_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;
   }
 
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
index e2908d7189604..d6bbdc047ff84 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
@@ -148,6 +148,7 @@ class CrosswalkModule : public SceneModuleInterface
     double min_acc_for_no_stop_decision;
     double max_jerk_for_no_stop_decision;
     double min_jerk_for_no_stop_decision;
+    double offset_length_for_no_stop_decision;
     double stop_object_velocity;
     double min_object_velocity;
     bool disable_yield_for_new_stopped_object;

From 09f6657e358dd31330ee82a0cc0dd2850f2a4792 Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Wed, 11 Sep 2024 17:37:17 +0900
Subject: [PATCH 09/12] change car names

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 .../src/manager.cpp                           | 17 +++--
 .../src/scene_crosswalk.cpp                   | 67 +++++++++++--------
 .../src/scene_crosswalk.hpp                   |  8 +--
 3 files changed, 50 insertions(+), 42 deletions(-)

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp
index 585f99c487500..e4ecd280dfcde 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp
@@ -24,8 +24,6 @@
 #include <string>
 #include <vector>
 
-
-
 namespace autoware::behavior_velocity_planner
 {
 
@@ -51,17 +49,18 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
     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_preferd =
-    getOrDeclareParameter<double>(node, ns + ".stop_position.stop_distance_from_object_preferd");
+  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_prefered = getOrDeclareParameter<double>(node, ns + ".stop_position.min_acc_prefered");
-  cp.min_jerk_prefered =
-    getOrDeclareParameter<double>(node, ns + ".stop_position.min_jerk_prefered");
+  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 =
@@ -115,8 +114,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
     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.offset_length_for_no_stop_decision =
-    getOrDeclareParameter<double>(node, ns + ".pass_judge.no_stop_decision.offset_length");
+  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 =
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
index 2ff184e84a590..574ff9c6e7651 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -328,6 +328,7 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
   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)
@@ -345,28 +346,36 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
   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 (!object.collision_point || object.collision_state != CollisionState::YIELD) {
-      continue;
-    }
-    const auto & collision_point = object.collision_point.value();
-
-    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) {
+      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;
       }
-    }
 
-    stop_factor_points.push_back(object.position);
+      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);
-    if (!dist_nearest_cp || dist_ego2cp < dist_nearest_cp) {
-      dist_nearest_cp = dist_ego2cp;
+      const auto dist_ego2cp =
+        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) {
@@ -406,17 +415,15 @@ std::optional<geometry_msgs::msg::Pose> CrosswalkModule::calcStopPose(
 
   const auto ped_stop_pref_opt = [&]() -> std::optional<StopCandidate> {
     const double dist =
-      dist_nearest_cp - base_link2front - planner_param_.stop_distance_from_object_preferd;
+      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_stop_opt = [&]() -> std::optional<StopCandidate> {
+  const auto without_acc_pref_stop_opt = [&]() -> std::optional<StopCandidate> {
     if (!ped_stop_pref_opt.has_value()) {
-      RCLCPP_WARN(
-        logger_,
-        "Failure to calculate stop_pose agaist the nearest pedestrian with a prefered margin.");
+      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 &&
@@ -437,14 +444,14 @@ std::optional<geometry_msgs::msg::Pose> CrosswalkModule::calcStopPose(
   if (!ped_stop_limit.has_value()) {
     RCLCPP_WARN(
       logger_,
-      "Failure to calculate stop_pose agaist the nearest pedestrian with a limit margin. "
-      "Stop will be canceled.");
+      "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_prefered, 10.0, p.min_jerk_prefered);
+      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;
@@ -456,8 +463,10 @@ std::optional<geometry_msgs::msg::Pose> CrosswalkModule::calcStopPose(
   }
 
   const auto selected_stop = [&]() {
-    if (without_acc_stop_opt.has_value() && weak_brk_stop->dist < without_acc_stop_opt->dist) {
-      return without_acc_stop_opt.value();
+    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 {
@@ -468,10 +477,10 @@ std::optional<geometry_msgs::msg::Pose> CrosswalkModule::calcStopPose(
   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,
-      p.max_jerk_for_no_stop_decision, p.min_jerk_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.offset_length_for_no_stop_decision) {
+  if (selected_stop.dist < strong_brk_dist + p.overrun_threshold_length_for_no_stop_decision) {
     RCLCPP_INFO(
       logger_,
       "Abandon to stop. "
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
index d6bbdc047ff84..0af072ba23b3c 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
@@ -113,13 +113,13 @@ class CrosswalkModule : public SceneModuleInterface
     bool show_processing_time;
     // param for stop position
     double stop_distance_from_object;
-    double stop_distance_from_object_preferd;
+    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_prefered;
-    double min_jerk_prefered;
+    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;
@@ -148,7 +148,7 @@ class CrosswalkModule : public SceneModuleInterface
     double min_acc_for_no_stop_decision;
     double max_jerk_for_no_stop_decision;
     double min_jerk_for_no_stop_decision;
-    double offset_length_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;

From 89a9d992a01fe4e0ad868ad937be86eb14408817 Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Wed, 11 Sep 2024 17:38:12 +0900
Subject: [PATCH 10/12] follow param def change

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 .../src/scene_crosswalk.cpp                                     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
index 574ff9c6e7651..9044a010ffff4 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -480,7 +480,7 @@ std::optional<geometry_msgs::msg::Pose> CrosswalkModule::calcStopPose(
       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) {
+  if (selected_stop.dist < strong_brk_dist - p.overrun_threshold_length_for_no_stop_decision) {
     RCLCPP_INFO(
       logger_,
       "Abandon to stop. "

From 776b5090d7436e0460d8e8966aade6cea18a1ea3 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Wed, 11 Sep 2024 08:44:56 +0000
Subject: [PATCH 11/12] style(pre-commit): autofix

---
 .../src/scene_crosswalk.cpp                                   | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
index 9044a010ffff4..c56ab1bd9fb70 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -476,8 +476,8 @@ std::optional<geometry_msgs::msg::Pose> CrosswalkModule::calcStopPose(
 
   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);
+      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) {

From dddb834d1e022f02590b6ffbaef9d489222baf1a Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Wed, 11 Sep 2024 17:48:21 +0900
Subject: [PATCH 12/12] delete unused params

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 .../src/manager.cpp                                         | 6 ------
 .../src/scene_crosswalk.hpp                                 | 3 ---
 2 files changed, 9 deletions(-)

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp
index e4ecd280dfcde..949a8de361409 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp
@@ -47,8 +47,6 @@ 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 =
@@ -106,12 +104,8 @@ 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>(
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
index 0af072ba23b3c..cd4b8eb46af6c 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
@@ -112,7 +112,6 @@ 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;
@@ -144,9 +143,7 @@ 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;