feat(lanelet2_extension): add road empty warning

Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp>
autowarefoundation · Jul 3, 2022 · ce34834 · ce34834
1 parent 0a17fe3
commit ce34834
Showing 1 changed file with 8 additions and 1 deletion.
diff --git a/map/lanelet2_extension/lib/query.cpp b/map/lanelet2_extension/lib/query.cpp
@@ -87,7 +87,14 @@ lanelet::ConstLanelets query::walkwayLanelets(const lanelet::ConstLanelets lls)
 
 lanelet::ConstLanelets query::roadLanelets(const lanelet::ConstLanelets lls)
 {
-  return query::subtypeLanelets(lls, lanelet::AttributeValueString::Road);
+  lanelet::ConstLanelets road_lanelets;
+  road_lanelets = query::subtypeLanelets(lls, lanelet::AttributeValueString::Road);
+  if (road_lanelets.empty()) {
+    RCLCPP_WARN_STREAM(
+      rclcpp::get_logger("lanelet2_extension.query"),
+      "No road found in lanelet map. Thus vector map will not appear on Rviz.");
+  };
+  return road_lanelets;
 }
 
 lanelet::ConstLanelets query::shoulderLanelets(const lanelet::ConstLanelets lls)