create PredictedPath debug marker

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
kyoichi-sugahara · Jun 9, 2023 · 1ae32a3 · 1ae32a3
1 parent 1bc6451
commit 1ae32a3
Show file tree

Hide file tree

Showing 3 changed files with 46 additions and 0 deletions.
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
@@ -108,6 +108,9 @@ MarkerArray createShiftGradMarkerArray(
   const PathWithLaneId & reference, std::string && ns, const float & r, const float & g,
   const float & b);
 
+MarkerArray createEgoPredictedPathMarkerArray(
+  const PredictedPath & ego_predicted_path, std::string && ns);
+
 MarkerArray createLaneletsAreaMarkerArray(
   const std::vector<lanelet::ConstLanelet> & lanelets, std::string && ns, const float & r,
   const float & g, const float & b);

diff --git a/planning/behavior_path_planner/src/marker_util/debug_utilities.cpp b/planning/behavior_path_planner/src/marker_util/debug_utilities.cpp
@@ -196,6 +196,38 @@ MarkerArray createShiftGradMarkerArray(
   return msg;
 }
 
+MarkerArray createEgoPredictedPathMarkerArray(
+  const PredictedPath & ego_predicted_path, std::string && ns)
+{
+  if (ego_predicted_path.path.empty()) {
+    return MarkerArray{};
+  }
+
+  MarkerArray msg;
+  constexpr auto colors = colorsList();
+  constexpr float scale_val = 0.2;
+  const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()};
+
+  int32_t id{0};
+
+  const auto & path = ego_predicted_path.path;
+  const auto & color = colors.at(0);
+
+  Marker marker = createDefaultMarker(
+    "map", current_time, ns, ++id, Marker::LINE_STRIP,
+    createMarkerScale(scale_val, scale_val, scale_val),
+    createMarkerColor(color[0], color[1], color[2], 0.9));
+
+  marker.points.reserve(path.size());
+
+  for (const auto & point : path) {
+    marker.points.push_back(point.position);
+  }
+
+  msg.markers.push_back(marker);
+  return msg;
+}
+
 MarkerArray createLaneletsAreaMarkerArray(
   const std::vector<lanelet::ConstLanelet> & lanelets, std::string && ns, const float & r,
   const float & g, const float & b)

diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
@@ -817,14 +817,25 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const
 
 void StartPlannerModule::setDebugData() const
 {
+  using marker_utils::createEgoPredictedPathMarkerArray;
   using marker_utils::createPathMarkerArray;
   using marker_utils::createPoseMarkerArray;
 
   const auto add = [this](const MarkerArray & added) {
     tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);
   };
 
+  const auto & ego_predicted_path = utils::createPredictedPathFromTargetVelocity(
+    status_.pull_out_path.partial_paths.back().points,
+    planner_data_->self_odometry->twist.twist.linear.x, 2.0, 0.5,
+    planner_data_->self_odometry->pose.pose,
+    motion_utils::findNearestIndex(
+      status_.pull_out_path.partial_paths.back().points,
+      planner_data_->self_odometry->pose.pose.position),
+    0.5, 1.0);
+
   debug_marker_.markers.clear();
+  add(createEgoPredictedPathMarkerArray(ego_predicted_path, "ego_predicted_path"));
   add(createPoseMarkerArray(status_.pull_out_start_pose, "back_end_pose", 0, 0.9, 0.3, 0.3));
   add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3));
   add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3));