From f874652d19af85cde790139970a53c3bdec6ae42 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 13 Sep 2024 18:28:49 +0900 Subject: [PATCH] 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 * update Signed-off-by: Takayuki Murooka * add comment Signed-off-by: Takayuki Murooka * fix namespace issue * fix namespace issue --------- Signed-off-by: Takayuki Murooka Co-authored-by: Shohei Sakai --- .../config/crosswalk.param.yaml | 5 ++++ .../src/manager.cpp | 6 ++++ .../src/scene_crosswalk.cpp | 30 ++++++++++++++++++- .../src/scene_crosswalk.hpp | 9 ++++++ 4 files changed, 49 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 240963309e9a2..c99b3d6d2fa10 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/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 diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index b8190cb6124e7..c38a542e18313 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -54,6 +54,12 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) cp.stop_position_threshold = getOrDeclareParameter(node, ns + ".stop_position.stop_position_threshold"); + // param for restart suppression + cp.min_dist_to_stop_for_restart_suppression = + getOrDeclareParameter(node, ns + ".restart_suppression.min_distance_to_stop"); + cp.max_dist_to_stop_for_restart_suppression = + getOrDeclareParameter(node, ns + ".restart_suppression.max_distance_to_stop"); + // param for ego velocity cp.min_slow_down_velocity = getOrDeclareParameter(node, ns + ".slow_down.min_slow_down_velocity"); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 3549232bdbed0..c2893ce7d1f92 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -197,6 +197,8 @@ CrosswalkModule::CrosswalkModule( collision_info_pub_ = node.create_publisher("~/debug/collision_info", 1); + + vehicle_stop_checker_ = std::make_unique(&node); } bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) @@ -1188,7 +1190,8 @@ void CrosswalkModule::planStop( PathWithLaneId & ego_path, const std::optional & nearest_stop_factor, const std::optional & default_stop_pose, StopReason * stop_reason) { - const auto stop_factor = [&]() -> std::optional { + // Calculate stop factor + auto stop_factor = [&]() -> std::optional { if (nearest_stop_factor) return *nearest_stop_factor; if (default_stop_pose) return createStopFactor(*default_stop_pose); return std::nullopt; @@ -1199,6 +1202,13 @@ 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); @@ -1206,4 +1216,22 @@ void CrosswalkModule::planStop( ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose, VelocityFactor::UNKNOWN); } + +bool CrosswalkModule::checkRestartSuppression( + const PathWithLaneId & ego_path, const std::optional & 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 diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 7f020fbe5422c..77b422f1bd5da 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -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 & 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 vehicle_stop_checker_{nullptr}; + // Stop watch StopWatch stop_watch_;