From cebfe7ba8b278bdad7f78e17cf8fd32740ec3b0a Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 9 Jan 2024 13:36:23 +0900 Subject: [PATCH] fix(lanelet2_extension): show exact relation Signed-off-by: satoshi-ota --- .../visualization/visualization.hpp | 7 ++- tmp/lanelet2_extension/lib/visualization.cpp | 43 +++++++------------ 2 files changed, 18 insertions(+), 32 deletions(-) diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp index ab862b7a..bd7e46a8 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp +++ b/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 & 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 diff --git a/tmp/lanelet2_extension/lib/visualization.cpp b/tmp/lanelet2_extension/lib/visualization.cpp index 91d3244a..e6b823da 100644 --- a/tmp/lanelet2_extension/lib/visualization.cpp +++ b/tmp/lanelet2_extension/lib/visualization.cpp @@ -539,12 +539,12 @@ visualization_msgs::msg::MarkerArray visualization::autowareTrafficLightsAsMarke } 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) + 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) { + for (const auto & lanelet : lanelets) { visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; marker.header.stamp = rclcpp::Time(); @@ -559,32 +559,19 @@ visualization_msgs::msg::MarkerArray visualization::generateTrafficLightIdMaker( marker.color = c; marker.scale.z = scale; marker.frame_locked = false; - std::ostringstream string_stream; - string_stream << "TLRegElemId:" << std::to_string(tl->id()); - marker.text = string_stream.str(); - - const auto stop_line = tl->stopLine(); - if (stop_line.has_value()) { - marker.id = static_cast(tl->id()); - marker.pose.position.x = (stop_line.value().front().x() + stop_line.value().back().x()) / 2; - marker.pose.position.y = (stop_line.value().front().y() + stop_line.value().back().y()) / 2; - marker.pose.position.z = stop_line.value().front().z(); - tl_id_marker_array.markers.push_back(marker); - continue; - } - 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(lsp); - - marker.id = static_cast(ls.id()); - 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; - tl_id_marker_array.markers.push_back(marker); - } + for (const auto & element : lanelet.regulatoryElementsAs()) { + std::ostringstream string_stream; + string_stream << "TLRegElemId:" << std::to_string(element->id()); + marker.text = string_stream.str(); + + marker.id = static_cast(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); } }