fix: replace subscription

Signed-off-by: Masahiro Kubota <norikenpi@gmail.com>

Author: masahiro-kubota

system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp

@@ -16,6 +16,7 @@
 #define SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_
 
 #include "tier4_autoware_utils/ros/logger_level_configure.hpp"
+#include "tier4_autoware_utils/ros/polling_subscriber.hpp"
 
 #include <rclcpp/create_timer.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -101,12 +102,14 @@ class AutowareErrorMonitor : public rclcpp::Node
 
   // Subscriber
   rclcpp::Subscription<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr sub_diag_array_;
-  rclcpp::Subscription<autoware_system_msgs::msg::AutowareState>::SharedPtr sub_autoware_state_;
-  rclcpp::Subscription<tier4_control_msgs::msg::GateMode>::SharedPtr sub_current_gate_mode_;
-  rclcpp::Subscription<autoware_vehicle_msgs::msg::ControlModeReport>::SharedPtr sub_control_mode_;
-  void onAutowareState(const autoware_system_msgs::msg::AutowareState::ConstSharedPtr msg);
-  void onCurrentGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg);
-  void onControlMode(const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg);
+  // polling subscribers
+  tier4_autoware_utils::InterProcessPollingSubscriber<autoware_system_msgs::msg::AutowareState> sub_autoware_state_{
+    this, "~/input/current_gate_mode"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<tier4_control_msgs::msg::GateMode> sub_current_gate_mode_{
+    this, "~/input/autoware_state"};
+  tier4_autoware_utils::InterProcessPollingSubscriber<autoware_vehicle_msgs::msg::ControlModeReport> sub_control_mode_{
+    this, "~/input/control_mode"};
+  
   void onDiagArray(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg);
 
   const size_t diag_buffer_size_ = 100;

system/system_error_monitor/src/system_error_monitor_core.cpp

@@ -232,15 +232,6 @@ AutowareErrorMonitor::AutowareErrorMonitor(const rclcpp::NodeOptions & options)
   // Subscriber
   sub_diag_array_ = create_subscription<diagnostic_msgs::msg::DiagnosticArray>(
     "input/diag_array", rclcpp::QoS{1}, std::bind(&AutowareErrorMonitor::onDiagArray, this, _1));
-  sub_current_gate_mode_ = create_subscription<tier4_control_msgs::msg::GateMode>(
-    "~/input/current_gate_mode", rclcpp::QoS{1},
-    std::bind(&AutowareErrorMonitor::onCurrentGateMode, this, _1));
-  sub_autoware_state_ = create_subscription<autoware_system_msgs::msg::AutowareState>(
-    "~/input/autoware_state", rclcpp::QoS{1},
-    std::bind(&AutowareErrorMonitor::onAutowareState, this, _1));
-  sub_control_mode_ = create_subscription<autoware_vehicle_msgs::msg::ControlModeReport>(
-    "~/input/control_mode", rclcpp::QoS{1},
-    std::bind(&AutowareErrorMonitor::onControlMode, this, _1));
 
   // Publisher
   pub_hazard_status_ = create_publisher<autoware_system_msgs::msg::HazardStatusStamped>(
@@ -440,6 +431,31 @@ bool AutowareErrorMonitor::isDataHeartbeatTimeout()
 
 void AutowareErrorMonitor::onTimer()
 {
+  const auto autoware_state_msg = sub_autoware_state_.takeData();
+  const auto current_gate_msg = sub_current_gate_mode_.takeData();
+  const auto control_mode_msg = sub_control_mode_.takeData();
+  
+  if (autoware_state_msg) {
+    autoware_state_ = autoware_state_msg;
+    // for Heartbeat
+    autoware_state_stamp_ = this->now();
+  }
+
+  if (current_gate_msg) {
+    current_gate_mode_ = current_gate_msg;
+    // for Heartbeat
+    current_gate_mode_stamp_ = this->now();
+  }
+
+  if (control_mode_msg) {
+    control_mode_ = control_mode_msg;
+    // for Heartbeat
+    control_mode_stamp_ = this->now();
+  }
+  
+
+  // for Heartbeat
+  current_gate_mode_stamp_ = this->now();
   if (!isDataReady()) {
     if ((this->now() - initialized_time_).seconds() > params_.data_ready_timeout) {
       RCLCPP_WARN_THROTTLE(