feat(lanelet2_extension,map_loader): add guard_rail wall fence as lan…

…elet tag (autowarefoundation#478) * feat(lanelet2_extension): add guard_rails fence wall as lanelet tag Signed-off-by: tanaka3 <[email protected]> * feat(map_loader): add visualization for partion lanelet Signed-off-by: tanaka3 <[email protected]>
hcheng1005 · Mar 5, 2022 · d638b95 · d638b95
1 parent 52cdb76
commit d638b95
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 3 deletions.
diff --git a/map/lanelet2_extension/include/lanelet2_extension/utility/query.hpp b/map/lanelet2_extension/include/lanelet2_extension/utility/query.hpp
@@ -123,6 +123,9 @@ lanelet::ConstPolygons3d getAllObstaclePolygons(
 // query all parking lots in lanelet2 map
 lanelet::ConstPolygons3d getAllParkingLots(const lanelet::LaneletMapConstPtr & lanelet_map_ptr);
 
+// query all partitions in lanelet2 map
+lanelet::ConstLineStrings3d getAllPartitions(const lanelet::LaneletMapConstPtr & lanelet_map_ptr);
+
 // query all pedestrian markings in lanelet2 map
 lanelet::ConstLineStrings3d getAllPedestrianMarkings(
   const lanelet::LaneletMapConstPtr & lanelet_map_ptr);

diff --git a/map/lanelet2_extension/lib/query.cpp b/map/lanelet2_extension/lib/query.cpp
@@ -241,6 +241,20 @@ lanelet::ConstPolygons3d query::getAllParkingLots(
   return parking_lots;
 }
 
+lanelet::ConstLineStrings3d query::getAllPartitions(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
+{
+  lanelet::ConstLineStrings3d partitions;
+  for (const auto & ls : lanelet_map_ptr->lineStringLayer) {
+    const std::string type = ls.attributeOr(lanelet::AttributeName::Type, "none");
+    if (
+      type.compare("guard_rail") == 0 || type.compare("fence") == 0 || type.compare("wall") == 0) {
+      partitions.push_back(ls);
+    }
+  }
+  return partitions;
+}
+
 lanelet::ConstLineStrings3d query::getAllPedestrianMarkings(
   const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
 {

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
@@ -95,6 +95,7 @@ void Lanelet2MapVisualizationNode::onMapBin(
   lanelet::ConstLanelets shoulder_lanelets = lanelet::utils::query::shoulderLanelets(all_lanelets);
   lanelet::ConstLanelets crosswalk_lanelets =
     lanelet::utils::query::crosswalkLanelets(all_lanelets);
+  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);
@@ -114,12 +115,13 @@ void Lanelet2MapVisualizationNode::onMapBin(
   lanelet::ConstPolygons3d obstacle_polygons =
     lanelet::utils::query::getAllObstaclePolygons(viz_lanelet_map);
 
-  std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_pedestrian_markings, cl_ll_borders,
-    cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas, cl_parking_lots,
-    cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, cl_no_stopping_areas;
+  std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings,
+    cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas,
+    cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, cl_no_stopping_areas;
   setColor(&cl_road, 0.27, 0.27, 0.27, 0.999);
   setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999);
   setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5);
+  setColor(&cl_partitions, 0.25, 0.25, 0.25, 0.999);
   setColor(&cl_pedestrian_markings, 0.5, 0.5, 0.5, 0.999);
   setColor(&cl_ll_borders, 0.5, 0.5, 0.5, 0.999);
   setColor(&cl_shoulder_borders, 0.2, 0.2, 0.2, 0.999);
@@ -137,6 +139,9 @@ void Lanelet2MapVisualizationNode::onMapBin(
   insertMarkerArray(
     &map_marker_array,
     lanelet::visualization::lineStringsAsMarkerArray(stop_lines, "stop_lines", cl_stoplines, 0.5));
+  insertMarkerArray(
+    &map_marker_array,
+    lanelet::visualization::lineStringsAsMarkerArray(partitions, "partitions", cl_partitions, 0.1));
   insertMarkerArray(
     &map_marker_array,
     lanelet::visualization::laneletDirectionAsMarkerArray(shoulder_lanelets, "shoulder_"));