From 8ae20e29a39de56ba23f4a6ae308324b79ad3982 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 3 Dec 2024 14:32:44 +0900 Subject: [PATCH] revert(behavior_velocity): revert unknown traffic light handling PR6036 Signed-off-by: Mamoru Sobue --- .../src/scene_crosswalk.cpp | 9 ++++----- .../src/scene_intersection.cpp | 10 ++++------ planning/behavior_velocity_planner/src/node.cpp | 16 +--------------- .../planner_data.hpp | 14 ++++---------- .../src/scene.cpp | 7 +++---- 5 files changed, 16 insertions(+), 40 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index b1a6b45d83cf0..14632aadec771 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -1033,20 +1033,19 @@ bool CrosswalkModule::isRedSignalForPedestrians() const crosswalk_.regulatoryElementsAs(); for (const auto & traffic_lights_reg_elem : traffic_lights_reg_elems) { - const auto traffic_signal_stamped_opt = + const auto traffic_signal_stamped = planner_data_->getTrafficSignal(traffic_lights_reg_elem->id()); - if (!traffic_signal_stamped_opt) { + if (!traffic_signal_stamped) { continue; } - const auto traffic_signal_stamped = traffic_signal_stamped_opt.value(); if ( planner_param_.traffic_light_state_timeout < - (clock_->now() - traffic_signal_stamped.stamp).seconds()) { + (clock_->now() - traffic_signal_stamped->stamp).seconds()) { continue; } - const auto & lights = traffic_signal_stamped.signal.elements; + const auto & lights = traffic_signal_stamped->signal.elements; if (lights.empty()) { continue; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index b4d75d6c7734b..08b039feb1573 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1175,13 +1175,12 @@ bool IntersectionModule::isGreenSolidOn() const // this lane has no traffic light return false; } - const auto tl_info_opt = planner_data_->getTrafficSignal( - tl_id.value(), true /* traffic light module keeps last observation*/); + const auto tl_info_opt = planner_data_->getTrafficSignal(tl_id.value()); if (!tl_info_opt) { // the info of this traffic light is not available return false; } - const auto & tl_info = tl_info_opt.value(); + const auto & tl_info = *tl_info_opt; for (auto && tl_light : tl_info.signal.elements) { if ( tl_light.color == TrafficSignalElement::GREEN && @@ -1208,12 +1207,11 @@ IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPriori // this lane has no traffic light return TrafficPrioritizedLevel::NOT_PRIORITIZED; } - const auto tl_info_opt = planner_data_->getTrafficSignal( - tl_id.value(), true /* traffic light module keeps last observation*/); + const auto tl_info_opt = planner_data_->getTrafficSignal(tl_id.value()); if (!tl_info_opt) { return TrafficPrioritizedLevel::NOT_PRIORITIZED; } - const auto & tl_info = tl_info_opt.value(); + const auto & tl_info = *tl_info_opt; bool has_amber_signal{false}; for (auto && tl_light : tl_info.signal.elements) { if (tl_light.color == TrafficSignalElement::AMBER) { diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 04bbea741b181..b5d019820bebe 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -334,21 +334,7 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; traffic_signal.signal = signal; - planner_data_.traffic_light_id_map_raw_[signal.traffic_signal_id] = traffic_signal; - const bool is_unknown_observation = - std::any_of(signal.elements.begin(), signal.elements.end(), [](const auto & element) { - return element.color == autoware_perception_msgs::msg::TrafficSignalElement::UNKNOWN; - }); - // if the observation is UNKNOWN and past observation is available, only update the timestamp - // and keep the body of the info - if ( - is_unknown_observation && - planner_data_.traffic_light_id_map_last_observed_.count(signal.traffic_signal_id) == 1) { - planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp = - msg->stamp; - } else { - planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal; - } + planner_data_.traffic_light_id_map[signal.traffic_signal_id] = traffic_signal; } } 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 9a7af95128c0c..0a5a912c381fd 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 @@ -77,10 +77,7 @@ struct PlannerData double ego_nearest_yaw_threshold; // other internal data - // traffic_light_id_map_raw is the raw observation, while traffic_light_id_map_keep_last keeps the - // last observed infomation for UNKNOWN - std::map traffic_light_id_map_raw_; - std::map traffic_light_id_map_last_observed_; + std::map traffic_light_id_map; std::optional external_velocity_limit; std::map traffic_light_time_to_red_id_map; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; @@ -139,15 +136,12 @@ struct PlannerData *@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 = false) const + std::shared_ptr getTrafficSignal(const lanelet::Id id) const { - const auto & traffic_light_id_map = - keep_last_observation ? traffic_light_id_map_last_observed_ : traffic_light_id_map_raw_; if (traffic_light_id_map.count(id) == 0) { - return std::nullopt; + return {}; } - return std::make_optional(traffic_light_id_map.at(id)); + return std::make_shared(traffic_light_id_map.at(id)); } std::optional getRestTimeToRedSignal(const int id) const diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index f9051259c180b..2afa02f167f44 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -413,15 +413,14 @@ bool TrafficLightModule::isTrafficSignalStop( 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(), true /* traffic light module keeps last observation */); - if (!traffic_signal_stamped_opt) { + const auto traffic_signal_stamped = planner_data_->getTrafficSignal(traffic_light_reg_elem_.id()); + if (!traffic_signal_stamped) { RCLCPP_WARN_THROTTLE( logger_, *clock_, 5000 /* ms */, "the traffic signal data associated with regulatory element id is not received"); return false; } - valid_traffic_signal = traffic_signal_stamped_opt.value(); + valid_traffic_signal = *traffic_signal_stamped; return true; }