Skip to content

Commit

Permalink
fix(lanelet2_extension): fix low FPS when visualizing center_line_arr…
Browse files Browse the repository at this point in the history 
…ows at a large map (tier4#1032)

* fix(lanelet2_extension): fix low FPS when visualizing center_line_arrows at a large map

Signed-off-by: h-ohta <hiroki.ota@tier4.jp>

* fix: fix size

* feat: change arrow size
  • Loading branch information
@h-ohta @boyali
h-ohta authored and boyali committed Oct 3, 2022
1 parent 733588a commit a9da3eb
Show file tree
Hide file tree
Showing 2 changed files with 77 additions and 42 deletions.
21 changes: 16 additions & 5 deletions map/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
98 changes: 61 additions & 37 deletions map/lanelet2_extension/lib/visualization.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -881,8 +881,7 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra

std::unordered_set<lanelet::Id> 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(
Expand All @@ -891,6 +890,8 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra
&start_bound_line_strip, "map", additional_namespace + "lane_start_bound", c);
visualization::initLineStringMarker(
&center_line_strip, "map", additional_namespace + "center_lane_line", c);
visualization::initArrowsMarker(
&center_arrows, "map", additional_namespace + "center_line_arrows", c);

for (auto li = lanelets.begin(); li != lanelets.end(); li++) {
lanelet::ConstLanelet lll = *li;
Expand Down Expand Up @@ -918,8 +919,7 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra
}
if (viz_centerline && !exists(added, center_ls.id())) {
visualization::pushLineStringMarker(&center_line_strip, center_ls, c, lss_center);
visualization::pushArrowMarkerArray(
&center_line_arrows, center_ls, "map", additional_namespace + "center_line_arrows", c);
visualization::pushArrowsMarker(&center_arrows, center_ls, c);
added.insert(center_ls.id());
}
}
Expand All @@ -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;
}

Expand Down Expand Up @@ -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;
}

Expand All @@ -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<geometry_msgs::msg::Point>().x((*i).x()).y((*i).y()).z((*i).z());
const auto next_point = geometry_msgs::build<geometry_msgs::msg::Point>()
.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);
}
}

Expand Down

0 comments on commit a9da3eb

Please sign in to comment.