feat: add timeout_diagnostics (#84)

* add: timeout_diagnostics (see below)

  * This changes are copied from `autoware_control_command_gate`
      - https://github.com/autowarefoundation/autoware_universe/tree/01ff80101dfc78a0288f063e975e6e16ac042bf7/control/autoware_control_command_gate
  * We are going to use `timeout_diagnostics` via `autoware_utils` not `autoware_universe` in near future for adaptation to new ADAPI (>= 1.9.0)

Signed-off-by: Junya Sasaki <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: Junya Sasaki <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: sasakisasaki

autoware_utils_diagnostics/CMakeLists.txt

@@ -6,12 +6,14 @@ autoware_package()
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
   "src/diagnostics_interface.cpp"
+  "src/timeout_diagnostics.cpp"
 )
 
 if(BUILD_TESTING)
   ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
     "test/main.cpp"
     "test/cases/diagnostics_interface.cpp"
+    "test/cases/timeout_diagnostics.cpp"
   )
   target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME})
 endif()

autoware_utils_diagnostics/include/autoware_utils_diagnostics/timeout_diagnostics.hpp

@@ -0,0 +1,53 @@
+// Copyright 2025 The Autoware Contributors
+//
+// 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 AUTOWARE_UTILS_DIAGNOSTICS__TIMEOUT_DIAGNOSTICS_HPP_
+#define AUTOWARE_UTILS_DIAGNOSTICS__TIMEOUT_DIAGNOSTICS_HPP_
+
+#include <diagnostic_updater/diagnostic_status_wrapper.hpp>
+#include <diagnostic_updater/diagnostic_updater.hpp>
+
+#include <string>
+
+namespace autoware_utils_diagnostics
+{
+
+class TimeoutDiag : public diagnostic_updater::DiagnosticTask
+{
+public:
+  struct Params
+  {
+    double warn_duration_;
+    double error_duration_;
+  };
+
+public:
+  using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus;
+
+  TimeoutDiag(const Params & params, const rclcpp::Clock & clock, const std::string & name);
+  void update();
+  bool is_error() const { return level_ >= DiagnosticStatus::ERROR; }
+
+private:
+  void run(diagnostic_updater::DiagnosticStatusWrapper & stat) override;
+
+  const Params params_;
+  rclcpp::Clock clock_;
+  std::optional<rclcpp::Time> last_stamp_;
+  uint8_t level_;
+};
+
+}  // namespace autoware_utils_diagnostics
+
+#endif  // AUTOWARE_UTILS_DIAGNOSTICS__TIMEOUT_DIAGNOSTICS_HPP_

autoware_utils_diagnostics/package.xml

@@ -15,6 +15,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>diagnostic_msgs</depend>
+  <depend>diagnostic_updater</depend>
   <depend>rclcpp</depend>
 
   <test_depend>ament_cmake_ros</test_depend>

autoware_utils_diagnostics/src/timeout_diagnostics.cpp

@@ -0,0 +1,55 @@
+// Copyright 2025 The Autoware Contributors
+//
+// 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.
+
+#include "autoware_utils_diagnostics/timeout_diagnostics.hpp"
+
+#include <string>
+
+namespace autoware_utils_diagnostics
+{
+
+TimeoutDiag::TimeoutDiag(
+  const Params & params, const rclcpp::Clock & clock, const std::string & name)
+: DiagnosticTask(name), params_(params), clock_(clock)
+{
+  level_ = DiagnosticStatus::STALE;
+}
+
+void TimeoutDiag::update()
+{
+  last_stamp_ = clock_.now();
+}
+
+void TimeoutDiag::run(diagnostic_updater::DiagnosticStatusWrapper & stat)
+{
+  if (!last_stamp_) {
+    stat.summary(DiagnosticStatus::ERROR, "no data");
+    return;
+  }
+
+  const auto duration = (clock_.now() - *last_stamp_).seconds();
+
+  if (duration < params_.warn_duration_) {
+    level_ = DiagnosticStatus::OK;
+  } else if (duration < params_.error_duration_) {
+    level_ = DiagnosticStatus::WARN;
+  } else {
+    level_ = DiagnosticStatus::ERROR;
+  }
+
+  stat.summary(level_, "");
+  stat.add("duration", duration);
+}
+
+}  // namespace autoware_utils_diagnostics

autoware_utils_diagnostics/test/cases/timeout_diagnostics.cpp

@@ -0,0 +1,28 @@
+// Copyright 2025 The Autoware Contributors
+//
+// 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.
+
+#include "autoware_utils_diagnostics/timeout_diagnostics.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+TEST(TimeoutDiagTest, Instantiation)
+{
+  const auto node = std::make_shared<rclcpp::Node>("test_node");
+  autoware_utils_diagnostics::TimeoutDiag::Params params{1.0, 2.0};
+  autoware_utils_diagnostics::TimeoutDiag timeout_diag(params, *node->get_clock(), "test_diag");
+}