change logic

Signed-off-by: Yuki Takagi <[email protected]>
tier4 · Sep 9, 2024 · 8a111d3 · 8a111d3
1 parent 6be818a
commit 8a111d3
Show file tree

Hide file tree

Showing 2 changed files with 56 additions and 22 deletions.
diff --git a/...ing/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/...ing/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 ego velocity
   cp.min_slow_down_velocity =

diff --git a/...vior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/...vior_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;
@@ -410,18 +440,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) {
@@ -430,28 +453,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();
     }
   }();