From e3b49b966b1d0981cd4e934aa570d7c332c26152 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Sun, 5 Jun 2022 22:09:15 +0900 Subject: [PATCH] fix(lanelet2_extension): fix low FPS when visualizing center_line_arrows at a large map (#1032) * fix(lanelet2_extension): fix low FPS when visualizing center_line_arrows at a large map Signed-off-by: h-ohta * fix: fix size * feat: change arrow size --- .../visualization/visualization.hpp | 21 +++- map/lanelet2_extension/lib/visualization.cpp | 98 ++++++++++++------- 2 files changed, 77 insertions(+), 42 deletions(-) diff --git a/map/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp b/map/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp index 026a78ea1521c..682f2c3d2c35b 100644 --- a/map/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp +++ b/map/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp @@ -84,14 +84,25 @@ void pushLineStringMarker( const std_msgs::msg::ColorRGBA c, const float lss = 0.1); /** - * [pushArrowMarkerArray pushes marker to visualize arrows] - * @param marker_array [output marker array message] + * [initArrowsMarker initializes marker to visualize arrows with TRIANGLE_LIST] + * @param marker [output marker message] + * @param frame_id [frame id of the marker] + * @param ns [namespace of the marker] + * @param c [color of the marker] + */ +void initArrowsMarker( + visualization_msgs::msg::Marker * marker, const std::string frame_id, const std::string ns, + const std_msgs::msg::ColorRGBA c); + +/** + * [pushArrowsMarker pushes marker to visualize arrows with TRIANGLE_LIST] + * @param marker [output marker message] * @param ls [input linestring] * @param c [color of the marker] */ -void pushArrowMarkerArray( - visualization_msgs::msg::MarkerArray * marker_array, const lanelet::ConstLineString3d & ls, - const std::string frame_id, const std::string ns, const std_msgs::msg::ColorRGBA c); +void pushArrowsMarker( + visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls, + const std_msgs::msg::ColorRGBA c); /** * [initTrafficLightTriangleMarker initializes marker to visualize shape of traffic diff --git a/map/lanelet2_extension/lib/visualization.cpp b/map/lanelet2_extension/lib/visualization.cpp index b846382c99b87..da7fe490a32cc 100644 --- a/map/lanelet2_extension/lib/visualization.cpp +++ b/map/lanelet2_extension/lib/visualization.cpp @@ -881,8 +881,7 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra std::unordered_set added; visualization_msgs::msg::Marker left_line_strip, right_line_strip, start_bound_line_strip, - center_line_strip; - visualization_msgs::msg::MarkerArray center_line_arrows; + center_line_strip, center_arrows; visualization::initLineStringMarker( &left_line_strip, "map", additional_namespace + "left_lane_bound", c); visualization::initLineStringMarker( @@ -891,6 +890,8 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra &start_bound_line_strip, "map", additional_namespace + "lane_start_bound", c); visualization::initLineStringMarker( ¢er_line_strip, "map", additional_namespace + "center_lane_line", c); + visualization::initArrowsMarker( + ¢er_arrows, "map", additional_namespace + "center_line_arrows", c); for (auto li = lanelets.begin(); li != lanelets.end(); li++) { lanelet::ConstLanelet lll = *li; @@ -918,8 +919,7 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra } if (viz_centerline && !exists(added, center_ls.id())) { visualization::pushLineStringMarker(¢er_line_strip, center_ls, c, lss_center); - visualization::pushArrowMarkerArray( - ¢er_line_arrows, center_ls, "map", additional_namespace + "center_line_arrows", c); + visualization::pushArrowsMarker(¢er_arrows, center_ls, c); added.insert(center_ls.id()); } } @@ -937,9 +937,9 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra if (!start_bound_line_strip.points.empty()) { marker_array.markers.push_back(start_bound_line_strip); } - marker_array.markers.insert( - marker_array.markers.end(), center_line_arrows.markers.begin(), - center_line_arrows.markers.end()); + if (!center_arrows.points.empty()) { + marker_array.markers.push_back(center_arrows); + } return marker_array; } @@ -1191,14 +1191,43 @@ void visualization::pushLineStringMarker( } } -void visualization::pushArrowMarkerArray( - visualization_msgs::msg::MarkerArray * marker_array, const lanelet::ConstLineString3d & ls, - const std::string frame_id, const std::string ns, const std_msgs::msg::ColorRGBA c) +void visualization::initArrowsMarker( + visualization_msgs::msg::Marker * marker, const std::string frame_id, const std::string ns, + const std_msgs::msg::ColorRGBA c) +{ + if (marker == nullptr) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("lanelet2_extension.visualization"), + __FUNCTION__ << ": marker is null pointer!"); + return; + } + + marker->header.frame_id = frame_id; + marker->header.stamp = rclcpp::Time(); + marker->frame_locked = true; + marker->ns = ns; + marker->action = visualization_msgs::msg::Marker::ADD; + marker->type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + + marker->id = 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->scale.x = 1.0; + marker->scale.y = 1.0; + marker->scale.z = 1.0; + marker->color = c; +} + +void visualization::pushArrowsMarker( + visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls, + const std_msgs::msg::ColorRGBA c) { - if (marker_array == nullptr) { + if (marker == nullptr) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("lanelet2_extension.visualization"), - __FUNCTION__ << ": marker_array is null pointer!"); + __FUNCTION__ << ": marker is null pointer!"); return; } @@ -1209,33 +1238,28 @@ void visualization::pushArrowMarkerArray( __FUNCTION__ << ": marker line size is 1 or 0!"); return; } - - visualization_msgs::msg::Marker marker; - marker.header.frame_id = frame_id; - marker.header.stamp = rclcpp::Time(); - marker.frame_locked = true; - marker.ns = ns; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.type = visualization_msgs::msg::Marker::ARROW; - - int32_t start_index = !marker_array->markers.empty() ? marker_array->markers.back().id : 0; for (auto i = ls.begin(); i + 1 != ls.end(); i++) { - const auto index = i - ls.begin(); - marker.id = start_index + index + 1; - const auto curr_point = - geometry_msgs::build().x((*i).x()).y((*i).y()).z((*i).z()); - const auto next_point = geometry_msgs::build() - .x((*(i + 1)).x()) - .y((*(i + 1)).y()) - .z((*(i + 1)).z()); - marker.pose.position = curr_point; - - const double yaw = tier4_autoware_utils::calcAzimuthAngle(curr_point, next_point); - marker.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); - marker.scale = tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1); - marker.color = c; + const float heading = std::atan2((*(i + 1)).y() - (*i).y(), (*(i + 1)).x() - (*i).x()); - marker_array->markers.push_back(marker); + const float sin_offset = std::sin(heading); + const float cos_offset = std::cos(heading); + const double width = 0.3; + const double height = 1.0; + + geometry_msgs::msg::Point p; + p.x = (*i).x() + sin_offset * width; + p.y = (*i).y() - cos_offset * width; + p.z = (*i).z(); + marker->points.push_back(p); + p.x = (*i).x() - sin_offset * width; + p.y = (*i).y() + cos_offset * width; + p.z = (*i).z(); + marker->points.push_back(p); + p.x = (*i).x() + cos_offset * height; + p.y = (*i).y() + sin_offset * height; + p.z = (*i).z(); + marker->points.push_back(p); + marker->colors.push_back(c); } }