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

feat: visualize lane boundaries #923

Merged
merged 3 commits into from
May 19, 2022
Merged

feat: visualize lane boundaries #923

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
7a70a3d
feat: visualize lane boundaries
h-ohta May 18, 2022
bab2021
fix: start_bound
h-ohta May 18, 2022
f85c6a8
ci(pre-commit): autofix
pre-commit-ci[bot] May 18, 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
17 changes: 16 additions & 1 deletion map/lanelet2_extension/lib/visualization.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -878,11 +878,14 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra
double lss_center = std::max(lss * 0.1, 0.02);

std::unordered_set<lanelet::Id> added;
visualization_msgs::msg::Marker left_line_strip, right_line_strip, center_line_strip;
visualization_msgs::msg::Marker left_line_strip, right_line_strip, start_bound_line_strip,
center_line_strip;
visualization::initLineStringMarker(
&left_line_strip, "map", additional_namespace + "left_lane_bound", c);
visualization::initLineStringMarker(
&right_line_strip, "map", additional_namespace + "right_lane_bound", c);
visualization::initLineStringMarker(
&start_bound_line_strip, "map", additional_namespace + "lane_start_bound", c);
visualization::initLineStringMarker(
&center_line_strip, "map", additional_namespace + "center_lane_line", c);

Expand All @@ -892,6 +895,11 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra
lanelet::ConstLineString3d left_ls = lll.leftBound();
lanelet::ConstLineString3d right_ls = lll.rightBound();
lanelet::ConstLineString3d center_ls = lll.centerline();
lanelet::LineString3d start_bound_ls(lanelet::utils::getId());
start_bound_ls.push_back(lanelet::Point3d(
lanelet::utils::getId(), left_ls.front().x(), left_ls.front().y(), left_ls.front().z()));
start_bound_ls.push_back(lanelet::Point3d(
lanelet::utils::getId(), right_ls.front().x(), right_ls.front().y(), right_ls.front().z()));

if (!exists(added, left_ls.id())) {
visualization::pushLineStringMarker(&left_line_strip, left_ls, c, lss);
Expand All @@ -901,6 +909,10 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra
visualization::pushLineStringMarker(&right_line_strip, right_ls, c, lss);
added.insert(right_ls.id());
}
if (!exists(added, start_bound_ls.id())) {
visualization::pushLineStringMarker(&start_bound_line_strip, start_bound_ls, c, lss);
added.insert(start_bound_ls.id());
}
if (viz_centerline && !exists(added, center_ls.id())) {
visualization::pushLineStringMarker(&center_line_strip, center_ls, c, lss_center);
added.insert(center_ls.id());
Expand All @@ -917,6 +929,9 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra
if (!center_line_strip.points.empty()) {
marker_array.markers.push_back(center_line_strip);
}
if (!start_bound_line_strip.points.empty()) {
marker_array.markers.push_back(start_bound_line_strip);
}
return marker_array;
}

Expand Down