feat(system error monitor): use polling subscriber

system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp

@@ -17,6 +17,7 @@
 
 #include "autoware/universe_utils/ros/logger_level_configure.hpp"
 
+#include <autoware/universe_utils/ros/polling_subscriber.hpp>
 #include <rclcpp/create_timer.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -30,6 +31,7 @@
 #include <boost/optional.hpp>
 
 #include <deque>
+#include <limits>
 #include <map>
 #include <memory>
 #include <string>
@@ -98,12 +100,21 @@ class AutowareErrorMonitor : public rclcpp::Node
   bool isDataReady();
   bool isDataHeartbeatTimeout();
   void onTimer();
+  void handleDiagArray();
+  void handleAutowareState();
+  void handleCurrentGateMode();
+  void handleControlMode();
 
   // 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_;
+  autoware::universe_utils::InterProcessPollingSubscriber<diagnostic_msgs::msg::DiagnosticArray>
+    sub_diag_array_{this, "~/input/diag_array", rclcpp::QoS(10)};
+  autoware::universe_utils::InterProcessPollingSubscriber<autoware_system_msgs::msg::AutowareState>
+    sub_autoware_state_{this, "~/input/current_gate_mode"};
+  autoware::universe_utils::InterProcessPollingSubscriber<tier4_control_msgs::msg::GateMode>
+    sub_current_gate_mode_{this, "~/input/autoware_state"};
+  autoware::universe_utils::InterProcessPollingSubscriber<
+    autoware_vehicle_msgs::msg::ControlModeReport>
+    sub_control_mode_{this, "~/input/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);

system/system_error_monitor/package.xml

@@ -5,6 +5,7 @@
   <version>0.1.1</version>
   <description>The system_error_monitor package in ROS 2</description>
   <maintainer email="fumihito.ito@tier4.jp">Fumihito Ito</maintainer>
+
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

system/system_error_monitor/src/system_error_monitor_core.cpp

@@ -1,4 +1,4 @@
 // Copyright 2020 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -229,18 +229,6 @@
 
   using std::placeholders::_1;
   using std::placeholders::_2;
-  // 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 +428,11 @@
 
 void AutowareErrorMonitor::onTimer()
 {
+  handleDiagArray();
+  handleAutowareState();
+  handleCurrentGateMode();
+  handleControlMode();
+
   if (!isDataReady()) {
     if ((this->now() - initialized_time_).seconds() > params_.data_ready_timeout) {
       RCLCPP_WARN_THROTTLE(
@@ -465,6 +458,39 @@
   publishHazardStatus(hazard_status_);
 }
 
+void AutowareErrorMonitor::handleDiagArray()
+{
+  auto diag_array_msg = sub_diag_array_.takeNewData();
+  while (diag_array_msg) {
+    onDiagArray(diag_array_msg);
+    diag_array_msg = sub_diag_array_.takeNewData();
+  }
+}
+
+void AutowareErrorMonitor::handleAutowareState()
+{
+  auto autoware_state_msg = sub_autoware_state_.takeNewData();
+  if (autoware_state_msg) {
+    onAutowareState(autoware_state_msg);
+  }
+}
+
+void AutowareErrorMonitor::handleCurrentGateMode()
+{
+  auto current_gate_msg = sub_current_gate_mode_.takeNewData();
+  if (current_gate_msg) {
+    onCurrentGateMode(current_gate_msg);
+  }
+}
+
+void AutowareErrorMonitor::handleControlMode()
+{
+  auto control_mode_msg = sub_control_mode_.takeNewData();
+  if (control_mode_msg) {
+    onControlMode(control_mode_msg);
+  }
+}
+
 boost::optional<DiagStamped> AutowareErrorMonitor::getLatestDiag(
   const std::string & diag_name) const
 {