feat(traffic_light_estimater): fix callback

perception/traffic_light_estimater/include/traffic_light_estimater/node.hpp

@@ -61,9 +61,7 @@ class TrafficLightEstimaterNode : public rclcpp::Node
   lanelet::routing::RoutingGraphPtr routing_graph_ptr_;
   std::shared_ptr<const lanelet::routing::RoutingGraphContainer> overall_graphs_ptr_;
 
-  std::unordered_map<lanelet::ConstLanelet /*crosswalk*/, lanelet::ConstLanelet /*road*/>
-    conflicting_crosswalks_;
-  // lanelet::ConstLanelets conflicting_crosswalks_;
+  lanelet::ConstLanelets conflicting_crosswalks_;
 
   void onMap(const HADMapBin::ConstSharedPtr msg);
   void onRoute(const HADMapRoute::ConstSharedPtr msg);

perception/traffic_light_estimater/launch/traffic_light_estimater.launch.xml

@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <launch>
-  <node pkg="traffic_light_estimater" exec="traffic_light_estimater_node" name="traffic_light_estimater">
+  <node pkg="traffic_light_estimater" exec="traffic_light_estimater_node" name="traffic_light_estimater" output="screen">
     <remap from="~/input/vector_map" to="/map/vector_map"/>
     <remap from="~/input/route" to="/planning/mission_planning/route"/>
     <remap from="~/input/observed/traffic_signals" to="~/observed/traffic_signals"/>

perception/traffic_light_estimater/src/node.cpp

