diff --git a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp index 22f1f4e9012c7..5aafd81e62703 100644 --- a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp +++ b/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 #include @@ -101,12 +102,14 @@ class AutowareErrorMonitor : public rclcpp::Node // Subscriber rclcpp::Subscription::SharedPtr sub_diag_array_; - rclcpp::Subscription::SharedPtr sub_autoware_state_; - rclcpp::Subscription::SharedPtr sub_current_gate_mode_; - rclcpp::Subscription::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 + sub_autoware_state_{this, "~/input/current_gate_mode"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_current_gate_mode_{this, "~/input/autoware_state"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_control_mode_{this, "~/input/control_mode"}; + void onDiagArray(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg); const size_t diag_buffer_size_ = 100; diff --git a/system/system_error_monitor/package.xml b/system/system_error_monitor/package.xml index 0c8e2fc0241aa..94d5c1df9133b 100644 --- a/system/system_error_monitor/package.xml +++ b/system/system_error_monitor/package.xml @@ -5,6 +5,7 @@ 0.1.1 The system_error_monitor package in ROS 2 Fumihito Ito + Masahiro Kubota Apache License 2.0 ament_cmake_auto diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index ad67521298643..6b0499d9e11c2 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/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( "input/diag_array", rclcpp::QoS{1}, std::bind(&AutowareErrorMonitor::onDiagArray, this, _1)); - sub_current_gate_mode_ = create_subscription( - "~/input/current_gate_mode", rclcpp::QoS{1}, - std::bind(&AutowareErrorMonitor::onCurrentGateMode, this, _1)); - sub_autoware_state_ = create_subscription( - "~/input/autoware_state", rclcpp::QoS{1}, - std::bind(&AutowareErrorMonitor::onAutowareState, this, _1)); - sub_control_mode_ = create_subscription( - "~/input/control_mode", rclcpp::QoS{1}, - std::bind(&AutowareErrorMonitor::onControlMode, this, _1)); // Publisher pub_hazard_status_ = create_publisher( @@ -440,6 +431,30 @@ 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(