fix(traffic_light_estimater): support new format of the regulatory el…

…ement

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

perception/traffic_light_estimater/include/traffic_light_estimater/node.hpp

@@ -17,6 +17,7 @@
 
 #include <lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp>
 #include <lanelet2_extension/utility/query.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
@@ -28,6 +29,7 @@
 #include <lanelet2_core/Attribute.h>
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_routing/RoutingGraph.h>
+#include <lanelet2_routing/RoutingGraphContainer.h>
 #include <lanelet2_traffic_rules/TrafficRulesFactory.h>
 
 #include <memory>
@@ -57,8 +59,9 @@ class TrafficLightEstimaterNode : public rclcpp::Node
   lanelet::LaneletMapPtr lanelet_map_ptr_;
   lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_;
   lanelet::routing::RoutingGraphPtr routing_graph_ptr_;
+  std::shared_ptr<const lanelet::routing::RoutingGraphContainer> overall_graphs_ptr_;
 
-  std::vector<lanelet::AutowareTrafficLightConstPtr> route_lanelet_traffic_lights_;
+  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)
 {
@@ -47,6 +100,17 @@ void TrafficLightEstimaterNode::onMap(const HADMapBin::ConstSharedPtr msg)
   lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
   lanelet::utils::conversion::fromBinMsg(
     *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_);
+  const auto traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create(
+    lanelet::Locations::Germany, lanelet::Participants::Vehicle);
+  const auto pedestrian_rules = lanelet::traffic_rules::TrafficRulesFactory::create(
+    lanelet::Locations::Germany, lanelet::Participants::Pedestrian);
+  lanelet::routing::RoutingGraphConstPtr vehicle_graph =
+    lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *traffic_rules);
+  lanelet::routing::RoutingGraphConstPtr pedestrian_graph =
+    lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *pedestrian_rules);
+  lanelet::routing::RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph});
+  overall_graphs_ptr_ =
+    std::make_shared<const lanelet::routing::RoutingGraphContainer>(overall_graphs);
   RCLCPP_INFO(get_logger(), "[TrafficLightEstimaterNode]: Map is loaded");
 }
 
@@ -56,6 +120,7 @@ void TrafficLightEstimaterNode::onRoute(const HADMapRoute::ConstSharedPtr msg)
     RCLCPP_WARN(get_logger(), "cannot set traffic light in route because don't receive map");
     return;
   }
+
   lanelet::ConstLanelets route_lanelets;
   for (const auto & segment : msg->segments) {
     for (const auto & primitive : segment.primitives) {
@@ -67,7 +132,17 @@ void TrafficLightEstimaterNode::onRoute(const HADMapRoute::ConstSharedPtr msg)
       }
     }
   }
-  route_lanelet_traffic_lights_ = lanelet::utils::query::autowareTrafficLights(route_lanelets);
+
+  conflicting_crosswalks_.clear();
+
+  for (const auto & route_lanelet : route_lanelets) {
+    constexpr int PEDESTRIAN_GRAPH_ID = 1;
+    const auto conflict_lls =
+      overall_graphs_ptr_->conflictingInGraph(route_lanelet, PEDESTRIAN_GRAPH_ID);
+    for (const auto & lanelet : conflict_lls) {
+      conflicting_crosswalks_.push_back(lanelet);
+    }
+  }
 }
 
 void TrafficLightEstimaterNode::onTrafficLightArray(const TrafficSignalArray::ConstSharedPtr msg)
@@ -79,35 +154,70 @@ void TrafficLightEstimaterNode::onTrafficLightArray(const TrafficSignalArray::Co
     traffic_light_id_map[traffic_signal.map_primitive_id] = traffic_signal;
   }
 
-  if (route_lanelet_traffic_lights_.empty()) {
-    pub_traffic_light_array_->publish(output);
-    return;
-  }
+  for (const auto & crosswalk : conflicting_crosswalks_) {
+    constexpr int VEHICLE_GRAPH_ID = 0;
+    const auto conflict_lls = overall_graphs_ptr_->conflictingInGraph(crosswalk, VEHICLE_GRAPH_ID);
 
-  for (const auto & traffic_lights_reg_elem : route_lanelet_traffic_lights_) {
-    lanelet::ConstLineStringsOrPolygons3d traffic_lights = traffic_lights_reg_elem->trafficLights();
-    const auto highest_traffic_signal =
-      getHighestConfidenceTrafficSignal(traffic_lights, traffic_light_id_map);
+    bool has_left_green_lane = false;
+    bool has_right_green_lane = false;
+    bool has_straight_green_lane = false;
+    bool has_straight_lane = false;
 
-    const auto is_green = highest_traffic_signal == TrafficLight::GREEN ||
-                          highest_traffic_signal == TrafficLight::RIGHT_ARROW ||
-                          highest_traffic_signal == TrafficLight::LEFT_ARROW ||
-                          highest_traffic_signal == TrafficLight::UP_ARROW;
+    lanelet::ConstLanelets green_lanes;
+    for (const auto & lanelet : conflict_lls) {
+      const auto tl_reg_elems_for_vehicle =
+        lanelet.regulatoryElementsAs<const lanelet::TrafficLight>();
 
-    for (const auto & traffic_light : traffic_lights) {
-      const auto ll_traffic_light = static_cast<lanelet::ConstLineString3d>(traffic_light);
-      const std::string type = ll_traffic_light.attributeOr(lanelet::AttributeName::Type, "none");
-      if (type.compare("traffic_light_crosswalk") != 0) {
-        continue;
+      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);
+
+        if (highest_traffic_signal == TrafficLight::GREEN) {
+          is_green = true;
+          green_lanes.push_back(lanelet);
+          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_);
 
-      TrafficSignal output_traffic_signal;
-      TrafficLight output_traffic_light;
-      output_traffic_light.color = is_green ? 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();
-      output.signals.push_back(output_traffic_signal);
+    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.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();
+
+      for (const auto & traffic_light : traffic_lights_for_pedestrian) {
+        const auto ll_traffic_light = static_cast<lanelet::ConstLineString3d>(traffic_light);
+
+        TrafficSignal output_traffic_signal;
+        TrafficLight output_traffic_light;
+        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();
+        output.signals.push_back(output_traffic_signal);
+      }
     }
   }