Merge pull request #1675 from tier4/revert/behavior_velocity/unknown-…

…traffic-light

revert(behavior_velocity): revert unknown traffic light handling PR6036

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -1033,20 +1033,19 @@ bool CrosswalkModule::isRedSignalForPedestrians() const
     crosswalk_.regulatoryElementsAs<const lanelet::TrafficLight>();
 
   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;
     }

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) {

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;
   }
 }
 

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<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_raw_;
-  std::map<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_last_observed_;
+  std::map<lanelet::Id, TrafficSignalStamped> traffic_light_id_map;
   std::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
   std::map<int, TrafficSignalTimeToRedStamped> 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<TrafficSignalStamped> getTrafficSignal(
-    const lanelet::Id id, const bool keep_last_observation = false) const
+  std::shared_ptr<TrafficSignalStamped> 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<TrafficSignalStamped>(traffic_light_id_map.at(id));
+    return std::make_shared<TrafficSignalStamped>(traffic_light_id_map.at(id));
   }
 
   std::optional<TrafficSignalTimeToRedStamped> getRestTimeToRedSignal(const int id) const

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;
 }