From 091160047d77f5cfd140aef8b8a58681590b9b80 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 27 Jan 2023 14:04:43 +0900 Subject: [PATCH] feat(intersection): add param for stuck stopline overshoot margin (#2756) Signed-off-by: Mamoru Sobue --- .../behavior_velocity_planner/config/intersection.param.yaml | 1 + .../include/scene_module/intersection/scene_intersection.hpp | 1 + .../src/scene_module/intersection/manager.cpp | 1 + .../src/scene_module/intersection/scene_intersection.cpp | 3 +-- 4 files changed, 4 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index a769848497680..9ed153bb1c3d4 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -20,6 +20,7 @@ use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning enable_front_car_decel_prediction: false # By default this feature is disabled + stop_overshoot_margin: 0.5 # [m] overshoot margin for stuck, collsion detection merge_from_private_road: stop_duration_sec: 1.0 diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 98589555bd7aa..7ac5c25ce38e1 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -93,6 +93,7 @@ class IntersectionModule : public SceneModuleInterface double assumed_front_car_decel; //! the expected deceleration of front car when front car as well bool enable_front_car_decel_prediction; //! flag for using above feature + double stop_overshoot_margin; //! overshoot margin for stuck, collsion detection }; IntersectionModule( diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index a8af4938b1bf0..8f4ee1c91cee2 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -69,6 +69,7 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.assumed_front_car_decel = node.declare_parameter(ns + ".assumed_front_car_decel", 1.0); ip.enable_front_car_decel_prediction = node.declare_parameter(ns + ".enable_front_car_decel_prediction", false); + ip.stop_overshoot_margin = node.declare_parameter(ns + ".stop_overshoot_margin", 0.5); } MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node) diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index bbe6955c2a601..ed44be0508ee5 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -202,10 +202,9 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const double dist_stuck_stopline = motion_utils::calcSignedArcLength( path->points, path->points.at(stuck_line_idx).point.pose.position, path->points.at(closest_idx).point.pose.position); - const double eps = 1e-1; // NOTE: check if sufficiently over the stuck stopline const bool is_over_stuck_stopline = util::isOverTargetIndex(*path, closest_idx, current_pose, stuck_line_idx) && - dist_stuck_stopline > eps; + dist_stuck_stopline > planner_param_.stop_overshoot_margin; if (is_stuck && !is_over_stuck_stopline) { stop_line_idx_final = stuck_line_idx; pass_judge_line_idx_final = stuck_line_idx;