diff --git a/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml b/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml index 992a2f0306782..9df48644f74f0 100644 --- a/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml +++ b/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml @@ -2,3 +2,6 @@ ros__parameters: virtual_traffic_light: max_delay_sec: 3.0 + near_line_distance: 1.0 + dead_line_margin: 1.0 + check_timeout_after_stop_line: true diff --git a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp index 2a7f19bcb164a..457a15eb3251b 100644 --- a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp @@ -66,6 +66,9 @@ class VirtualTrafficLightModule : public SceneModuleInterface struct PlannerParam { double max_delay_sec; + double near_line_distance; + double dead_line_margin; + bool check_timeout_after_stop_line; }; public: diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp index 0dc04d1d11b3f..a94ec5eab3c97 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp @@ -68,6 +68,10 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node { auto & p = planner_param_; p.max_delay_sec = node.declare_parameter(ns + ".max_delay_sec", 3.0); + p.near_line_distance = node.declare_parameter(ns + ".near_line_distance", 1.0); + p.dead_line_margin = node.declare_parameter(ns + ".dead_line_margin", 1.0); + p.check_timeout_after_stop_line = + node.declare_parameter(ns + ".check_timeout_after_stop_line", true); } } diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp index 7541c7495e0d7..fcccaca11eee3 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp @@ -399,9 +399,9 @@ bool VirtualTrafficLightModule::modifyPathVelocity( return true; } - // Stop at stop_line if state is timeout before stop_line - if (isBeforeStopLine()) { - if (!isStateTimeout(*virtual_traffic_light_state)) { + // Stop at stop_line if state is timeout + if (isBeforeStopLine() || planner_param_.check_timeout_after_stop_line) { + if (isStateTimeout(*virtual_traffic_light_state)) { RCLCPP_DEBUG(logger_, "state is timeout"); insertStopVelocityAtStopLine(path, stop_reason); } @@ -472,7 +472,7 @@ bool VirtualTrafficLightModule::isBeforeStopLine() const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength( module_data_.path.points, module_data_.head_pose.position, collision->point); - return signed_arc_length > 0; + return signed_arc_length > -planner_param_.dead_line_margin; } bool VirtualTrafficLightModule::isAfterAnyEndLine() @@ -493,8 +493,7 @@ bool VirtualTrafficLightModule::isAfterAnyEndLine() const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength( module_data_.path.points, module_data_.head_pose.position, collision->point); - constexpr double max_excess_distance = 3.0; - return signed_arc_length < -max_excess_distance; + return signed_arc_length < -planner_param_.dead_line_margin; } bool VirtualTrafficLightModule::isNearAnyEndLine() @@ -508,8 +507,7 @@ bool VirtualTrafficLightModule::isNearAnyEndLine() const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength( module_data_.path.points, module_data_.head_pose.position, collision->point); - constexpr double near_distance = 1.0; - return std::abs(signed_arc_length) < near_distance; + return std::abs(signed_arc_length) < planner_param_.near_line_distance; } boost::optional @@ -535,10 +533,10 @@ bool VirtualTrafficLightModule::isStateTimeout( const auto delay = (clock_->now() - rclcpp::Time(state.stamp)).seconds(); if (delay > planner_param_.max_delay_sec) { RCLCPP_DEBUG(logger_, "delay=%f, max_delay=%f", delay, planner_param_.max_delay_sec); - return false; + return true; } - return true; + return false; } bool VirtualTrafficLightModule::hasRightOfWay(