feat(map loader): visualize bus stop area and bicycle_lane (#8777)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

map/map_loader/package.xml

@@ -11,6 +11,7 @@
   <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
   <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
   <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>
 
   <license>Apache License 2.0</license>
   <author email="ryohsuke.mitsudome@tier4.jp">Ryohsuke Mitsudome</author>

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);
@@ -130,6 +132,8 @@ void Lanelet2MapVisualizationNode::on_map_bin(
   std::vector<lanelet::NoParkingAreaConstPtr> no_parking_reg_elems =
     lanelet::utils::query::noParkingAreas(all_lanelets);
   lanelet::ConstLineStrings3d curbstones = lanelet::utils::query::curbstones(viz_lanelet_map);
+  std::vector<lanelet::BusStopAreaConstPtr> bus_stop_reg_elems =
+    lanelet::utils::query::busStopAreas(all_lanelets);
 
   std_msgs::msg::ColorRGBA cl_road;
   std_msgs::msg::ColorRGBA cl_shoulder;
@@ -155,6 +159,8 @@ void Lanelet2MapVisualizationNode::on_map_bin(
   std_msgs::msg::ColorRGBA cl_no_parking_areas;
   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);
@@ -179,6 +185,8 @@ void Lanelet2MapVisualizationNode::on_map_bin(
   set_color(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5);
   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;
 
@@ -286,6 +294,24 @@ void Lanelet2MapVisualizationNode::on_map_bin(
     &map_marker_array, lanelet::visualization::intersectionAreaAsMarkerArray(
                          intersection_areas, cl_intersection_area));
 
+  insert_marker_array(
+    &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);
 }