From b9abed6cbc8ae760dafcc91f0f2572a4de90d4ee Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 10 Sep 2024 11:39:09 +0900 Subject: [PATCH 1/5] feat(crosswalk): suppress restart when the ego is close to the next stop point Signed-off-by: Takayuki Murooka --- .../config/crosswalk.param.yaml | 4 +++ .../src/manager.cpp | 4 +++ .../src/scene_crosswalk.cpp | 25 ++++++++++++++++++- .../src/scene_crosswalk.hpp | 7 ++++++ 4 files changed, 39 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..337ce859d2b59 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -60,6 +60,10 @@ 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: + 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..cc0f5b4111608 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -54,6 +54,10 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) cp.stop_position_threshold = getOrDeclareParameter(node, ns + ".stop_position.stop_position_threshold"); + // param for restart suppression + cp.dist_to_stop_for_restart_suppression = + getOrDeclareParameter(node, ns + ".restart_suppression.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..b3ee553f12fc4 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(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,17 @@ void CrosswalkModule::planStop( ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose, VelocityFactor::UNKNOWN); } + +bool CrosswalkModule::checkRestartSuppression(const std::optional & stop_factor) +{ + 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 = + autoware::universe_utils::calcDistance2d(ego_pos, stop_factor->stop_pose); + return dist_to_stop < planner_param_.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..852f4aed1111b 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,8 @@ class CrosswalkModule : public SceneModuleInterface double stop_distance_from_crosswalk; double far_object_threshold; double stop_position_threshold; + // param for restart suppression + double dist_to_stop_for_restart_suppression; // param for ego velocity float min_slow_down_velocity; double max_slow_down_jerk; @@ -404,6 +407,8 @@ class CrosswalkModule : public SceneModuleInterface static geometry_msgs::msg::Polygon createVehiclePolygon( const vehicle_info_util::VehicleInfo & vehicle_info); + bool checkRestartSuppression(const std::optional & stop_factor); + void recordTime(const int step_num) { RCLCPP_INFO_EXPRESSION( @@ -429,6 +434,8 @@ class CrosswalkModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; + std::unique_ptr vehicle_stop_checker_{nullptr}; + // Stop watch StopWatch stop_watch_; From 1e5eca0887edbbea95e812255c73b8433a0e3508 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 10 Sep 2024 12:45:02 +0900 Subject: [PATCH 2/5] update Signed-off-by: Takayuki Murooka --- .../config/crosswalk.param.yaml | 3 ++- .../src/manager.cpp | 6 ++++-- .../src/scene_crosswalk.cpp | 11 +++++++---- .../src/scene_crosswalk.hpp | 6 ++++-- 4 files changed, 17 insertions(+), 9 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 337ce859d2b59..c99b3d6d2fa10 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -62,7 +62,8 @@ # params for suppressing the ego to restart when the ego is close to the next stop position. restart_suppression: - distance_to_stop: 1.0 + min_distance_to_stop: 0.5 + max_distance_to_stop: 1.0 # param for target object filtering object_filtering: diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index cc0f5b4111608..c38a542e18313 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -55,8 +55,10 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".stop_position.stop_position_threshold"); // param for restart suppression - cp.dist_to_stop_for_restart_suppression = - getOrDeclareParameter(node, ns + ".restart_suppression.distance_to_stop"); + 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 = diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index b3ee553f12fc4..22fd5202bbf17 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -1203,7 +1203,7 @@ void CrosswalkModule::planStop( } // Check if the restart should be suppressed. - const bool suppress_restart = checkRestartSuppression(stop_factor); + 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; @@ -1217,7 +1217,8 @@ void CrosswalkModule::planStop( VelocityFactor::UNKNOWN); } -bool CrosswalkModule::checkRestartSuppression(const std::optional & stop_factor) +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) { @@ -1226,7 +1227,9 @@ bool CrosswalkModule::checkRestartSuppression(const std::optional & const auto & ego_pos = planner_data_->current_odometry->pose.position; const double dist_to_stop = - autoware::universe_utils::calcDistance2d(ego_pos, stop_factor->stop_pose); - return dist_to_stop < planner_param_.dist_to_stop_for_restart_suppression; + calcSignedArcLength(ego_path.points, ego_pos, stop_factor->stop_pose.position); + + 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 852f4aed1111b..5af97498b1a81 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -117,7 +117,8 @@ class CrosswalkModule : public SceneModuleInterface double far_object_threshold; double stop_position_threshold; // param for restart suppression - double dist_to_stop_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; @@ -407,7 +408,8 @@ class CrosswalkModule : public SceneModuleInterface static geometry_msgs::msg::Polygon createVehiclePolygon( const vehicle_info_util::VehicleInfo & vehicle_info); - bool checkRestartSuppression(const std::optional & stop_factor); + bool checkRestartSuppression( + const PathWithLaneId & ego_path, const std::optional & stop_factor) const; void recordTime(const int step_num) { From 9a5527111cd22cf1039c4a6c0cad24a62aba0e80 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 10 Sep 2024 15:07:59 +0900 Subject: [PATCH 3/5] add comment Signed-off-by: Takayuki Murooka --- .../behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 22fd5202bbf17..c31402662a944 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -1229,6 +1229,8 @@ bool CrosswalkModule::checkRestartSuppression( 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; } From a8dec11941e3a189d1eea5c5c964e6ac62715d17 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Tue, 10 Sep 2024 19:55:11 +0900 Subject: [PATCH 4/5] fix namespace issue --- .../behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index c31402662a944..c2893ce7d1f92 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -198,7 +198,7 @@ CrosswalkModule::CrosswalkModule( collision_info_pub_ = node.create_publisher("~/debug/collision_info", 1); - vehicle_stop_checker_ = std::make_unique(&node); + vehicle_stop_checker_ = std::make_unique(&node); } bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) From e0f28e5dc5e625e7d1202d186f462118e153e6c8 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Tue, 10 Sep 2024 19:55:53 +0900 Subject: [PATCH 5/5] fix namespace issue --- .../behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 5af97498b1a81..77b422f1bd5da 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -436,7 +436,7 @@ class CrosswalkModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; - std::unique_ptr vehicle_stop_checker_{nullptr}; + std::unique_ptr vehicle_stop_checker_{nullptr}; // Stop watch StopWatch stop_watch_;