@@ -22,6 +22,59 @@
 
 namespace traffic_light
 {
+namespace
+{
+
+bool hasMergeLane(
+  const lanelet::ConstLanelet & lanelet_1, const lanelet::ConstLanelet & lanelet_2,
+  const lanelet::routing::RoutingGraphPtr & routing_graph_ptr)
+{
+  const auto next_lanelets_1 = routing_graph_ptr->following(lanelet_1);
+  const auto next_lanelets_2 = routing_graph_ptr->following(lanelet_2);
+
+  for (const auto next_lanelet_1 : next_lanelets_1) {
+    for (const auto next_lanelet_2 : next_lanelets_2) {
+      if (next_lanelet_1.id() == next_lanelet_2.id()) {
+        return true;
+      }
+    }
+  }
+
+  return false;
+}
+
+bool hasMergeLane(
+  const lanelet::ConstLanelets & lanelets,
+  const lanelet::routing::RoutingGraphPtr & routing_graph_ptr)
+{
+  for (size_t i = 0; i < lanelets.size(); ++i) {
+    for (size_t j = i + 1; j < lanelets.size(); ++j) {
+      const auto lanelet_1 = lanelets.at(i);
+      const auto lanelet_2 = lanelets.at(j);
+
+      if (lanelet_1.id() == lanelet_2.id()) {
+        continue;
+      }
+
+      const std::string turn_direction_1 = lanelet_1.attributeOr("turn_direction", "none");
+      const std::string turn_direction_2 = lanelet_2.attributeOr("turn_direction", "none");
+      if (turn_direction_1 == turn_direction_2) {
+        continue;
+      }
+
+      if (!hasMergeLane(lanelet_1, lanelet_2, routing_graph_ptr)) {
+        continue;
+      }
+
+      return true;
+    }
+  }
+
+  return false;
+}
+
+}  // namespace
+
 TrafficLightEstimaterNode::TrafficLightEstimaterNode(const rclcpp::NodeOptions & options)
 : Node("traffic_light_estimater", options)
 {
@@ -84,10 +137,10 @@ void TrafficLightEstimaterNode::onRoute(const HADMapRoute::ConstSharedPtr msg)
 
   for (const auto & route_lanelet : route_lanelets) {
     constexpr int PEDESTRIAN_GRAPH_ID = 1;
-    const auto conflicting_crosswalks =
+    const auto conflict_lls =
       overall_graphs_ptr_->conflictingInGraph(route_lanelet, PEDESTRIAN_GRAPH_ID);
-    for (const auto & conflicting_crosswalk : conflicting_crosswalks) {
-      conflicting_crosswalks_.insert(std::make_pair(conflicting_crosswalk, route_lanelet));
+    for (const auto & lanelet : conflict_lls) {
+      conflicting_crosswalks_.push_back(lanelet);
     }
   }
 }
@@ -102,65 +155,55 @@ void TrafficLightEstimaterNode::onTrafficLightArray(const TrafficSignalArray::Co
   }
 
   for (const auto & crosswalk : conflicting_crosswalks_) {
-    const auto & conflicting_lanelet_within_route = crosswalk.second;
-    const auto tl_reg_elems_route =
-      conflicting_lanelet_within_route.regulatoryElementsAs<const lanelet::TrafficLight>();
-    const std::string turn_direction =
-      conflicting_lanelet_within_route.attributeOr("turn_direction", "none");
-
-    std::vector<lanelet::Id> forcus_lanelet_ids;
-    if (turn_direction == "right" || turn_direction == "left") {
-      constexpr int VEHICLE_GRAPH_ID = 0;
-      const auto lanelets_1 =
-        overall_graphs_ptr_->conflictingInGraph(crosswalk.first, VEHICLE_GRAPH_ID);
-      std::vector<lanelet::Id> lanelets_1_id;
-      for (const auto & lanelet : lanelets_1) {
-        lanelets_1_id.push_back(lanelet.id());
-      }
-      const auto lanelets_2 =
-        overall_graphs_ptr_->conflictingInGraph(conflicting_lanelet_within_route, VEHICLE_GRAPH_ID);
-      std::vector<lanelet::Id> lanelets_2_id;
-      for (const auto & lanelet : lanelets_2) {
-        const auto tl_reg_elems = lanelet.regulatoryElementsAs<const lanelet::TrafficLight>();
-        if (tl_reg_elems.empty() || tl_reg_elems_route.empty()) {
-          continue;
-        }
-        if (tl_reg_elems.front()->id() != tl_reg_elems_route.front()->id()) {
-          continue;
-        }
-        lanelets_2_id.push_back(lanelet.id());
-      }
-      std::sort(lanelets_1_id.begin(), lanelets_1_id.end());
-      std::sort(lanelets_2_id.begin(), lanelets_2_id.end());
-      std::set_intersection(
-        lanelets_1_id.begin(), lanelets_1_id.end(), lanelets_2_id.begin(), lanelets_2_id.end(),
-        std::back_inserter(forcus_lanelet_ids));
-    } else {
-      forcus_lanelet_ids.push_back(conflicting_lanelet_within_route.id());
-    }
+    constexpr int VEHICLE_GRAPH_ID = 0;
+    const auto conflict_lls = overall_graphs_ptr_->conflictingInGraph(crosswalk, VEHICLE_GRAPH_ID);
 
-    bool is_green = false;
+    bool has_left_green_lane = false;
+    bool has_right_green_lane = false;
+    bool has_straight_green_lane = false;
+    bool has_straight_lane = false;
 
-    for (const auto & forcus_lanelet_id : forcus_lanelet_ids) {
-      const auto forcus_lanelet = lanelet_map_ptr_->laneletLayer.get(forcus_lanelet_id);
+    lanelet::ConstLanelets green_lanes;
+    for (const auto & lanelet : conflict_lls) {
       const auto tl_reg_elems_for_vehicle =
-        forcus_lanelet.regulatoryElementsAs<const lanelet::TrafficLight>();
+        lanelet.regulatoryElementsAs<const lanelet::TrafficLight>();
+
+      bool is_green = false;
       for (const auto & tl_reg_elem_for_vehicle : tl_reg_elems_for_vehicle) {
         const auto traffic_lights_for_vehicle = tl_reg_elem_for_vehicle->trafficLights();
 
         const auto highest_traffic_signal =
           getHighestConfidenceTrafficSignal(traffic_lights_for_vehicle, traffic_light_id_map);
 
-        is_green = highest_traffic_signal == TrafficLight::GREEN;
+        if (highest_traffic_signal == TrafficLight::GREEN) {
+          is_green = true;
+          green_lanes.push_back(lanelet);
+          break;
+        }
       }
 
-      if (!is_green) {
-        break;
+      const std::string turn_direction = lanelet.attributeOr("turn_direction", "none");
+      if (turn_direction == "left") {
+        has_left_green_lane = has_left_green_lane || is_green;
+      } else if (turn_direction == "right") {
+        has_right_green_lane = has_right_green_lane || is_green;
+      } else {
+        has_straight_green_lane = has_straight_green_lane || is_green;
+        has_straight_lane = true;
       }
     }
 
+    const auto has_merge_lane = hasMergeLane(green_lanes, routing_graph_ptr_);
+
+    bool is_red_crosswalk = false;
+    if (has_straight_lane) {
+      is_red_crosswalk = has_straight_green_lane;
+    } else {
+      is_red_crosswalk = !has_merge_lane && has_left_green_lane && has_right_green_lane;
+    }
+
     const auto tl_reg_elems_for_pedestrian =
-      crosswalk.first.regulatoryElementsAs<const lanelet::TrafficLight>();
+      crosswalk.regulatoryElementsAs<const lanelet::TrafficLight>();
     for (const auto & tl_reg_elem_for_pedestrian : tl_reg_elems_for_pedestrian) {
       const auto traffic_lights_for_pedestrian = tl_reg_elem_for_pedestrian->trafficLights();
 
@@ -169,7 +212,7 @@ void TrafficLightEstimaterNode::onTrafficLightArray(const TrafficSignalArray::Co
 
         TrafficSignal output_traffic_signal;
         TrafficLight output_traffic_light;
-        output_traffic_light.color = is_green ? TrafficLight::RED : TrafficLight::UNKNOWN;
+        output_traffic_light.color = is_red_crosswalk ? TrafficLight::RED : TrafficLight::UNKNOWN;
         output_traffic_light.confidence = 1.0;
         output_traffic_signal.lights.push_back(output_traffic_light);
         output_traffic_signal.map_primitive_id = ll_traffic_light.id();