diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 089aa0c308832..fabd1afaeca81 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -953,7 +953,7 @@ bool IntersectionModule::isGreenSolidOn(lanelet::ConstLanelet lane) // this lane has no traffic light return false; } - const auto tl_info_opt = planner_data_->getTrafficSignal(tl_id.value()); + const auto tl_info_opt = planner_data_->getTrafficSignal(tl_id.value(), true); if (!tl_info_opt) { // the info of this traffic light is not available return false; 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 ccb7c53a70304..606e41ad4b1d1 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 @@ -128,8 +128,13 @@ struct PlannerData return true; } + /** + *@fn + *@brief queries the traffic signal information of given Id. if keep_last_observation = true, + *recent UNKNOWN observation is overwritten as the last non-UNKNOWN observation + */ std::optional getTrafficSignal( - const lanelet::Id id, const bool keep_last_observation = true) const + const lanelet::Id id, const bool keep_last_observation = false) const { const auto & traffic_light_id_map = keep_last_observation ? traffic_light_id_map_last_observed_ : traffic_light_id_map_raw_; diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 4be094af83d6d..9982ec34c4bca 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -353,8 +353,8 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const bool TrafficLightModule::findValidTrafficSignal(TrafficSignalStamped & valid_traffic_signal) const { // get traffic signal associated with the regulatory element id - const auto traffic_signal_stamped_opt = - planner_data_->getTrafficSignal(traffic_light_reg_elem_.id()); + const auto traffic_signal_stamped_opt = planner_data_->getTrafficSignal( + traffic_light_reg_elem_.id(), true /* traffic light module keeps last observation */); if (!traffic_signal_stamped_opt) { RCLCPP_WARN_THROTTLE( logger_, *clock_, 5000 /* ms */,