feat(accel_brake_map_calibrator): replace polling takeData function with the callback function

vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp

@@ -1,5 +1,5 @@
 //
-// Copyright 2020 Tier IV, Inc. All rights reserved.
+// Copyright 2024 Tier IV, Inc. All rights reserved.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -23,12 +23,13 @@
 #include "rclcpp/rclcpp.hpp"
 #include "tf2/utils.h"
 #include "tier4_autoware_utils/ros/logger_level_configure.hpp"
+#include "tier4_autoware_utils/ros/polling_subscriber.hpp"
 #include "tier4_autoware_utils/ros/transform_listener.hpp"
 
 #include <Eigen/Dense>
 
-#include "autoware_vehicle_msgs/msg/steering_report.hpp"
-#include "autoware_vehicle_msgs/msg/velocity_report.hpp"
+#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
+#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "std_msgs/msg/bool.hpp"
@@ -56,8 +57,8 @@
 namespace accel_brake_map_calibrator
 {
 
-using autoware_vehicle_msgs::msg::SteeringReport;
-using autoware_vehicle_msgs::msg::VelocityReport;
+using autoware_auto_vehicle_msgs::msg::SteeringReport;
+using autoware_auto_vehicle_msgs::msg::VelocityReport;
 using geometry_msgs::msg::TwistStamped;
 using nav_msgs::msg::OccupancyGrid;
 using raw_vehicle_cmd_converter::AccelMap;
@@ -107,10 +108,14 @@ class AccelBrakeMapCalibrator : public rclcpp::Node
   rclcpp::Publisher<Float32Stamped>::SharedPtr map_error_ratio_pub_;
   rclcpp::Publisher<CalibrationStatus>::SharedPtr calibration_status_pub_;
 
-  rclcpp::Subscription<VelocityReport>::SharedPtr velocity_sub_;
-  rclcpp::Subscription<SteeringReport>::SharedPtr steer_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"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<ActuationCommandStamped> actuation_cmd_sub_{
+    this, "~/input/actuation_cmd"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<VelocityReport> velocity_sub_{
+    this, "~/input/velocity"};
 
   // Service
   rclcpp::Service<UpdateAccelBrakeMap>::SharedPtr update_map_dir_server_;
@@ -244,12 +249,12 @@ class AccelBrakeMapCalibrator : public rclcpp::Node
     const int brake_pedal_index, const int brake_vel_index, const double measured_acc,
     const double map_acc);
   void updateTotalMapOffset(const double measured_acc, const double map_acc);
-  void callbackActuation(
-    const std_msgs::msg::Header header, const double accel, const double brake);
-  void callbackActuationCommand(const ActuationCommandStamped::ConstSharedPtr msg);
-  void callbackActuationStatus(const ActuationStatusStamped::ConstSharedPtr msg);
-  void callbackVelocity(const VelocityReport::ConstSharedPtr msg);
-  void callbackSteer(const SteeringReport::ConstSharedPtr msg);
+
+  void takeActuation(const std_msgs::msg::Header header, const double accel, const double brake);
+  void takeActuationCommand(const ActuationCommandStamped::ConstSharedPtr msg);
+  void takeActuationStatus(const ActuationStatusStamped::ConstSharedPtr msg);
+  void takeVelocity(const VelocityReport::ConstSharedPtr msg);
+
   bool callbackUpdateMapService(
     const std::shared_ptr<rmw_request_id_t> request_header,
     UpdateAccelBrakeMap::Request::SharedPtr req, UpdateAccelBrakeMap::Response::SharedPtr res);

vehicle/accel_brake_map_calibrator/package.xml

@@ -7,6 +7,7 @@
   <maintainer email="[email protected]">Tomoya Kimura</maintainer>
   <maintainer email="[email protected]">Taiki Tanaka</maintainer>
   <maintainer email="[email protected]">Takeshi Miura</maintainer>
+  <maintainer email="[email protected]">Eiki Nagata</maintainer>
 
   <license>Apache License 2.0</license>
 

vehicle/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp

@@ -1,5 +1,5 @@
 //
-// Copyright 2020 Tier IV, Inc. All rights reserved.
+// Copyright 2024 Tier IV, Inc. All rights reserved.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -207,23 +207,7 @@
   using std::placeholders::_1;
   using std::placeholders::_2;
   using std::placeholders::_3;
 
