From 801c361ccebb511d020a317e4d17b24a6a125dde Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 13 May 2024 19:02:42 +0900 Subject: [PATCH] fix(accel_brake_map_calibrator): fix accel_brake_map_calibrator not to output invalid maps (#6992) Signed-off-by: tomoya.kimura --- .../src/accel_brake_map_calibrator_node.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/vehicle/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index ecfc312d2bdfd..37f0a11165df3 100644 --- a/vehicle/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -703,17 +703,19 @@ void AccelBrakeMapCalibrator::takeConsistencyOfAccelMap() { const double bit = std::pow(1e-01, precision_); for (std::size_t ped_idx = 0; ped_idx < update_accel_map_value_.size() - 1; ped_idx++) { - for (std::size_t vel_idx = 0; vel_idx < update_accel_map_value_.at(0).size() - 1; vel_idx++) { + for (std::size_t vel_idx = update_accel_map_value_.at(0).size() - 1;; vel_idx--) { + if (vel_idx == 0) break; + const double current_acc = update_accel_map_value_.at(ped_idx).at(vel_idx); const double next_ped_acc = update_accel_map_value_.at(ped_idx + 1).at(vel_idx); - const double next_vel_acc = update_accel_map_value_.at(ped_idx).at(vel_idx + 1); + const double prev_vel_acc = update_accel_map_value_.at(ped_idx).at(vel_idx - 1); - if (current_acc <= next_vel_acc) { + if (current_acc + bit >= prev_vel_acc) { // the higher the velocity, the lower the acceleration - update_accel_map_value_.at(ped_idx).at(vel_idx + 1) = current_acc - bit; + update_accel_map_value_.at(ped_idx).at(vel_idx - 1) = current_acc + bit; } - if (current_acc >= next_ped_acc) { + if (current_acc + bit >= next_ped_acc) { // the higher the accel pedal, the higher the acceleration update_accel_map_value_.at(ped_idx + 1).at(vel_idx) = current_acc + bit; }