feat(map_loader): visualize BusStopArea

Signed-off-by: Mamoru Sobue <[email protected]>
autowarefoundation · Sep 4, 2024 · 49793df · 49793df
1 parent 0efb315
commit 49793df
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 0 deletions.
diff --git a/map/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp b/map/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp
@@ -30,7 +30,11 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::str
   lanelet::ErrorMessages errors{};
   lanelet::projection::MGRSProjector projector{};
   const lanelet::LaneletMapPtr map = lanelet::load(filename, projector, &errors);
+
   if (!errors.empty()) {
+    for (const auto & error : errors) {
+      std::cout << error << std::endl;
+    }
     throw std::runtime_error("Error occurred while loading lanelet2 map");
   }
 

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
@@ -130,6 +130,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 +157,7 @@ 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;
   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 +182,7 @@ 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);
 
   visualization_msgs::msg::MarkerArray map_marker_array;
 
@@ -286,6 +290,10 @@ 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));
+
   pub_marker_->publish(map_marker_array);
 }