-  velocity_sub_ = create_subscription<VelocityReport>(
-    "~/input/velocity", queue_size,
-    std::bind(&AccelBrakeMapCalibrator::callbackVelocity, this, _1));
-  steer_sub_ = create_subscription<SteeringReport>(
-    "~/input/steer", queue_size, std::bind(&AccelBrakeMapCalibrator::callbackSteer, 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,
-      std::bind(&AccelBrakeMapCalibrator::callbackActuationCommand, this, _1));
-  }
-
   // Service
   update_map_dir_server_ = create_service<UpdateAccelBrakeMap>(
     "~/input/update_map_dir",
@@ -296,19 +280,42 @@
                               << "\n\t"
                               << "update_fail_count_: " << update_fail_count_ << "\n");
 
-  /* valid check */
+  // take data from subscribers
+
+  // take actuation data
+  if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::STATUS) {
+    ActuationStatusStamped::ConstSharedPtr actuation_status_ptr = actuation_status_sub_.takeData();
+    if (!actuation_status_ptr) return;
+    takeActuationStatus(actuation_status_ptr);
+  }
+  if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::COMMAND) {
+    ActuationCommandStamped::ConstSharedPtr actuation_cmd_ptr = actuation_cmd_sub_.takeData();
+    if (!actuation_cmd_ptr) return;
+    takeActuationCommand(actuation_cmd_ptr);
+  }
+
+  // take velocity data
+  VelocityReport::ConstSharedPtr velocity_ptr = velocity_sub_.takeData();
+  if (!velocity_ptr) return;
+  takeVelocity(velocity_ptr);
 
+  // take steer data
+  steer_ptr_ = steer_sub_.takeData();
+
+  /* valid check */
   // data check
   if (
     !twist_ptr_ || !steer_ptr_ || !accel_pedal_ptr_ || !brake_pedal_ptr_ ||
     !delayed_accel_pedal_ptr_ || !delayed_brake_pedal_ptr_) {
     // lack of data
     RCLCPP_WARN_STREAM_THROTTLE(
       get_logger(), *get_clock(), 5000, "lack of topics (twist, steer, accel, brake)");
     lack_of_data_count_++;
     return;
   }
 
+  debug_values_.data.at(CURRENT_STEER) = steer_ptr_->steering_tire_angle;
+
   // data check 2
   if (
     isTimeout(twist_ptr_->header.stamp, timeout_sec_) ||
@@ -458,7 +465,7 @@
   }
 }
 
-void AccelBrakeMapCalibrator::callbackVelocity(const VelocityReport::ConstSharedPtr msg)
+void AccelBrakeMapCalibrator::takeVelocity(const VelocityReport::ConstSharedPtr msg)
 {
   // convert velocity-report to twist-stamped
   auto twist_msg = std::make_shared<TwistStamped>();
@@ -496,13 +503,7 @@
   pushDataToVec(twist_msg, twist_vec_max_size_, &twist_vec_);
 }
 
-void AccelBrakeMapCalibrator::callbackSteer(const SteeringReport::ConstSharedPtr msg)
-{
-  debug_values_.data.at(CURRENT_STEER) = msg->steering_tire_angle;
-  steer_ptr_ = msg;
-}
-
-void AccelBrakeMapCalibrator::callbackActuation(
+void AccelBrakeMapCalibrator::takeActuation(
   const std_msgs::msg::Header header, const double accel, const double brake)
 {
   // get accel data
@@ -538,22 +539,21 @@
     getNearestTimeDataFromVec(brake_pedal_ptr_, pedal_to_accel_delay_, brake_pedal_vec_);
 }
 
-void AccelBrakeMapCalibrator::callbackActuationCommand(
+void AccelBrakeMapCalibrator::takeActuationCommand(
   const ActuationCommandStamped::ConstSharedPtr msg)
 {
   const auto header = msg->header;
   const auto accel = msg->actuation.accel_cmd;
   const auto brake = msg->actuation.brake_cmd;
-  callbackActuation(header, accel, brake);
+  takeActuation(header, accel, brake);
 }
 
-void AccelBrakeMapCalibrator::callbackActuationStatus(
-  const ActuationStatusStamped::ConstSharedPtr msg)
+void AccelBrakeMapCalibrator::takeActuationStatus(const ActuationStatusStamped::ConstSharedPtr msg)
 {
   const auto header = msg->header;
   const auto accel = msg->status.accel_status;
   const auto brake = msg->status.brake_status;
-  callbackActuation(header, accel, brake);
+  takeActuation(header, accel, brake);
 }
 
 bool AccelBrakeMapCalibrator::callbackUpdateMapService(