use take function for get actuation_status

Signed-off-by: N-Eiki <[email protected]>
N-Eiki · Jun 5, 2024 · e004617 · e004617
1 parent 2ac6e68
commit e004617
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 6 deletions.
diff --git a/...ake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/...ake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp
@@ -110,10 +110,12 @@ class AccelBrakeMapCalibrator : public rclcpp::Node
 
   rclcpp::Subscription<VelocityReport>::SharedPtr velocity_sub_;
   // rclcpp::Subscription<SteeringReport>::SharedPtr steer_sub_;
-  rclcpp::Subscription<ActuationStatusStamped>::SharedPtr actuation_status_sub_;
+  // rclcpp::Subscription<ActuationStatusStamped>::SharedPtr actuation_status_sub_;
   rclcpp::Subscription<ActuationCommandStamped>::SharedPtr actuation_cmd_sub_;
   tier4_autoware_utils::InterProcessPollingSubscriber<SteeringReport> steer_sub_{
     this, "~/input/steer"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<ActuationStatusStamped> actuation_status_sub_{
+    this, "~/input/actuation_status"};
 
   // Service
   rclcpp::Service<UpdateAccelBrakeMap>::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
@@ -211,11 +211,6 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod
   velocity_sub_ = create_subscription<VelocityReport>(
     "~/input/velocity", queue_size,
     std::bind(&AccelBrakeMapCalibrator::callbackVelocity, this, _1));
-  if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::STATUS) {
-    actuation_status_sub_ = create_subscription<ActuationStatusStamped>(
-      "~/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<ActuationCommandStamped>(
       "~/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_ ||