@@ -286,15 +286,14 @@ void AccelBrakeMapCalibrator::timerCallback()
286
286
" map updating... count: " << update_success_count_ << " / " << update_count_ << " \n\t "
287
287
<< " lack_of_data_count: " << lack_of_data_count_ << " \n\t "
288
288
<< " failed_to_get_pitch_count: " << failed_to_get_pitch_count_
289
+ << " \n\t " << " too_large_pitch_count: " << too_large_pitch_count_
290
+ << " \n\t " << " too_low_speed_count: " << too_low_speed_count_
291
+ << " \n\t " << " too_large_steer_count: " << too_large_steer_count_
292
+ << " \n\t " << " too_large_jerk_count: " << too_large_jerk_count_
293
+ << " \n\t " << " invalid_acc_brake_count: " << invalid_acc_brake_count_
289
294
<< " \n\t "
290
- << " too_large_pitch_count: " << too_large_pitch_count_ << " \n\t "
291
- << " too_low_speed_count: " << too_low_speed_count_ << " \n\t "
292
- << " too_large_steer_count: " << too_large_steer_count_ << " \n\t "
293
- << " too_large_jerk_count: " << too_large_jerk_count_ << " \n\t "
294
- << " invalid_acc_brake_count: " << invalid_acc_brake_count_ << " \n\t "
295
295
<< " too_large_pedal_spd_count: " << too_large_pedal_spd_count_
296
- << " \n\t "
297
- << " update_fail_count_: " << update_fail_count_ << " \n " );
296
+ << " \n\t " << " update_fail_count_: " << update_fail_count_ << " \n " );
298
297
299
298
/* valid check */
300
299
@@ -1031,8 +1030,7 @@ bool AccelBrakeMapCalibrator::updateEachValOffset(
1031
1030
RCLCPP_DEBUG_STREAM (
1032
1031
get_logger (), " index: " << ped_idx << " , " << vel_idx
1033
1032
<< " : map_offset_ = " << map_offset_vec_.at (ped_idx).at (vel_idx)
1034
- << " -> " << map_offset << " \t "
1035
- << " covariance = " << covariance);
1033
+ << " -> " << map_offset << " \t " << " covariance = " << covariance);
1036
1034
1037
1035
/* input calculated result and update map */
1038
1036
map_offset_vec_.at (ped_idx).at (vel_idx) = map_offset;
@@ -1060,8 +1058,7 @@ void AccelBrakeMapCalibrator::updateTotalMapOffset(const double measured_acc, co
1060
1058
map_offset_ = map_offset_ + coef * error_map_offset;
1061
1059
1062
1060
RCLCPP_DEBUG_STREAM (
1063
- get_logger (), " map_offset_ = " << map_offset_ << " \t "
1064
- << " covariance = " << covariance_);
1061
+ get_logger (), " map_offset_ = " << map_offset_ << " \t " << " covariance = " << covariance_);
1065
1062
1066
1063
/* update map */
1067
1064
for (std::size_t ped_idx = 0 ; ped_idx < update_accel_map_value_.size () - 1 ; ped_idx++) {
0 commit comments