From 1ec53d857607604b0114739a6c19addc2dba0f33 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Fri, 6 Oct 2023 13:54:15 +0900 Subject: [PATCH] feat(intersection)!: disable the exception behavior in the private areas (#5229) * feat: make configurable to disable the exception treat of stuck obstacle in the private areas feat: change behavior in the private areas Signed-off-by: Yuki Takagi * delete the coment outed lines Signed-off-by: Yuki Takagi * change "enabled" to "enable" Signed-off-by: Yuki Takagi * fix setting Signed-off-by: Yuki Takagi * delete unused variables Signed-off-by: Yuki Takagi --------- Signed-off-by: Yuki Takagi --- .../config/intersection.param.yaml | 1 + .../src/manager.cpp | 2 ++ .../src/scene_intersection.cpp | 11 ++--------- .../src/scene_intersection.hpp | 2 +- 4 files changed, 6 insertions(+), 10 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index ccd3612203af5..e210d2fb21b88 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -19,6 +19,7 @@ # enable_front_car_decel_prediction: false # By default this feature is disabled # assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning timeout_private_area: 3.0 # [s] cancel stuck vehicle stop in private area + enable_private_area_stuck_disregard: false #In the intersections which labeled as "private area", the obstacle vehicles judged as "stuck" are neglected if this param is set as true. collision_detection: state_transit_margin_time: 1.0 diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 6f47dfbbe31fb..3c96a171516bd 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -77,6 +77,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) */ ip.stuck_vehicle.timeout_private_area = getOrDeclareParameter(node, ns + ".stuck_vehicle.timeout_private_area"); + ip.stuck_vehicle.enable_private_area_stuck_disregard = + getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_private_area_stuck_disregard"); ip.collision_detection.min_predicted_path_confidence = getOrDeclareParameter(node, ns + ".collision_detection.min_predicted_path_confidence"); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 10f1ed73bf435..b30a57f9e8dad 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -96,10 +96,6 @@ IntersectionModule::IntersectionModule( occlusion_stop_state_machine_.setMarginTime(planner_param_.occlusion.stop_release_margin_time); occlusion_stop_state_machine_.setState(StateMachine::State::GO); } - { - stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area); - stuck_private_area_timeout_.setState(StateMachine::State::STOP); - } { temporal_stop_before_attention_state_machine_.setMarginTime( planner_param_.occlusion.before_creep_stop_time); @@ -918,16 +914,13 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); if (stuck_detected && stuck_stop_line_idx_opt) { auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); - const bool stopped_at_stuck_line = stoppedForDuration( - stuck_stop_line_idx, planner_param_.stuck_vehicle.timeout_private_area, - stuck_private_area_timeout_); - const bool timeout = (is_private_area_ && stopped_at_stuck_line); - if (!timeout) { + if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) { if ( default_stop_line_idx_opt && fromEgoDist(stuck_stop_line_idx) < -planner_param_.common.stop_overshoot_margin) { stuck_stop_line_idx = default_stop_line_idx_opt.value(); } + } else { return IntersectionModule::StuckStop{ closest_idx, stuck_stop_line_idx, occlusion_peeking_stop_line_idx_opt}; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index b3a8e69b70b6a..f1721e5ac5b4f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -71,6 +71,7 @@ class IntersectionModule : public SceneModuleInterface bool enable_front_car_decel_prediction; //! flag for using above feature */ double timeout_private_area; + bool enable_private_area_stuck_disregard; } stuck_vehicle; struct CollisionDetection { @@ -256,7 +257,6 @@ class IntersectionModule : public SceneModuleInterface // for stuck vehicle detection const bool is_private_area_; - StateMachine stuck_private_area_timeout_; // for RTC const UUID occlusion_uuid_;