diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 8326a85cbadc8..7748795851865 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -1266,8 +1266,12 @@ 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. @@ -1275,6 +1279,8 @@ bool NormalLaneChange::get_path_using_path_shifter( 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(candidate_paths.size()) - 1; try { if (check_candidate_path_safety(candidate_path, target_objects)) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index 857d077e087f3..bf0af0d133dc7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp +++ b/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);