diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp index 2e204e8c672ad..790efc60dc291 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp @@ -14,6 +14,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTIL__DEBUG_UTILITIES_HPP_ #define BEHAVIOR_PATH_PLANNER__MARKER_UTIL__DEBUG_UTILITIES_HPP_ +#include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -36,6 +37,7 @@ namespace marker_utils using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::DrivableLanes; using behavior_path_planner::ShiftLineArray; using geometry_msgs::msg::Point; using geometry_msgs::msg::Polygon; @@ -126,6 +128,9 @@ MarkerArray createObjectsMarkerArray( const PredictedObjects & objects, std::string && ns, const int64_t & lane_id, const float & r, const float & g, const float & b); +MarkerArray createDrivableLanesMarkerArray( + const std::vector & drivable_lanes, std::string && ns); + } // namespace marker_utils #endif // BEHAVIOR_PATH_PLANNER__MARKER_UTIL__DEBUG_UTILITIES_HPP_ diff --git a/planning/behavior_path_planner/src/marker_util/debug_utilities.cpp b/planning/behavior_path_planner/src/marker_util/debug_utilities.cpp index 2077982ecdeb6..9599efeaf643f 100644 --- a/planning/behavior_path_planner/src/marker_util/debug_utilities.cpp +++ b/planning/behavior_path_planner/src/marker_util/debug_utilities.cpp @@ -361,4 +361,51 @@ MarkerArray createObjectsMarkerArray( return msg; } + +MarkerArray createDrivableLanesMarkerArray( + const std::vector & drivable_lanes, std::string && ns) +{ + MarkerArray msg; + + int32_t i{0}; + + const auto get_lanelet_marker = + [&ns, &i](const auto & lanelet, const auto r, const auto g, const auto b) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, bitShift(lanelet.id()) + i++, + Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + + if (lanelet.polygon3d().empty()) { + return marker; + } + + marker.points.reserve(lanelet.polygon3d().size() + 1); + + for (const auto & p : lanelet.polygon3d()) { + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + + marker.points.push_back(marker.points.front()); + + return marker; + }; + + const auto get_drivable_lanes = [&msg, &get_lanelet_marker](const DrivableLanes & drivable_lane) { + { + msg.markers.push_back(get_lanelet_marker(drivable_lane.right_lane, 1.0, 0.0, 0.0)); + } + + { + msg.markers.push_back(get_lanelet_marker(drivable_lane.left_lane, 0.0, 1.0, 0.0)); + } + + std::for_each( + drivable_lane.middle_lanes.begin(), drivable_lane.middle_lanes.end(), + [&](const auto & lane) { msg.markers.push_back(get_lanelet_marker(lane, 0.0, 0.0, 1.0)); }); + }; + + std::for_each(drivable_lanes.begin(), drivable_lanes.end(), get_drivable_lanes); + + return msg; +} } // namespace marker_utils