fix(lanelet2_extension): improve traffic regulatory element id visualization

tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp

@@ -187,8 +187,20 @@ visualization_msgs::msg::MarkerArray autowareTrafficLightsAsMarkerArray(
   const double scale = 1.0);
 
 /**
- * [generateTrafficLightIdMaker creates marker array to visualize traffic id
- * lights]
+ * [generateTrafficLightRegulatoryElementIdMaker creates marker array to visualize traffic
+ * light regulatory element ids]
+ * @param lanelets      [lanelets]
+ * @param  c            [color of the marker]
+ * @param  duration     [lifetime of the marker]
+ * @return              [created marker array]
+ */
+visualization_msgs::msg::MarkerArray generateTrafficLightRegulatoryElementIdMaker(
+  const lanelet::ConstLanelets & lanelets, const std_msgs::msg::ColorRGBA & c,
+  const rclcpp::Duration & duration = rclcpp::Duration(0, 0), const double scale = 0.5);
+
+/**
+ * [generateTrafficLightIdMarkerArray creates marker array to visualize traffic
+ * light ids]
  * @param  tl_reg_elems [traffic light regulatory elements]
  * @param  c            [color of the marker]
  * @param  duration     [lifetime of the marker]
@@ -197,7 +209,7 @@ visualization_msgs::msg::MarkerArray autowareTrafficLightsAsMarkerArray(
 visualization_msgs::msg::MarkerArray generateTrafficLightIdMaker(
   const std::vector<lanelet::AutowareTrafficLightConstPtr> & tl_reg_elems,
   const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration = rclcpp::Duration(0, 0),
-  const double scale = 1.0);
+  const double scale = 0.5);
 
 /**
  * [trafficLightsAsTriangleMarkerArray creates marker array to visualize shape

tmp/lanelet2_extension/lib/visualization.cpp

@@ -538,43 +538,95 @@
   return tl_marker_array;
 }
 
+visualization_msgs::msg::MarkerArray visualization::generateTrafficLightRegulatoryElementIdMaker(
+  const lanelet::ConstLanelets & lanelets, const std_msgs::msg::ColorRGBA & c,
+  const rclcpp::Duration & duration, const double scale)
+{
+  visualization_msgs::msg::MarkerArray tl_id_marker_array;
+
+  for (const auto & lanelet : lanelets) {
+    for (const auto & element : lanelet.regulatoryElementsAs<lanelet::TrafficLight>()) {
+      visualization_msgs::msg::Marker marker;
+      marker.header.frame_id = "map";
+      marker.header.stamp = rclcpp::Time();
+      marker.ns = "traffic_light_reg_elem_id";
+      marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+      marker.lifetime = duration;
+      marker.action = visualization_msgs::msg::Marker::ADD;
+      marker.pose.orientation.x = 0.0;
+      marker.pose.orientation.y = 0.0;
+      marker.pose.orientation.z = 0.0;
+      marker.pose.orientation.w = 1.0;
+      marker.color = c;
+      marker.scale.z = scale;
+      marker.frame_locked = false;
+
+      std::ostringstream string_stream;
+      string_stream << "TLRegElemId:" << std::to_string(element->id());
+      marker.text = string_stream.str();
+
+      marker.id = static_cast<int32_t>(lanelet.id());
+      marker.pose.position.x =
+        (lanelet.rightBound().front().x() + lanelet.leftBound().front().x()) / 2;
+      marker.pose.position.y =
+        (lanelet.rightBound().front().y() + lanelet.leftBound().front().y()) / 2;
+      marker.pose.position.z = lanelet.rightBound().front().z();
+      tl_id_marker_array.markers.push_back(marker);
+    }
+  }
+
+  return tl_id_marker_array;
+}
+
 visualization_msgs::msg::MarkerArray visualization::generateTrafficLightIdMaker(
   const std::vector<lanelet::AutowareTrafficLightConstPtr> & tl_reg_elems,
   const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration, const double scale)
 {
   visualization_msgs::msg::MarkerArray tl_id_marker_array;
 
-  for (const auto & tl : tl_reg_elems) {
-    const auto lights = tl->trafficLights();
-    for (const auto & lsp : lights) {
-      if (lsp.isLineString()) {  // traffic lights can either polygons or
-                                 // linestrings
-        lanelet::ConstLineString3d ls = static_cast<lanelet::ConstLineString3d>(lsp);
-
+  std::unordered_map<lanelet::Id, visualization_msgs::msg::Marker> traffic_light_map{};
+  for (const auto & element : tl_reg_elems) {
+    for (const auto & light : element->trafficLights()) {
+      if (!light.isLineString()) {
+        continue;
+      }
+      const auto line = static_cast<lanelet::ConstLineString3d>(light);
+      if (traffic_light_map.count(line.id()) == 0) {
         visualization_msgs::msg::Marker marker;
         marker.header.frame_id = "map";
         marker.header.stamp = rclcpp::Time();
         marker.ns = "traffic_light_id";
-        marker.id = static_cast<int32_t>(ls.id());
+        marker.id = static_cast<int32_t>(line.id());
         marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
         marker.lifetime = duration;
         marker.action = visualization_msgs::msg::Marker::ADD;
-        marker.pose.position.x = (ls.front().x() + ls.back().x()) / 2;
-        marker.pose.position.y = (ls.front().y() + ls.back().y()) / 2;
-        marker.pose.position.z = ls.front().z() + 1.0;
+        marker.pose.position.x = (line.front().x() + line.back().x()) / 2;
+        marker.pose.position.y = (line.front().y() + line.back().y()) / 2;
+        marker.pose.position.z = line.front().z() + 1.0;
         marker.pose.orientation.x = 0.0;
         marker.pose.orientation.y = 0.0;
         marker.pose.orientation.z = 0.0;
         marker.pose.orientation.w = 1.0;
         marker.color = c;
         marker.scale.z = scale;
         marker.frame_locked = false;
-        marker.text = std::to_string(tl->id());
-        tl_id_marker_array.markers.push_back(marker);
+
+        std::ostringstream string_stream;
+        string_stream << "referrer:" << element->id() << ",";
+        marker.text = string_stream.str();
+        traffic_light_map.emplace(line.id(), marker);
+      } else {
+        std::ostringstream string_stream;
+        string_stream << element->id() << ",";
+        traffic_light_map.at(line.id()).text += string_stream.str();
       }
     }
   }
 
+  for (const auto & [id, marker] : traffic_light_map) {
+    tl_id_marker_array.markers.push_back(marker);
+  }
+
   return tl_id_marker_array;
 }