add params

Signed-off-by: Yuki Takagi <[email protected]>

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 ego velocity
   cp.min_slow_down_velocity =

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -396,37 +396,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;
@@ -438,33 +430,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();
     }
   }();
 
@@ -474,11 +461,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(

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -112,9 +112,14 @@ 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 ego velocity
     float min_slow_down_velocity;
     double max_slow_down_jerk;