From 015be4b944b3005d32b9cd8a39fa2f1da59a7ce5 Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi Date: Sun, 3 Jul 2022 13:55:15 +0900 Subject: [PATCH] feat(lanelet2_extension): add road empty warning Signed-off-by: Shumpei Wakabayashi --- .../lanelet2_map_visualization_node.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) 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 55b16080c94fa..a8f3ea635b77e 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,10 +95,20 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::ConstLanelets shoulder_lanelets = lanelet::utils::query::shoulderLanelets(all_lanelets); lanelet::ConstLanelets crosswalk_lanelets = lanelet::utils::query::crosswalkLanelets(all_lanelets); + lanelet::ConstLanelets walkway_lanelets = lanelet::utils::query::walkwayLanelets(all_lanelets); + + if (road_lanelets.size() + shoulder_lanelets.size() + crosswalk_lanelets.size() + + walkway_lanelets.size() != all_lanelets.size()) { + RCLCPP_WARN_STREAM( + this->get_logger(), + "The correct subtype must be defined for each lanelet." + "Otherwise, the lanelet will not appear on Rviz."); + } + lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(viz_lanelet_map); lanelet::ConstLineStrings3d pedestrian_markings = lanelet::utils::query::getAllPedestrianMarkings(viz_lanelet_map); - lanelet::ConstLanelets walkway_lanelets = lanelet::utils::query::walkwayLanelets(all_lanelets); + std::vector stop_lines = lanelet::utils::query::stopLinesLanelets(road_lanelets); std::vector tl_reg_elems =