feat: crosswalk keep stopping v0.19.1.1 (#1530)

* feat(crosswalk): suppress restart when the ego is close to the next stop point

Signed-off-by: Takayuki Murooka <[email protected]>

* update

Signed-off-by: Takayuki Murooka <[email protected]>

* add comment

Signed-off-by: Takayuki Murooka <[email protected]>

* fix namespace issue

* fix namespace issue

---------

Signed-off-by: Takayuki Murooka <[email protected]>
Co-authored-by: Shohei Sakai <[email protected]>

planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml

@@ -60,6 +60,11 @@
         timeout_map_for_no_intention_to_walk: [3.0, 0.0] # [s]
         timeout_ego_stop_for_yield: 3.0 # [s] If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough.
 
+      # params for suppressing the ego to restart when the ego is close to the next stop position.
+      restart_suppression:
+        min_distance_to_stop: 0.5
+        max_distance_to_stop: 1.0
+
       # param for target object filtering
       object_filtering:
         crosswalk_attention_range: 1.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk

planning/behavior_velocity_crosswalk_module/src/manager.cpp

@@ -54,6 +54,12 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
   cp.stop_position_threshold =
     getOrDeclareParameter<double>(node, ns + ".stop_position.stop_position_threshold");
 
+  // param for restart suppression
+  cp.min_dist_to_stop_for_restart_suppression =
+    getOrDeclareParameter<double>(node, ns + ".restart_suppression.min_distance_to_stop");
+  cp.max_dist_to_stop_for_restart_suppression =
+    getOrDeclareParameter<double>(node, ns + ".restart_suppression.max_distance_to_stop");
+
   // param for ego velocity
   cp.min_slow_down_velocity =
     getOrDeclareParameter<double>(node, ns + ".slow_down.min_slow_down_velocity");

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -197,6 +197,8 @@ CrosswalkModule::CrosswalkModule(
 
   collision_info_pub_ =
     node.create_publisher<tier4_debug_msgs::msg::StringStamped>("~/debug/collision_info", 1);
+
+  vehicle_stop_checker_ = std::make_unique<motion_utils::VehicleStopChecker>(&node);
 }
 
 bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
@@ -1188,7 +1190,8 @@ void CrosswalkModule::planStop(
   PathWithLaneId & ego_path, const std::optional<StopFactor> & nearest_stop_factor,
   const std::optional<geometry_msgs::msg::Pose> & default_stop_pose, StopReason * stop_reason)
 {
-  const auto stop_factor = [&]() -> std::optional<StopFactor> {
+  // Calculate stop factor
+  auto stop_factor = [&]() -> std::optional<StopFactor> {
     if (nearest_stop_factor) return *nearest_stop_factor;
     if (default_stop_pose) return createStopFactor(*default_stop_pose);
     return std::nullopt;
@@ -1199,11 +1202,36 @@ void CrosswalkModule::planStop(
     return;
   }
 
+  // Check if the restart should be suppressed.
+  const bool suppress_restart = checkRestartSuppression(ego_path, stop_factor);
+  if (suppress_restart) {
+    const auto & ego_pose = planner_data_->current_odometry->pose;
+    stop_factor->stop_pose = ego_pose;
+  }
+
   // Plan stop
   insertDecelPointWithDebugInfo(stop_factor->stop_pose.position, 0.0, ego_path);
   planning_utils::appendStopReason(*stop_factor, stop_reason);
   velocity_factor_.set(
     ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose,
     VelocityFactor::UNKNOWN);
 }
+
+bool CrosswalkModule::checkRestartSuppression(
+  const PathWithLaneId & ego_path, const std::optional<StopFactor> & stop_factor) const
+{
+  const auto is_vehicle_stopped = vehicle_stop_checker_->isVehicleStopped();
+  if (!is_vehicle_stopped) {
+    return false;
+  }
+
+  const auto & ego_pos = planner_data_->current_odometry->pose.position;
+  const double dist_to_stop =
+    calcSignedArcLength(ego_path.points, ego_pos, stop_factor->stop_pose.position);
+
+  // NOTE: min_dist_to_stop_for_restart_suppression is supposed to be the same as
+  //      the pid_longitudinal_controller's drive_state_stop_dist.
+  return planner_param_.min_dist_to_stop_for_restart_suppression < dist_to_stop &&
+         dist_to_stop < planner_param_.max_dist_to_stop_for_restart_suppression;
+}
 }  // namespace behavior_velocity_planner

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -19,6 +19,7 @@
 
 #include <behavior_velocity_planner_common/scene_module_interface.hpp>
 #include <lanelet2_extension/regulatory_elements/crosswalk.hpp>
+#include <motion_utils/vehicle/vehicle_state_checker.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/geometry/boost_geometry.hpp>
 #include <tier4_autoware_utils/system/stop_watch.hpp>
@@ -115,6 +116,9 @@ class CrosswalkModule : public SceneModuleInterface
     double stop_distance_from_crosswalk;
     double far_object_threshold;
     double stop_position_threshold;
+    // param for restart suppression
+    double min_dist_to_stop_for_restart_suppression;
+    double max_dist_to_stop_for_restart_suppression;
     // param for ego velocity
     float min_slow_down_velocity;
     double max_slow_down_jerk;
@@ -404,6 +408,9 @@ class CrosswalkModule : public SceneModuleInterface
   static geometry_msgs::msg::Polygon createVehiclePolygon(
     const vehicle_info_util::VehicleInfo & vehicle_info);
 
+  bool checkRestartSuppression(
+    const PathWithLaneId & ego_path, const std::optional<StopFactor> & stop_factor) const;
+
   void recordTime(const int step_num)
   {
     RCLCPP_INFO_EXPRESSION(
@@ -429,6 +436,8 @@ class CrosswalkModule : public SceneModuleInterface
   // Debug
   mutable DebugData debug_data_;
 
+  std::unique_ptr<motion_utils::VehicleStopChecker> vehicle_stop_checker_{nullptr};
+
   // Stop watch
   StopWatch<std::chrono::milliseconds> stop_watch_;