From 49793dfe7608078af330c43016fb760a8d931324 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 4 Sep 2024 18:37:49 +0900 Subject: [PATCH] feat(map_loader): visualize BusStopArea Signed-off-by: Mamoru Sobue --- .../src/load_info_from_lanelet2_map.cpp | 4 ++++ .../lanelet2_map_visualization_node.cpp | 8 ++++++++ 2 files changed, 12 insertions(+) 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 index ce4cda5c2c677..feb9fbb5047ab 100644 --- 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 index 4bff12c640c30..9b81e5516ca67 100644 --- 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 no_parking_reg_elems = lanelet::utils::query::noParkingAreas(all_lanelets); lanelet::ConstLineStrings3d curbstones = lanelet::utils::query::curbstones(viz_lanelet_map); + std::vector 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); }