fix conflict

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

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -1266,15 +1266,21 @@ bool NormalLaneChange::get_path_using_path_shifter(
     const auto shift_length =
       lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose);
 
+    lane_change_debug_.lane_change_metrics.emplace_back();
+    auto & debug_metrics = lane_change_debug_.lane_change_metrics.back();
+    debug_metrics.prep_metric = prep_metric;
+    debug_metrics.max_prepare_length = common_data_ptr_->transient_data.dist_to_terminal_start;
     const auto lane_changing_metrics = get_lane_changing_metrics(
-      prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element);
+      prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element, debug_metrics);
 
     // set_prepare_velocity must only be called after computing lane change metrics, as lane change
     // metrics rely on the prepare segment's original velocity as max_path_velocity.
     utils::lane_change::set_prepare_velocity(
       prepare_segment, common_data_ptr_->get_ego_speed(), prep_metric.velocity);
 
     for (const auto & lc_metric : lane_changing_metrics) {
+      debug_metrics.lc_metrics.emplace_back(lc_metric, -1);
+
       const auto debug_print_lat = [&](const std::string & s) {
         RCLCPP_DEBUG(
           logger_, "%s | lc_time: %.5f | lon_acc: %.5f | lat_acc: %.5f | lc_len: %.5f", s.c_str(),
@@ -1296,6 +1302,7 @@ bool NormalLaneChange::get_path_using_path_shifter(
       }
 
       candidate_paths.push_back(candidate_path);
+      debug_metrics.lc_metrics.back().second = static_cast<int>(candidate_paths.size()) - 1;
 
       try {
         if (check_candidate_path_safety(candidate_path, target_objects)) {

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

@@ -206,27 +206,30 @@ MarkerArray ShowLaneChangeMetricsInfo(
   text_marker.pose = autoware::universe_utils::calcOffsetPose(pose, 10.0, 15.0, 0.0);
 
   if (!debug_data.lane_change_metrics.empty()) {
-    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]");
+    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}|", "max_length_th[m]") + fmt::format("{:^15}\n", "path_index");
     for (const auto & metrics : debug_data.lane_change_metrics) {
-      text_marker.text += fmt::format("{:-<170}\n", "");
+      text_marker.text += fmt::format("{:-<190}\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);
+        fmt::format("{:^17.3f}", metrics.max_prepare_length) + fmt::format("{:^15}\n", "-");
       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);
+      for (const auto & lc_m : metrics.lc_metrics) {
+        const auto & metric = lc_m.first;
+        const auto path_index = lc_m.second < 0 ? "-" : std::to_string(lc_m.second);
+        text_marker.text += fmt::format("{:<15}", "") + fmt::format("{:^10.3f}", metric.lat_accel) +
+                            fmt::format("{:^21.3f}", metric.actual_lon_accel) +
+                            fmt::format("{:^12.3f}", metric.velocity) +
+                            fmt::format("{:^15.3f}", metric.duration) +
+                            fmt::format("{:^15.3f}", metric.length) +
+                            fmt::format("{:^17.3f}", metrics.max_lane_changing_length) +
+                            fmt::format("{:^15}\n", path_index);
       }
     }
     marker_array.markers.push_back(text_marker);