From d639bb5835a98711d499fcc374901edecd5d6c8e Mon Sep 17 00:00:00 2001
From: h-ohta <hiroki.ota@tier4.jp>
Date: Mon, 24 Oct 2022 18:38:51 +0900
Subject: [PATCH] fix(emergency_handler): stop using heartbeat_checher to make
 subscriber unique

---
 .../system/heartbeat_checker.hpp              | 58 -------------------
 .../emergency_handler_core.hpp                |  4 +-
 system/emergency_handler/package.xml          |  1 -
 .../emergency_handler_core.cpp                | 10 ++--
 4 files changed, 5 insertions(+), 68 deletions(-)
 delete mode 100644 common/tier4_autoware_utils/include/tier4_autoware_utils/system/heartbeat_checker.hpp

diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/heartbeat_checker.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/system/heartbeat_checker.hpp
deleted file mode 100644
index 1c2f9d5dfb5da..0000000000000
--- a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/heartbeat_checker.hpp
+++ /dev/null
@@ -1,58 +0,0 @@
-// 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.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef TIER4_AUTOWARE_UTILS__SYSTEM__HEARTBEAT_CHECKER_HPP_
-#define TIER4_AUTOWARE_UTILS__SYSTEM__HEARTBEAT_CHECKER_HPP_
-
-#include <rclcpp/rclcpp.hpp>
-
-#include <string>
-
-template <class HeartbeatMsg>
-class HeaderlessHeartbeatChecker
-{
-public:
-  HeaderlessHeartbeatChecker(
-    rclcpp::Node & node, const std::string & topic_name, const double timeout)
-  : clock_(node.get_clock()), timeout_(timeout)
-  {
-    using std::placeholders::_1;
-    sub_heartbeat_ = node.create_subscription<HeartbeatMsg>(
-      topic_name, rclcpp::QoS{1}, std::bind(&HeaderlessHeartbeatChecker::onHeartbeat, this, _1));
-  }
-
-  bool isTimeout()
-  {
-    const auto time_from_last_heartbeat = clock_->now() - last_heartbeat_time_;
-    return time_from_last_heartbeat.seconds() > timeout_;
-  }
-
-private:
-  // Clock
-  rclcpp::Clock::SharedPtr clock_;
-
-  // Parameter
-  double timeout_;
-
-  // Subscriber
-  typename rclcpp::Subscription<HeartbeatMsg>::SharedPtr sub_heartbeat_;
-  rclcpp::Time last_heartbeat_time_ = rclcpp::Time(0);
-
-  void onHeartbeat([[maybe_unused]] const typename HeartbeatMsg::ConstSharedPtr msg)
-  {
-    last_heartbeat_time_ = clock_->now();
-  }
-};
-
-#endif  // TIER4_AUTOWARE_UTILS__SYSTEM__HEARTBEAT_CHECKER_HPP_
diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp
index db3acf500fb97..3a20956ddd841 100644
--- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp
+++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp
@@ -30,7 +30,6 @@
 // ROS2 core
 #include <rclcpp/create_timer.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <tier4_autoware_utils/system/heartbeat_checker.hpp>
 
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <nav_msgs/msg/odometry.hpp>
@@ -102,8 +101,7 @@ class EmergencyHandler : public rclcpp::Node
   void onTimer();
 
   // Heartbeat
-  std::shared_ptr<HeaderlessHeartbeatChecker<autoware_auto_system_msgs::msg::HazardStatusStamped>>
-    heartbeat_hazard_status_;
+  rclcpp::Time stamp_hazard_status_;
 
   // Algorithm
   autoware_auto_system_msgs::msg::EmergencyState::_state_type emergency_state_{
diff --git a/system/emergency_handler/package.xml b/system/emergency_handler/package.xml
index 588421d013808..e19b8c8054c9a 100644
--- a/system/emergency_handler/package.xml
+++ b/system/emergency_handler/package.xml
@@ -18,7 +18,6 @@
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
-  <depend>tier4_autoware_utils</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp
index afff72edb0ed9..8a695b8cb227d 100644
--- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp
+++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp
@@ -45,11 +45,6 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler")
   sub_control_mode_ = create_subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>(
     "~/input/control_mode", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onControlMode, this, _1));
 
-  // Heartbeat
-  heartbeat_hazard_status_ = std::make_shared<
-    HeaderlessHeartbeatChecker<autoware_auto_system_msgs::msg::HazardStatusStamped>>(
-    *this, "~/input/hazard_status", param_.timeout_hazard_status);
-
   // Publisher
   pub_control_command_ = create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>(
     "~/output/control_command", rclcpp::QoS{1});
@@ -76,6 +71,7 @@ void EmergencyHandler::onHazardStatusStamped(
   const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg)
 {
   hazard_status_stamped_ = msg;
+  stamp_hazard_status_ = this->now();
 }
 
 void EmergencyHandler::onPrevControlCommand(
@@ -178,7 +174,9 @@ void EmergencyHandler::onTimer()
   if (!isDataReady()) {
     return;
   }
-  if (heartbeat_hazard_status_->isTimeout()) {
+  const bool is_hazard_status_timeout =
+    (this->now() - stamp_hazard_status_).seconds() > param_.timeout_hazard_status;
+  if (is_hazard_status_timeout) {
     RCLCPP_WARN_THROTTLE(
       this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(),
       "heartbeat_hazard_status is timeout");