From c6aacaa8d1f7081ba9c93a64dfd7fef2a882e79f Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 5 Jan 2024 10:52:24 +0900 Subject: [PATCH 1/5] fix(lanelet2_extension): improve traffic regulatory element id visualization Signed-off-by: satoshi-ota --- tmp/lanelet2_extension/lib/visualization.cpp | 43 +++++++++++++------- 1 file changed, 28 insertions(+), 15 deletions(-) diff --git a/tmp/lanelet2_extension/lib/visualization.cpp b/tmp/lanelet2_extension/lib/visualization.cpp index 95301cef..91d3244a 100644 --- a/tmp/lanelet2_extension/lib/visualization.cpp +++ b/tmp/lanelet2_extension/lib/visualization.cpp @@ -545,31 +545,44 @@ visualization_msgs::msg::MarkerArray visualization::generateTrafficLightIdMaker( visualization_msgs::msg::MarkerArray tl_id_marker_array; for (const auto & tl : tl_reg_elems) { + 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; + 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); - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = rclcpp::Time(); - marker.ns = "traffic_light_id"; marker.id = static_cast(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); } } From cebfe7ba8b278bdad7f78e17cf8fd32740ec3b0a Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 9 Jan 2024 13:36:23 +0900 Subject: [PATCH 2/5] 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); } } From 3e26cdcca1f9e23f4d8da221d34c574a43a24be1 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 9 Jan 2024 18:52:11 +0900 Subject: [PATCH 3/5] 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) From ccc3293e1d7106db2f7cb76221029dc3d6b74ca8 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 9 Jan 2024 19:00:54 +0900 Subject: [PATCH 4/5] fix: typo Signed-off-by: satoshi-ota --- tmp/lanelet2_extension/lib/visualization.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmp/lanelet2_extension/lib/visualization.cpp b/tmp/lanelet2_extension/lib/visualization.cpp index 6efe0653..3a955532 100644 --- a/tmp/lanelet2_extension/lib/visualization.cpp +++ b/tmp/lanelet2_extension/lib/visualization.cpp @@ -612,7 +612,7 @@ visualization_msgs::msg::MarkerArray visualization::generateTrafficLightIdMaker( marker.frame_locked = false; std::ostringstream string_stream; - string_stream << "Refferer:" << element->id() << ", "; + string_stream << "referrer:" << element->id() << ", "; marker.text = string_stream.str(); traffic_light_map.emplace(line.id(), marker); } else { From 15782bdf0e07068c96ae0b1a1a07ef09c5d70f43 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Wed, 10 Jan 2024 08:38:46 +0900 Subject: [PATCH 5/5] fix(lanelet2_extension): remove whitespace Signed-off-by: satoshi-ota --- tmp/lanelet2_extension/lib/visualization.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tmp/lanelet2_extension/lib/visualization.cpp b/tmp/lanelet2_extension/lib/visualization.cpp index 3a955532..26ba5d8b 100644 --- a/tmp/lanelet2_extension/lib/visualization.cpp +++ b/tmp/lanelet2_extension/lib/visualization.cpp @@ -612,12 +612,12 @@ visualization_msgs::msg::MarkerArray visualization::generateTrafficLightIdMaker( marker.frame_locked = false; std::ostringstream string_stream; - string_stream << "referrer:" << element->id() << ", "; + 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() << ", "; + string_stream << element->id() << ","; traffic_light_map.at(line.id()).text += string_stream.str(); } }