diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index c2f78f28005dd..2103ef25c6cfc 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -148,17 +148,18 @@ static std::string formatOcclusionType(const IntersectionModule::OcclusionType & intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { - const auto traffic_prioritized_level = getTrafficPrioritizedLevel(); - const bool is_prioritized = - traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; - - const auto prepare_data = prepareIntersectionData(is_prioritized, path); + const auto prepare_data = prepareIntersectionData(path); if (!prepare_data) { return prepare_data.err(); } const auto [interpolated_path_info, intersection_stoplines, path_lanelets] = prepare_data.ok(); const auto & intersection_lanelets = intersection_lanelets_.value(); + // NOTE: this level is based on the updateTrafficSignalObservation() which is latest + const auto traffic_prioritized_level = getTrafficPrioritizedLevel(); + const bool is_prioritized = + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; + // ========================================================================================== // stuck detection // diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index fcb1ef3a1985a..73736c804fb02 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -442,6 +442,9 @@ class IntersectionModule : public SceneModuleInterface //! unavailable) std::optional> tl_id_and_point_; std::optional last_tl_valid_observation_{std::nullopt}; + + //! save previous priority level to detect change from NotPrioritized to Prioritized + TrafficPrioritizedLevel previous_prioritized_level_{TrafficPrioritizedLevel::NOT_PRIORITIZED}; /** @} */ private: @@ -548,7 +551,7 @@ class IntersectionModule : public SceneModuleInterface * To simplify modifyPathVelocityDetail(), this function is used at first */ intersection::Result prepareIntersectionData( - const bool is_prioritized, PathWithLaneId * path); + PathWithLaneId * path); /** * @brief find the associated stopline road marking of assigned lanelet diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index 2a66aedd36ac7..b741d43bb025a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -63,10 +63,23 @@ IntersectionModule::getOcclusionStatus( !is_amber_or_red_or_no_tl_info_ever) ? detectOcclusion(interpolated_path_info) : NotOccluded{}; - occlusion_stop_state_machine_.setStateWithMarginTime( - std::holds_alternative(occlusion_status) ? StateMachine::State::GO - : StateMachine::STOP, - logger_.get_child("occlusion_stop"), *clock_); + + // ========================================================================================== + // if the traffic light changed from green to yellow/red, hysteresis time for occlusion is + // unnecessary + // ========================================================================================== + const auto transition_to_prioritized = + (previous_prioritized_level_ == TrafficPrioritizedLevel::NOT_PRIORITIZED && + traffic_prioritized_level != TrafficPrioritizedLevel::NOT_PRIORITIZED); + if (transition_to_prioritized) { + occlusion_stop_state_machine_.setState(StateMachine::State::GO); + } else { + occlusion_stop_state_machine_.setStateWithMarginTime( + std::holds_alternative(occlusion_status) ? StateMachine::State::GO + : StateMachine::STOP, + logger_.get_child("occlusion_stop"), *clock_); + } + const bool is_occlusion_cleared_with_margin = (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); // module's detection // distinguish if ego detected occlusion or RTC detects occlusion diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 52c6b06541796..97d05aef26137 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -166,7 +166,7 @@ using intersection::make_ok; using intersection::Result; Result -IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithLaneId * path) +IntersectionModule::prepareIntersectionData(PathWithLaneId * path) { const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); @@ -175,6 +175,18 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); const auto & current_pose = planner_data_->current_odometry->pose; + // ========================================================================================== + // update traffic light information + // updateTrafficSignalObservation() must be called at first because other traffic signal + // fuctions use last_valid_observation_ + // ========================================================================================== + // save previous information before calling updateTrafficSignalObservation() + previous_prioritized_level_ = getTrafficPrioritizedLevel(); + updateTrafficSignalObservation(); + const auto traffic_prioritized_level = getTrafficPrioritizedLevel(); + const bool is_prioritized = + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; + // spline interpolation const auto interpolated_path_info_opt = util::generateInterpolatedPath( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); @@ -264,13 +276,7 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL planner_data_->occupancy_grid->info.resolution); } - // ========================================================================================== - // update traffic light information - // updateTrafficSignalObservation() must be called at first to because other traffic signal - // fuctions use last_valid_observation_ - // ========================================================================================== if (has_traffic_light_) { - updateTrafficSignalObservation(); const bool is_green_solid_on = isGreenSolidOn(); if (is_green_solid_on && !initial_green_light_observed_time_) { const auto assigned_lane_begin_point = assigned_lanelet.centerline().front();