From c6bc4a4ea93f83d2e0f5af46500e96f1a6d60e8b Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 4 Sep 2024 18:37:49 +0900 Subject: [PATCH 1/3] 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); } From 6938115ef4118ef101d5598d2e23603b31f947c9 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 4 Sep 2024 19:58:10 +0900 Subject: [PATCH 2/3] feat(map_loader): visualize bicycle_lane Signed-off-by: Mamoru Sobue --- .../src/load_info_from_lanelet2_map.cpp | 1 - .../lanelet2_map_visualization_node.cpp | 18 ++++++++++++++++++ 2 files changed, 18 insertions(+), 1 deletion(-) 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 feb9fbb5047ab..7d3c74defda2a 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,6 @@ 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; 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 9b81e5516ca67..4a9aae78e8eb3 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 @@ -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); } From 58fda6464b60fa2d81636d470d8f678217e53372 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 6 Sep 2024 18:15:51 +0900 Subject: [PATCH 3/3] add maitainer Signed-off-by: Mamoru Sobue --- .../src/load_info_from_lanelet2_map.cpp | 3 --- map/map_loader/package.xml | 1 + 2 files changed, 1 insertion(+), 3 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 index 7d3c74defda2a..ce4cda5c2c677 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 @@ -31,9 +31,6 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::str 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/package.xml b/map/map_loader/package.xml index b8f92504a8b99..d86767861c5ce 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -11,6 +11,7 @@ NGUYEN Viet Anh Taiki Yamada Shintaro Sakoda + Mamoru Sobue Apache License 2.0 Ryohsuke Mitsudome