feat(behavior_velocity): add the option to keep the last valid observation (autowarefoundation#6036)

* feat(behavior_velocity): add the option to keep the last valid observation

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* keep_last_observation is false by default and intersection/traffic_light is explicily true

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* for intersection

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

---------

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -1035,19 +1035,20 @@ 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 =
+    const auto traffic_signal_stamped_opt =
       planner_data_->getTrafficSignal(traffic_lights_reg_elem->id());
-    if (!traffic_signal_stamped) {
+    if (!traffic_signal_stamped_opt) {
       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

@@ -940,12 +940,11 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   return true;
 }
 
-static bool isGreenSolidOn(
-  lanelet::ConstLanelet lane, const std::map<int, TrafficSignalStamped> & tl_infos)
+bool IntersectionModule::isGreenSolidOn(lanelet::ConstLanelet lane)
 {
   using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
 
-  std::optional<int> tl_id = std::nullopt;
+  std::optional<lanelet::Id> tl_id = std::nullopt;
   for (auto && tl_reg_elem : lane.regulatoryElementsAs<lanelet::TrafficLight>()) {
     tl_id = tl_reg_elem->id();
     break;
@@ -954,12 +953,13 @@ static bool isGreenSolidOn(
     // this lane has no traffic light
     return false;
   }
-  const auto tl_info_it = tl_infos.find(tl_id.value());
-  if (tl_info_it == tl_infos.end()) {
+  const auto tl_info_opt = planner_data_->getTrafficSignal(
+    tl_id.value(), true /* traffic light module keeps last observation*/);
+  if (!tl_info_opt) {
     // the info of this traffic light is not available
     return false;
   }
-  const auto & tl_info = tl_info_it->second;
+  const auto & tl_info = tl_info_opt.value();
   for (auto && tl_light : tl_info.signal.elements) {
     if (
       tl_light.color == TrafficSignalElement::GREEN &&
@@ -1004,8 +1004,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
 
   // at the very first time of regisTration of this module, the path may not be conflicting with the
   // attention area, so update() is called to update the internal data as well as traffic light info
-  const auto traffic_prioritized_level =
-    getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map);
+  const auto traffic_prioritized_level = getTrafficPrioritizedLevel(assigned_lanelet);
   const bool is_prioritized =
     traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED;
   intersection_lanelets.update(
@@ -1300,8 +1299,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   // If there are any vehicles on the attention area when ego entered the intersection on green
   // light, do pseudo collision detection because the vehicles are very slow and no collisions may
   // be detected. check if ego vehicle entered assigned lanelet
-  const bool is_green_solid_on =
-    isGreenSolidOn(assigned_lanelet, planner_data_->traffic_light_id_map);
+  const bool is_green_solid_on = isGreenSolidOn(assigned_lanelet);
   if (is_green_solid_on) {
     if (!initial_green_light_observed_time_) {
       const auto assigned_lane_begin_point = assigned_lanelet.centerline().front();
@@ -1458,12 +1456,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   }
 }
 
-TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel(
-  lanelet::ConstLanelet lane, const std::map<int, TrafficSignalStamped> & tl_infos)
+TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel(lanelet::ConstLanelet lane)
 {
   using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
 
-  std::optional<int> tl_id = std::nullopt;
+  std::optional<lanelet::Id> tl_id = std::nullopt;
   for (auto && tl_reg_elem : lane.regulatoryElementsAs<lanelet::TrafficLight>()) {
     tl_id = tl_reg_elem->id();
     break;
@@ -1472,12 +1469,12 @@ TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel(
     // this lane has no traffic light
     return TrafficPrioritizedLevel::NOT_PRIORITIZED;
   }
-  const auto tl_info_it = tl_infos.find(tl_id.value());
-  if (tl_info_it == tl_infos.end()) {
-    // the info of this traffic light is not available
+  const auto tl_info_opt = planner_data_->getTrafficSignal(
+    tl_id.value(), true /* traffic light module keeps last observation*/);
+  if (!tl_info_opt) {
     return TrafficPrioritizedLevel::NOT_PRIORITIZED;
   }
-  const auto & tl_info = tl_info_it->second;
+  const auto & tl_info = tl_info_opt.value();
   bool has_amber_signal{false};
   for (auto && tl_light : tl_info.signal.elements) {
     if (tl_light.color == TrafficSignalElement::AMBER) {

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -668,8 +668,7 @@ class IntersectionModule : public SceneModuleInterface
    * @fn
    * @brief find TrafficPrioritizedLevel
    */
-  TrafficPrioritizedLevel getTrafficPrioritizedLevel(
-    lanelet::ConstLanelet lane, const std::map<int, TrafficSignalStamped> & tl_infos);
+  TrafficPrioritizedLevel getTrafficPrioritizedLevel(lanelet::ConstLanelet lane);
 
   /**
    * @fn
@@ -776,6 +775,12 @@ class IntersectionModule : public SceneModuleInterface
     const std::vector<lanelet::ConstLineString3d> & lane_divisions,
     const TargetObjects & target_objects);
 
+  /*
+   * @fn
+   * @brief check if associated traffic light is green
+   */
+  bool isGreenSolidOn(lanelet::ConstLanelet lane);
+
   /*
   bool IntersectionModule::checkFrontVehicleDeceleration(
     lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet,

planning/behavior_velocity_planner/src/node.cpp

@@ -334,7 +334,21 @@ void BehaviorVelocityPlannerNode::onTrafficSignals(
     TrafficSignalStamped traffic_signal;
     traffic_signal.stamp = msg->stamp;
     traffic_signal.signal = signal;
-    planner_data_.traffic_light_id_map[signal.traffic_signal_id] = traffic_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;
+    }
   }
 }
 

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp

@@ -77,7 +77,10 @@ struct PlannerData
   double ego_nearest_yaw_threshold;
 
   // other internal data
-  std::map<int, TrafficSignalStamped> traffic_light_id_map;
+  // 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::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;
@@ -131,12 +134,20 @@ struct PlannerData
     return true;
   }
 
-  std::shared_ptr<TrafficSignalStamped> getTrafficSignal(const int id) const
+  /**
+   *@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<TrafficSignalStamped> getTrafficSignal(
+    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_;
     if (traffic_light_id_map.count(id) == 0) {
-      return {};
+      return std::nullopt;
     }
-    return std::make_shared<TrafficSignalStamped>(traffic_light_id_map.at(id));
+    return std::make_optional<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,15 @@ 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 = planner_data_->getTrafficSignal(traffic_light_reg_elem_.id());
-  if (!traffic_signal_stamped) {
+  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 */,
       "the traffic signal data associated with regulatory element id is not received");
     return false;
   }
-
-  valid_traffic_signal = *traffic_signal_stamped;
+  valid_traffic_signal = traffic_signal_stamped_opt.value();
   return true;
 }