From 7bb1158c19e8f70c9c9467c48b17e67a94e7ff6e Mon Sep 17 00:00:00 2001 From: eiki <53928021+N-Eiki@users.noreply.github.com> Date: Tue, 27 Aug 2024 10:13:26 +0900 Subject: [PATCH] feat(autoware_accel_brake_map_calibrator): conditional actuation data processing based on source (#8593) * fix: Conditional Actuation Data Processing Based on Source Signed-off-by: N-Eiki * style(pre-commit): autofix * delete extra comentout, indent Signed-off-by: N-Eiki * add take validation Signed-off-by: N-Eiki * style(pre-commit): autofix --------- Signed-off-by: N-Eiki Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/accel_brake_map_calibrator_node.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index 6a88d5c61a2d8..a99fcd6b16401 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -266,15 +266,16 @@ bool AccelBrakeMapCalibrator::get_current_pitch_from_tf(double * pitch) bool AccelBrakeMapCalibrator::take_data() { // take data from subscribers - // take actuation data - ActuationStatusStamped::ConstSharedPtr actuation_status_ptr = actuation_status_sub_.takeData(); - ActuationCommandStamped::ConstSharedPtr actuation_cmd_ptr = actuation_cmd_sub_.takeData(); - if (actuation_status_ptr) { + if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::STATUS) { + ActuationStatusStamped::ConstSharedPtr actuation_status_ptr = actuation_status_sub_.takeData(); + if (!actuation_status_ptr) return false; take_actuation_status(actuation_status_ptr); - } else if (actuation_cmd_ptr) { + } + // take actuation data + if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::COMMAND) { + ActuationCommandStamped::ConstSharedPtr actuation_cmd_ptr = actuation_cmd_sub_.takeData(); + if (!actuation_cmd_ptr) return false; take_actuation_command(actuation_cmd_ptr); - } else { - return false; } // take velocity data