feat(behavior_velocity_traffic_light): do not overwrite unknown with …

…past known color (autowarefoundation#7044) (autowarefoundation#1305)

feat(behavior_velocity_traffic_light): do not overwrite UNKNOWN with past KNOWN color

remove first_stop_path_point_index

Revert "remove first_stop_path_point_index"

This reverts commit 09bba6981c0478832d90e3c0af1212ff2b94d928.

fix

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

planning/behavior_velocity_planner/src/node.cpp

@@ -352,6 +352,7 @@ void BehaviorVelocityPlannerNode::onTrafficSignals(
       planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp =
         msg->stamp;
     } else {
+      // if (1)the observation is not UNKNOWN or (2)the very first observation is UNKNOWN
       planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal;
     }
   }

planning/behavior_velocity_traffic_light_module/package.xml

@@ -8,6 +8,7 @@
   <maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
   <maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
   <maintainer email="shumpei.wakabayashi@tier4.jp">Shumpei Wakabayashi</maintainer>
+  <maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>
 
   <license>Apache License 2.0</license>
 

planning/behavior_velocity_traffic_light_module/src/manager.cpp

@@ -62,7 +62,7 @@ void TrafficLightModuleManager::modifyPathVelocity(
   stop_reason_array.header.stamp = path->header.stamp;
 
   first_stop_path_point_index_ = static_cast<int>(path->points.size() - 1);
-  first_ref_stop_path_point_index_ = static_cast<int>(path->points.size() - 1);
+  nearest_ref_stop_path_point_index_ = static_cast<int>(path->points.size() - 1);
   for (const auto & scene_module : scene_modules_) {
     tier4_planning_msgs::msg::StopReason stop_reason;
     std::shared_ptr<TrafficLightModule> traffic_light_scene_module(
@@ -85,8 +85,8 @@ void TrafficLightModuleManager::modifyPathVelocity(
     }
     if (
       traffic_light_scene_module->getFirstRefStopPathPointIndex() <
-      first_ref_stop_path_point_index_) {
-      first_ref_stop_path_point_index_ =
+      nearest_ref_stop_path_point_index_) {
+      nearest_ref_stop_path_point_index_ =
         traffic_light_scene_module->getFirstRefStopPathPointIndex();
       if (
         traffic_light_scene_module->getTrafficLightModuleState() !=
@@ -126,14 +126,13 @@ void TrafficLightModuleManager::launchNewModules(
 
     // Use lanelet_id to unregister module when the route is changed
     const auto lane_id = traffic_light_reg_elem.second.id();
-    const auto module_id = lane_id;
-    if (!isModuleRegisteredFromExistingAssociatedModule(module_id)) {
+    if (!isModuleRegisteredFromExistingAssociatedModule(lane_id)) {
       registerModule(std::make_shared<TrafficLightModule>(
-        module_id, lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second,
-        planner_param_, logger_.get_child("traffic_light_module"), clock_));
-      generateUUID(module_id);
+        lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_,
+        logger_.get_child("traffic_light_module"), clock_));
+      generateUUID(lane_id);
       updateRTCStatus(
-        getUUID(module_id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
+        getUUID(lane_id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
     }
   }
 }
@@ -145,33 +144,17 @@ TrafficLightModuleManager::getModuleExpiredFunction(
   const auto lanelet_id_set = planning_utils::getLaneletIdSetOnPath<TrafficLight>(
     path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose);
 
-  return [this, lanelet_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
+  return [this, lanelet_id_set](
+           [[maybe_unused]] const std::shared_ptr<SceneModuleInterface> & scene_module) {
     for (const auto & id : lanelet_id_set) {
-      if (isModuleRegisteredFromRegElement(id, scene_module->getModuleId())) {
+      if (isModuleRegisteredFromExistingAssociatedModule(id)) {
         return false;
       }
     }
     return true;
   };
 }
 
-bool TrafficLightModuleManager::isModuleRegisteredFromRegElement(
-  const lanelet::Id & id, const size_t module_id) const
-{
-  const auto lane = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(id);
-
-  const auto registered_lane =
-    planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(module_id);
-  for (const auto & registered_element : registered_lane.regulatoryElementsAs<TrafficLight>()) {
-    for (const auto & element : lane.regulatoryElementsAs<TrafficLight>()) {
-      if (hasSameTrafficLight(element, registered_element)) {
-        return true;
-      }
-    }
-  }
-  return false;
-}
-
 bool TrafficLightModuleManager::isModuleRegisteredFromExistingAssociatedModule(
   const lanelet::Id & id) const
 {

planning/behavior_velocity_traffic_light_module/src/manager.hpp

@@ -58,7 +58,7 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC
   // Debug
   rclcpp::Publisher<autoware_perception_msgs::msg::TrafficSignal>::SharedPtr pub_tl_state_;
 
-  std::optional<int> first_ref_stop_path_point_index_;
+  std::optional<int> nearest_ref_stop_path_point_index_;
 };
 
 class TrafficLightModulePlugin : public PluginWrapper<TrafficLightModuleManager>

planning/behavior_velocity_traffic_light_module/src/scene.cpp

@@ -162,11 +162,10 @@ bool calcStopPointAndInsertIndex(
 }  // namespace
 
 TrafficLightModule::TrafficLightModule(
-  const int64_t module_id, const int64_t lane_id,
-  const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane,
-  const PlannerParam & planner_param, const rclcpp::Logger logger,
+  const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem,
+  lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger,
   const rclcpp::Clock::SharedPtr clock)
-: SceneModuleInterface(module_id, logger, clock),
+: SceneModuleInterface(lane_id, logger, clock),
   lane_id_(lane_id),
   traffic_light_reg_elem_(traffic_light_reg_elem),
   lane_(lane),
@@ -365,7 +364,7 @@ bool TrafficLightModule::findValidTrafficSignal(TrafficSignalStamped & valid_tra
 {
   // 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 */);
+    traffic_light_reg_elem_.id(), false /* traffic light module does not keep last observation */);
   if (!traffic_signal_stamped_opt) {
     RCLCPP_WARN_STREAM_ONCE(
       logger_, "the traffic signal data associated with regulatory element id "

planning/behavior_velocity_traffic_light_module/src/scene.hpp

@@ -66,9 +66,8 @@ class TrafficLightModule : public SceneModuleInterface
 
 public:
   TrafficLightModule(
-    const int64_t module_id, const int64_t lane_id,
-    const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane,
-    const PlannerParam & planner_param, const rclcpp::Logger logger,
+    const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem,
+    lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger,
     const rclcpp::Clock::SharedPtr clock);
 
   bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;