From 3e26cdcca1f9e23f4d8da221d34c574a43a24be1 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 9 Jan 2024 18:52:11 +0900 Subject: [PATCH] feat(lanelet2_extension): show traffic light id Signed-off-by: satoshi-ota --- .../visualization/visualization.hpp | 21 ++++- tmp/lanelet2_extension/lib/visualization.cpp | 84 +++++++++++++++---- 2 files changed, 85 insertions(+), 20 deletions(-) diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp index bd7e46a8..5cf3bf4b 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp @@ -187,16 +187,29 @@ 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 generateTrafficLightIdMaker( +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 = 1.0); + 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] + * @return [created marker array] + */ +visualization_msgs::msg::MarkerArray generateTrafficLightIdMaker( + const std::vector & tl_reg_elems, + const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration = rclcpp::Duration(0, 0), + const double scale = 0.5); /** * [trafficLightsAsTriangleMarkerArray creates marker array to visualize shape diff --git a/tmp/lanelet2_extension/lib/visualization.cpp b/tmp/lanelet2_extension/lib/visualization.cpp index e6b823da..6efe0653 100644 --- a/tmp/lanelet2_extension/lib/visualization.cpp +++ b/tmp/lanelet2_extension/lib/visualization.cpp @@ -538,29 +538,29 @@ visualization_msgs::msg::MarkerArray visualization::autowareTrafficLightsAsMarke return tl_marker_array; } -visualization_msgs::msg::MarkerArray visualization::generateTrafficLightIdMaker( +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) { - 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; - for (const auto & element : lanelet.regulatoryElementsAs()) { + 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(); @@ -578,6 +578,58 @@ visualization_msgs::msg::MarkerArray visualization::generateTrafficLightIdMaker( return tl_id_marker_array; } +visualization_msgs::msg::MarkerArray visualization::generateTrafficLightIdMaker( + const std::vector & tl_reg_elems, + const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration, const double scale) +{ + visualization_msgs::msg::MarkerArray tl_id_marker_array; + + std::unordered_map 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(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(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 = (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; + + std::ostringstream string_stream; + string_stream << "Refferer:" << 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; +} + visualization_msgs::msg::MarkerArray visualization::detectionAreasAsMarkerArray( const std::vector & da_reg_elems, const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration)