diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index fc301620093da..ad374294d0686 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -141,12 +141,14 @@ struct PlannerData } return std::make_shared(traffic_light_id_map.at(id)); } - double getRestTimeToRedSignal(const int id) const + + std::optional getRestTimeToRedSignal(const int id) const { - if (traffic_light_time_to_red_id_map.count(id) == 0) { - return std::numeric_limits::max(); + try { + return traffic_light_time_to_red_id_map.at(id); + } catch (std::out_of_range &) { + return std::nullopt; } - return traffic_light_time_to_red_id_map.at(id); } }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml index 73e0944067c9b..5d4bc392571ea 100644 --- a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -8,6 +8,7 @@ enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval v2i: - last_time_allowed_to_pass: 2.0 # # relative time against at the time of turn to red + use_rest_time: true + last_time_allowed_to_pass: 2.0 # relative time against at the time of turn to red velocity_threshold: 0.5 # change the decision logic whether the current velocity is faster or not - required_time_to_departure: 3.0 + required_time_to_departure: 3.0 # prevent low speed pass \ No newline at end of file diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index f9080f9004258..8ef247847c503 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -41,6 +41,7 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); planner_param_.yellow_lamp_period = getOrDeclareParameter(node, ns + ".yellow_lamp_period"); + planner_param_.v2i_use_rest_time = getOrDeclareParameter(node, ns + ".v2i.use_rest_time"); planner_param_.v2i_last_time_allowed_to_pass = getOrDeclareParameter(node, ns + ".v2i.last_time_allowed_to_pass"); planner_param_.v2i_velocity_threshold = diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 4e0664a3385aa..8c3620f07b1f6 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -225,36 +225,33 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * first_ref_stop_path_point_index_ = stop_line_point_idx; - const double rest_time_to_red_signal = + const std::optional rest_time_to_red_signal = planner_data_->getRestTimeToRedSignal(traffic_light_reg_elem_.id()); - const double rest_time_allowed_to_go_ahead = - rest_time_to_red_signal - planner_param_.v2i_last_time_allowed_to_pass; - - RCLCPP_DEBUG( - logger_, "\nrest_time_allowed_to_go_ahead: %2.2f = %2.2f - %2.2f", - rest_time_allowed_to_go_ahead, rest_time_to_red_signal, - planner_param_.v2i_last_time_allowed_to_pass); - - const double ego_v = planner_data_->current_velocity->twist.linear.x; - if (ego_v >= planner_param_.v2i_velocity_threshold) { - RCLCPP_DEBUG( - logger_, "\nego moves enough fast %2.2f > %2.2f", ego_v, - planner_param_.v2i_velocity_threshold); - if (ego_v * rest_time_allowed_to_go_ahead <= signed_arc_length_to_stop_point) { - RCLCPP_DEBUG( - logger_, "\nplan to stop because ego will not reach %2.2f <= %2.2f", - ego_v * rest_time_allowed_to_go_ahead, signed_arc_length_to_stop_point); - *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); - } - } else { - if (rest_time_allowed_to_go_ahead < planner_param_.v2i_required_time_to_departure) { - RCLCPP_DEBUG( - logger_, "\nplan to stop because there is enough rest time to departure %2.2f < %2.2f", - rest_time_allowed_to_go_ahead, planner_param_.v2i_required_time_to_departure); - *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); + if (planner_param_.v2i_use_rest_time && rest_time_to_red_signal) { + const double rest_time_allowed_to_go_ahead = + rest_time_to_red_signal.value() - planner_param_.v2i_last_time_allowed_to_pass; + + const double ego_v = planner_data_->current_velocity->twist.linear.x; + if (ego_v >= planner_param_.v2i_velocity_threshold) { + if (ego_v * rest_time_allowed_to_go_ahead <= signed_arc_length_to_stop_point) { + *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); + } + } else { + if (rest_time_allowed_to_go_ahead < planner_param_.v2i_required_time_to_departure) { + *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); + } } + return true; + } + setSafe(!isStopSignal()); + if (isActivated()) { + is_prev_state_stop_ = false; + return true; + } + if (!isPassthrough(signed_arc_length_to_stop_point)) { + *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); + is_prev_state_stop_ = true; } - return true; } else if (state_ == State::GO_OUT) { // Initialize if vehicle is far from stop_line constexpr bool use_initialization_after_start = true; @@ -295,8 +292,7 @@ bool TrafficLightModule::updateTrafficSignal() return true; } -bool TrafficLightModule::isPassthrough( - const double & signed_arc_length) const // unused in the V2I temporary implementation +bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const { const double max_acc = planner_data_->max_stop_acceleration_threshold; const double max_jerk = planner_data_->max_stop_jerk_threshold; diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index c48686ae793ea..b53f9c55e7bb6 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -61,6 +61,7 @@ class TrafficLightModule : public SceneModuleInterface double tl_state_timeout; double yellow_lamp_period; bool enable_pass_judge; + bool v2i_use_rest_time; double v2i_last_time_allowed_to_pass; double v2i_velocity_threshold; double v2i_required_time_to_departure;