From daa6f045855ebdb3c2cac4703b4d92516a09d685 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Tue, 12 Sep 2023 14:18:39 +0900 Subject: [PATCH] display curbstone as marker array Signed-off-by: Shohei Sakai --- .../lanelet2_map_visualization_node.cpp | 8 +++++++- 1 file changed, 7 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 53794fb93b7c6..c325f183fd783 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 @@ -127,13 +127,14 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "hatched_road_markings"); std::vector no_parking_reg_elems = lanelet::utils::query::noParkingAreas(all_lanelets); + lanelet::ConstLineStrings3d curbstones = lanelet::utils::query::curbstones(viz_lanelet_map); 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_speed_bumps, cl_crosswalks, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, cl_no_stopping_areas, cl_no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area, - cl_hatched_road_markings_line, cl_no_parking_areas; + cl_hatched_road_markings_line, cl_no_parking_areas, cl_curbstones; 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); @@ -156,6 +157,7 @@ void Lanelet2MapVisualizationNode::onMapBin( setColor(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5); setColor(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999); setColor(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5); + setColor(&cl_curbstones, 0.1, 0.1, 0.2, 0.999); visualization_msgs::msg::MarkerArray map_marker_array; @@ -242,6 +244,10 @@ void Lanelet2MapVisualizationNode::onMapBin( &map_marker_array, lanelet::visualization::noParkingAreasAsMarkerArray(no_parking_reg_elems, cl_no_parking_areas)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::lineStringsAsMarkerArray(curbstones, "curbstone", cl_curbstones, 0.2)); + pub_marker_->publish(map_marker_array); }