feat(lane_change): add text display for candidate path sampling metrics (autowarefoundation#9810)

* display candidate path sampling metrics on rviz

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* rename struct

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

---------

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

Author: mkquda

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp

@@ -25,10 +25,20 @@
 
 #include <limits>
 #include <string>
+#include <vector>
 
 namespace autoware::behavior_path_planner::lane_change
 {
 using utils::path_safety_checker::CollisionCheckDebugMap;
+
+struct MetricsDebug
+{
+  LaneChangePhaseMetrics prep_metric;
+  std::vector<LaneChangePhaseMetrics> lc_metrics;
+  double max_prepare_length;
+  double max_lane_changing_length;
+};
+
 struct Debug
 {
   std::string module_type;
@@ -41,6 +51,7 @@ struct Debug
   lanelet::ConstLanelets current_lanes;
   lanelet::ConstLanelets target_lanes;
   lanelet::ConstLanelets target_backward_lanes;
+  std::vector<MetricsDebug> lane_change_metrics;
   double collision_check_object_debug_lifetime{0.0};
   double distance_to_end_of_current_lane{std::numeric_limits<double>::max()};
   double distance_to_lane_change_finished{std::numeric_limits<double>::max()};
@@ -64,6 +75,7 @@ struct Debug
     current_lanes.clear();
     target_lanes.clear();
     target_backward_lanes.clear();
+    lane_change_metrics.clear();
 
     collision_check_object_debug_lifetime = 0.0;
     distance_to_end_of_current_lane = std::numeric_limits<double>::max();

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -1061,14 +1061,20 @@ std::vector<LaneChangePhaseMetrics> NormalLaneChange::get_lane_changing_metrics(
   });
 
   const auto max_path_velocity = prep_segment.points.back().point.longitudinal_velocity_mps;
-  return calculation::calc_shift_phase_metrics(
+  const auto lc_metrics = calculation::calc_shift_phase_metrics(
     common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity,
     prep_metric.sampled_lon_accel, max_lane_changing_length);
+
+  const auto max_prep_length = common_data_ptr_->transient_data.dist_to_terminal_start;
+  lane_change_debug_.lane_change_metrics.push_back(
+    {prep_metric, lc_metrics, max_prep_length, max_lane_changing_length});
+  return lc_metrics;
 }
 
 bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const
 {
   lane_change_debug_.collision_check_objects.clear();
+  lane_change_debug_.lane_change_metrics.clear();
 
   if (!common_data_ptr_->is_lanes_available()) {
     RCLCPP_WARN(logger_, "lanes are not available. Not expected.");

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp

@@ -16,6 +16,7 @@
 #include "autoware/behavior_path_planner_common/marker_utils/utils.hpp"
 
 #include <autoware/behavior_path_lane_change_module/utils/markers.hpp>
+#include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/ros/marker_helper.hpp>
 #include <autoware_lanelet2_extension/visualization/visualization.hpp>
 
@@ -185,6 +186,46 @@ MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg
   return marker_array;
 }
 
+MarkerArray ShowLaneChangeMetricsInfo(
+  const Debug & debug_data, const geometry_msgs::msg::Pose & pose)
+{
+  MarkerArray marker_array;
+  if (debug_data.lane_change_metrics.empty()) {
+    return marker_array;
+  }
+
+  auto text_marker = createDefaultMarker(
+    "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "sampling_metrics", 0, Marker::TEXT_VIEW_FACING,
+    createMarkerScale(0.6, 0.6, 0.6), colors::yellow());
+  text_marker.pose = autoware::universe_utils::calcOffsetPose(pose, 10.0, 15.0, 0.0);
+
+  text_marker.text = fmt::format("{:<12}", "") + fmt::format("{:^18}|", "lat_accel[m/s2]") +
+                     fmt::format("{:^18}|", "lon_accel[m/s2]") +
+                     fmt::format("{:^17}|", "velocity[m/s]") +
+                     fmt::format("{:^15}|", "duration[s]") + fmt::format("{:^15}|", "length[m]") +
+                     fmt::format("{:^20}\n", "max_length_th[m]");
+  for (const auto & metrics : debug_data.lane_change_metrics) {
+    text_marker.text += fmt::format("{:-<170}\n", "");
+    const auto & p_m = metrics.prep_metric;
+    text_marker.text +=
+      fmt::format("{:<17}", "prep_metrics:") + fmt::format("{:^10.3f}", p_m.lat_accel) +
+      fmt::format("{:^21.3f}", p_m.actual_lon_accel) + fmt::format("{:^12.3f}", p_m.velocity) +
+      fmt::format("{:^15.3f}", p_m.duration) + fmt::format("{:^15.3f}", p_m.length) +
+      fmt::format("{:^17.3f}\n", metrics.max_prepare_length);
+    text_marker.text += fmt::format("{:<20}\n", "lc_metrics:");
+    for (const auto lc_m : metrics.lc_metrics) {
+      text_marker.text +=
+        fmt::format("{:<15}", "") + fmt::format("{:^10.3f}", lc_m.lat_accel) +
+        fmt::format("{:^21.3f}", lc_m.actual_lon_accel) + fmt::format("{:^12.3f}", lc_m.velocity) +
+        fmt::format("{:^15.3f}", lc_m.duration) + fmt::format("{:^15.3f}", lc_m.length) +
+        fmt::format("{:^17.3f}\n", metrics.max_lane_changing_length);
+    }
+  }
+
+  marker_array.markers.push_back(text_marker);
+  return marker_array;
+}
+
 MarkerArray createDebugMarkerArray(
   const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose)
 {
@@ -212,6 +253,7 @@ MarkerArray createDebugMarkerArray(
   }
 
   add(showExecutionInfo(debug_data, ego_pose));
+  add(ShowLaneChangeMetricsInfo(debug_data, ego_pose));
 
   // lanes
   add(laneletsAsTriangleMarkerArray(