diff --git a/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index 129cabacdcd88..ef03e2b222765 100644 --- a/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -110,10 +110,12 @@ class AccelBrakeMapCalibrator : public rclcpp::Node rclcpp::Subscription::SharedPtr velocity_sub_; // rclcpp::Subscription::SharedPtr steer_sub_; - rclcpp::Subscription::SharedPtr actuation_status_sub_; + // rclcpp::Subscription::SharedPtr actuation_status_sub_; rclcpp::Subscription::SharedPtr actuation_cmd_sub_; tier4_autoware_utils::InterProcessPollingSubscriber steer_sub_{ this, "~/input/steer"}; + tier4_autoware_utils::InterProcessPollingSubscriber actuation_status_sub_{ + this, "~/input/actuation_status"}; // Service rclcpp::Service::SharedPtr update_map_dir_server_; 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 f94ed848688b8..d3be918044455 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 @@ -211,11 +211,6 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod velocity_sub_ = create_subscription( "~/input/velocity", queue_size, std::bind(&AccelBrakeMapCalibrator::callbackVelocity, this, _1)); - if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::STATUS) { - actuation_status_sub_ = create_subscription( - "~/input/actuation_status", queue_size, - std::bind(&AccelBrakeMapCalibrator::callbackActuationStatus, this, _1)); - } if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::COMMAND) { actuation_cmd_sub_ = create_subscription( "~/input/actuation_cmd", queue_size, @@ -299,6 +294,12 @@ void AccelBrakeMapCalibrator::timerCallback() // take steer_ptr_ data steer_ptr_ = steer_sub_.takeData(); + if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::STATUS) { + ActuationStatusStamped::ConstSharedPtr actuation_ptr = actuation_status_sub_.takeData(); + if (!actuation_ptr) return; + callbackActuationStatus(actuation_ptr); + } + // data check if ( !twist_ptr_ || !steer_ptr_ || !accel_pedal_ptr_ || !brake_pedal_ptr_ ||