diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index 86f1a390261c6..0ac074d8dde41 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -36,6 +36,7 @@ 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::ShiftLine; using behavior_path_planner::ShiftLineArray; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugPair; @@ -86,6 +87,14 @@ MarkerArray createLaneletsAreaMarkerArray( const std::vector & lanelets, std::string && ns, const float & r, const float & g, const float & b); +MarkerArray createShiftLineMarker( + const ShiftLine & shift_line, const rclcpp::Time & current_time, const std::string & ns, + const double current_offset, int & id, const double line_width, const ColorRGBA & color); + +MarkerArray createShiftLineMarker( + const Pose & start_pose, const Pose & end_pose, const rclcpp::Time & current_time, + const std::string & ns, int & id, const double line_width, const ColorRGBA & color); + MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3d & linestrings); MarkerArray createPolygonMarkerArray( diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp index a7604124ebe43..ddb9c37167ac3 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp @@ -63,6 +63,14 @@ MarkerArray showAllValidLaneChangePath(const std::vector & lanes } marker_array.markers.push_back(marker); + + const auto & shift_line = lanes.at(idx).info.shift_line; + const auto shift_line_marker = createShiftLineMarker( + shift_line.start, shift_line.end, current_time, ns + "_sl", ++id, 0.2, color); + + marker_array.markers.insert( + marker_array.markers.end(), shift_line_marker.markers.begin(), + shift_line_marker.markers.end()); } return marker_array; } diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index 031926f2d9d1d..82784c03f779e 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -16,8 +16,10 @@ #include "behavior_path_planner/marker_utils/colors.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include #include #include @@ -25,6 +27,7 @@ #include #include +#include #include namespace marker_utils @@ -119,49 +122,25 @@ MarkerArray createShiftLineMarkerArray( const ShiftLineArray & shift_lines, const double & base_shift, std::string && ns, const float & r, const float & g, const float & b, const float & w) { - ShiftLineArray shift_lines_local = shift_lines; + MarkerArray msg; if (shift_lines.empty()) { - shift_lines_local.push_back(ShiftLine()); + return msg; } - MarkerArray msg; - msg.markers.reserve(shift_lines_local.size() * 3); + // Note: every shift_line_marker will consist of 3 marker, i.e.:CUBE, CUBE, LINE + msg.markers.reserve(shift_lines.size() * 3); const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); int id{0}; - // TODO(Horibe) now assuming the shift point is aligned in longitudinal distance order + // TODO(Horibe) now assuming the shift line is aligned in longitudinal distance order double current_shift = base_shift; - for (const auto & sp : shift_lines_local) { + for (const auto & sp : shift_lines) { // ROS_ERROR("sp: s = (%f, %f), g = (%f, %f)", sp.start.x, sp.start.y, sp.end.x, sp.end.y); - Marker basic_marker = createDefaultMarker( - "map", current_time, ns, 0L, Marker::CUBE, createMarkerScale(0.5, 0.5, 0.5), - createMarkerColor(r, g, b, 0.5)); - basic_marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); - { - // start point - auto marker_s = basic_marker; - marker_s.id = ++id; - marker_s.pose = sp.start; - marker_s.pose = calcOffsetPose(marker_s.pose, 0.0, current_shift, 0.0); - msg.markers.push_back(marker_s); - - // end point - auto marker_e = basic_marker; - marker_e.id = ++id; - marker_e.pose = sp.end; - marker_e.pose = calcOffsetPose(marker_e.pose, 0.0, sp.end_shift_length, 0.0); - msg.markers.push_back(marker_e); - - // start-to-end line - auto marker_l = basic_marker; - marker_l.id = ++id; - marker_l.type = Marker::LINE_STRIP; - marker_l.scale = createMarkerScale(w, 0.0, 0.0); - marker_l.points.reserve(2); - marker_l.points.push_back(marker_s.pose.position); - marker_l.points.push_back(marker_e.pose.position); - msg.markers.push_back(marker_l); - } + const auto shift_line_marker = createShiftLineMarker( + sp, current_time, ns, current_shift, ++id, w, createMarkerColor(r, g, b, 0.5)); + + msg.markers.insert( + msg.markers.end(), shift_line_marker.markers.begin(), shift_line_marker.markers.end()); current_shift = sp.end_shift_length; } @@ -258,6 +237,61 @@ MarkerArray createLaneletsAreaMarkerArray( return msg; } +MarkerArray createShiftLineMarker( + const ShiftLine & shift_line, const rclcpp::Time & current_time, const std::string & ns, + const double current_offset, int & id, const double line_width, const ColorRGBA & color) +{ + return createShiftLineMarker( + calcOffsetPose(shift_line.start, 0.0, current_offset, 0.0), + calcOffsetPose(shift_line.end, 0.0, shift_line.end_shift_length, 0.0), current_time, ns, id, + line_width, color); +} + +MarkerArray createShiftLineMarker( + const Pose & start_pose, const Pose & end_pose, const rclcpp::Time & current_time, + const std::string & ns, int & id, const double line_width, const ColorRGBA & color) +{ + MarkerArray shift_line_marker; + const auto default_cube_marker = []( + const Pose & shift_pose, const rclcpp::Time & current_time, + const std::string & ns, int id, const ColorRGBA & color) { + auto marker = createDefaultMarker( + "map", current_time, ns, 0L, Marker::CUBE, createMarkerScale(0.5, 0.5, 0.5), color); + marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); + marker.id = ++id; + marker.pose = shift_pose; + return marker; + }; + + const auto default_line_marker = []( + const Pose & start_pose, const Pose & end_pose, + const rclcpp::Time & current_time, const std::string & ns, + double line_width, int id, const ColorRGBA & color) { + auto marker = createDefaultMarker( + "map", current_time, ns, 0L, Marker::LINE_STRIP, createMarkerScale(line_width, 0.0, 0.0), + color); + marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); + marker.id = ++id; + marker.points.reserve(2); + marker.points.push_back(start_pose.position); + marker.points.push_back(end_pose.position); + return marker; + }; + + // start point + const auto marker_s = default_cube_marker(start_pose, current_time, ns, ++id, color); + // end point + const auto marker_e = default_cube_marker(end_pose, current_time, ns, ++id, color); + // start-to-end line + auto marker_l = + default_line_marker(marker_s.pose, marker_e.pose, current_time, ns, line_width, ++id, color); + + shift_line_marker.markers.push_back(marker_s); + shift_line_marker.markers.push_back(marker_e); + shift_line_marker.markers.push_back(marker_l); + return shift_line_marker; +} + MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3d & linestrings) { if (linestrings.empty()) {