feat(lane_change): visualize path info

Signed-off-by: Zulfaqar Azmi <[email protected]>

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<lanelet::ConstLanelet> & 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(

planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp

@@ -63,6 +63,14 @@ MarkerArray showAllValidLaneChangePath(const std::vector<LaneChangePath> & 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;
 }

planning/behavior_path_planner/src/marker_utils/utils.cpp

@@ -16,15 +16,18 @@
 
 #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 <rclcpp/time.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 #include <tier4_autoware_utils/ros/uuid_helper.hpp>
 
 #include <lanelet2_core/primitives/CompoundPolygon.h>
 #include <lanelet2_core/primitives/Lanelet.h>
 #include <lanelet2_core/primitives/LineString.h>
 
+#include <algorithm>
 #include <cstdint>
 
 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()) {