Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Merged
merged 3 commits into from
Jun 5, 2022
Merged

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

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
7940672
fix(lanelet2_extension): fix low FPS when visualizing center_line_arr…
h-ohta Jun 3, 2022
e0f6905
fix: fix size
h-ohta Jun 3, 2022
fa44cd7
feat: change arrow size
h-ohta Jun 3, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 17 additions & 5 deletions map/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,14 +84,26 @@ 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]
* @param lss [thickness 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, const float lss = 0.1);

/**
* [initTrafficLightTriangleMarker initializes marker to visualize shape of traffic
Expand Down
96 changes: 59 additions & 37 deletions map/lanelet2_extension/lib/visualization.cpp
View file
Open in desktop
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, lss);
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, const float lss)
{
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,26 @@ 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 = lss * std::sin(heading);
const float cos_offset = lss * std::cos(heading);

geometry_msgs::msg::Point p;
p.x = (*i).x() + sin_offset;
p.y = (*i).y() - cos_offset;
p.z = (*i).z();
marker->points.push_back(p);
p.x = (*i).x() - sin_offset;
p.y = (*i).y() + cos_offset;
p.z = (*i).z();
marker->points.push_back(p);
p.x = (*i).x() + cos_offset;
p.y = (*i).y() + sin_offset;
p.z = (*i).z();
marker->points.push_back(p);
marker->colors.push_back(c);
}
}

Expand Down