fix(lanelet2_extension): improve traffic regulatory element id visualization

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

@@ -189,15 +189,14 @@ visualization_msgs::msg::MarkerArray autowareTrafficLightsAsMarkerArray(
 /**
  * [generateTrafficLightIdMaker creates marker array to visualize traffic id
  * lights]
- * @param  tl_reg_elems [traffic light regulatory elements]
+ * @param lanelets      [lanelets]
  * @param  c            [color of the marker]
  * @param  duration     [lifetime of the marker]
  * @return              [created marker array]
  */
 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 lanelet::ConstLanelets & lanelets, const std_msgs::msg::ColorRGBA & c,
+  const rclcpp::Duration & duration = rclcpp::Duration(0, 0), const double scale = 1.0);
 
 /**
  * [trafficLightsAsTriangleMarkerArray creates marker array to visualize shape

tmp/lanelet2_extension/lib/visualization.cpp

@@ -539,39 +539,39 @@ visualization_msgs::msg::MarkerArray visualization::autowareTrafficLightsAsMarke
 }
 
 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)
+  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 & 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);
+  for (const auto & lanelet : lanelets) {
+    visualization_msgs::msg::Marker marker;
+    marker.header.frame_id = "map";
+    marker.header.stamp = rclcpp::Time();
+    marker.ns = "traffic_light_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;
 
-        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.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.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);
-      }
+    for (const auto & element : lanelet.regulatoryElementsAs<lanelet::TrafficLight>()) {
+      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);
     }
   }