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); }