feat(map_loader): visualize bicycle_lane

Signed-off-by: Mamoru Sobue <[email protected]>
autowarefoundation · Sep 4, 2024 · 76c57b8 · 76c57b8
1 parent 49793df
commit 76c57b8
Showing 1 changed file with 18 additions and 0 deletions.
diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp
@@ -95,6 +95,8 @@ void Lanelet2MapVisualizationNode::on_map_bin(
   lanelet::ConstLanelets shoulder_lanelets = lanelet::utils::query::shoulderLanelets(all_lanelets);
   lanelet::ConstLanelets crosswalk_lanelets =
     lanelet::utils::query::crosswalkLanelets(all_lanelets);
+  lanelet::ConstLanelets bicycle_lane_lanelets =
+    lanelet::utils::query::bicycleLaneLanelets(all_lanelets);
   lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(viz_lanelet_map);
   lanelet::ConstLineStrings3d pedestrian_polygon_markings =
     lanelet::utils::query::getAllPedestrianPolygonMarkings(viz_lanelet_map);
@@ -158,6 +160,7 @@ void Lanelet2MapVisualizationNode::on_map_bin(
   std_msgs::msg::ColorRGBA cl_curbstones;
   std_msgs::msg::ColorRGBA cl_intersection_area;
   std_msgs::msg::ColorRGBA cl_bus_stop_area;
+  std_msgs::msg::ColorRGBA cl_bicycle_lane;
   set_color(&cl_road, 0.27, 0.27, 0.27, 0.999);
   set_color(&cl_shoulder, 0.15, 0.15, 0.15, 0.999);
   set_color(&cl_cross, 0.27, 0.3, 0.27, 0.5);
@@ -183,6 +186,7 @@ void Lanelet2MapVisualizationNode::on_map_bin(
   set_color(&cl_curbstones, 0.1, 0.1, 0.2, 0.999);
   set_color(&cl_intersection_area, 0.16, 1.0, 0.69, 0.5);
   set_color(&cl_bus_stop_area, 0.863, 0.863, 0.863, 0.5);
+  set_color(&cl_bicycle_lane, 0.0, 0.3843, 0.6274, 0.5);
 
   visualization_msgs::msg::MarkerArray map_marker_array;
 
@@ -294,6 +298,20 @@ void Lanelet2MapVisualizationNode::on_map_bin(
     &map_marker_array,
     lanelet::visualization::busStopAreasAsMarkerArray(bus_stop_reg_elems, cl_bus_stop_area));
 
+  insert_marker_array(
+    &map_marker_array,
+    lanelet::visualization::laneletDirectionAsMarkerArray(bicycle_lane_lanelets, "bicycle_lane_"));
+  insert_marker_array(
+    &map_marker_array, lanelet::visualization::laneletsBoundaryAsMarkerArray(
+                         bicycle_lane_lanelets, cl_ll_borders /* use ll_border color */,
+                         viz_lanelets_centerline_, "bicycle_lane_"));
+  insert_marker_array(
+    &map_marker_array, lanelet::visualization::generateLaneletIdMarker(
+                         bicycle_lane_lanelets, cl_lanelet_id /* use lanelet_id color */));
+  insert_marker_array(
+    &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray(
+                         "bicycle_lane_lanelets", bicycle_lane_lanelets, cl_bicycle_lane));
+
   pub_marker_->publish(map_marker_array);
 }