From cd69d732129efea2b69156ab11b9e5de97db07b0 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Thu, 19 May 2022 10:17:06 +0900 Subject: [PATCH] feat: visualize lane boundaries (#923) * feat: visualize lane boundaries * fix: start_bound * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Signed-off-by: Shumpei Wakabayashi --- map/lanelet2_extension/lib/visualization.cpp | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/map/lanelet2_extension/lib/visualization.cpp b/map/lanelet2_extension/lib/visualization.cpp index fc9f068669d96..a95a9aa1e49e0 100644 --- a/map/lanelet2_extension/lib/visualization.cpp +++ b/map/lanelet2_extension/lib/visualization.cpp @@ -878,11 +878,14 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra double lss_center = std::max(lss * 0.1, 0.02); std::unordered_set 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( ¢er_line_strip, "map", additional_namespace + "center_lane_line", c); @@ -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); @@ -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(¢er_line_strip, center_ls, c, lss_center); added.insert(center_ls.id()); @@ -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; }