feat: visualize lane boundaries (autowarefoundation#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 <[email protected]>
shmpwk · May 24, 2022 · cd69d73 · cd69d73
1 parent 44c3088
commit cd69d73
Showing 1 changed file with 16 additions and 1 deletion.
diff --git 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<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);
 
@@ -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(&center_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;
 }