keep_last_observation is false by default and intersection/traffic_li…

…ght is explicily true Signed-off-by: Mamoru Sobue <[email protected]>
autowarefoundation · Jan 10, 2024 · ee5e57a · ee5e57a
1 parent 920a70e
commit ee5e57a
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 4 deletions.
diff --git 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/...ehavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/...ehavior_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<TrafficSignalStamped> 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
@@ -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 */,