feat(component_interface_tools): add service log checker (autowarefou…

…ndation#2503)

* feat(component_interface_utils): add service log checker

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat(component_interface_tools): add service log checker

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat(component_interface_tools): add diagnostics

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: update system error monitor config

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

common/component_interface_tools/CMakeLists.txt

@@ -0,0 +1,7 @@
+cmake_minimum_required(VERSION 3.14)
+project(component_interface_tools)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+ament_auto_add_executable(service_log_checker src/service_log_checker.cpp)
+ament_auto_package(INSTALL_TO_SHARE launch)

common/component_interface_tools/launch/service_log_checker.launch.xml

@@ -0,0 +1,3 @@
+<launch>
+  <node pkg="component_interface_tools" exec="service_log_checker" name="service_log_checker"/>
+</launch>

common/component_interface_tools/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>component_interface_tools</name>
+  <version>0.1.0</version>
+  <description>The component_interface_tools package</description>
+  <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
+  <maintainer email="makoto.yabuta@tier4.jp">yabuta</maintainer>
+  <maintainer email="kahhooi.tan@tier4.jp">Kah Hooi Tan</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>diagnostic_updater</depend>
+  <depend>fmt</depend>
+  <depend>rclcpp</depend>
+  <depend>tier4_system_msgs</depend>
+  <depend>yaml_cpp_vendor</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

common/component_interface_tools/src/service_log_checker.cpp

@@ -0,0 +1,110 @@
+// Copyright 2022 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.
+
+#include "service_log_checker.hpp"
+
+#include <yaml-cpp/yaml.h>
+
+#include <memory>
+#include <string>
+
+#define FMT_HEADER_ONLY
+#include <fmt/format.h>
+
+ServiceLogChecker::ServiceLogChecker() : Node("service_log_checker"), diagnostics_(this)
+{
+  sub_ = create_subscription<ServiceLog>(
+    "/service_log", 50, std::bind(&ServiceLogChecker::on_service_log, this, std::placeholders::_1));
+
+  diagnostics_.setHardwareID(get_name());
+  diagnostics_.add("response_status", this, &ServiceLogChecker::update_diagnostics);
+}
+
+void ServiceLogChecker::on_service_log(const ServiceLog::ConstSharedPtr msg)
+{
+  try {
+    // Ignore service request.
+    if (msg->type == ServiceLog::CLIENT_REQUEST || msg->type == ServiceLog::SERVER_REQUEST) {
+      return;
+    }
+
+    // Ignore service errors.
+    if (msg->type == ServiceLog::ERROR_UNREADY) {
+      return set_error(*msg, "not ready");
+    }
+    if (msg->type == ServiceLog::ERROR_TIMEOUT) {
+      return set_error(*msg, "timeout");
+    }
+
+    // Ignore version API because it doesn't have response status.
+    if (msg->name == "/api/interface/version") {
+      return;
+    }
+
+    // Parse response data.
+    const auto status = YAML::Load(msg->yaml)["status"];
+    if (!status) {
+      return set_error(*msg, "no response status");
+    }
+
+    // Check response status.
+    const auto success = status["success"].as<bool>();
+    if (!success) {
+      const auto message = status["message"].as<std::string>();
+      const auto code = status["code"].as<uint16_t>();
+      return set_error(*msg, fmt::format("status code {} '{}'", code, message));
+    }
+  } catch (const YAML::Exception & error) {
+    return set_error(*msg, fmt::format("invalid data: '{}'", error.what()));
+  }
+
+  set_success(*msg);
+}
+
+void ServiceLogChecker::set_success(const ServiceLog & msg)
+{
+  errors_.erase(fmt::format("{} ({})", msg.name, msg.node));
+}
+
+void ServiceLogChecker::set_error(const ServiceLog & msg, const std::string & log)
+{
+  errors_[fmt::format("{} ({})", msg.name, msg.node)] = log;
+  RCLCPP_ERROR_STREAM(get_logger(), fmt::format("{}: {} ({})", msg.name, log, msg.node));
+}
+
+void ServiceLogChecker::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat)
+{
+  using diagnostic_msgs::msg::DiagnosticStatus;
+
+  for (const auto & error : errors_) {
+    stat.add(error.first, error.second);
+  }
+
+  if (errors_.empty()) {
+    stat.summary(DiagnosticStatus::OK, "OK");
+  } else {
+    stat.summary(DiagnosticStatus::ERROR, "ERROR");
+  }
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::executors::SingleThreadedExecutor executor;
+  auto node = std::make_shared<ServiceLogChecker>();
+  executor.add_node(node);
+  executor.spin();
+  executor.remove_node(node);
+  rclcpp::shutdown();
+}

common/component_interface_tools/src/service_log_checker.hpp

@@ -0,0 +1,42 @@
+// Copyright 2022 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 SERVICE_LOG_CHECKER_HPP_
+#define SERVICE_LOG_CHECKER_HPP_
+
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <tier4_system_msgs/msg/service_log.hpp>
+
+#include <string>
+#include <unordered_map>
+
+class ServiceLogChecker : public rclcpp::Node
+{
+public:
+  ServiceLogChecker();
+
+private:
+  using ServiceLog = tier4_system_msgs::msg::ServiceLog;
+  rclcpp::Subscription<ServiceLog>::SharedPtr sub_;
+  diagnostic_updater::Updater diagnostics_;
+  void on_service_log(const ServiceLog::ConstSharedPtr msg);
+  void set_success(const ServiceLog & msg);
+  void set_error(const ServiceLog & msg, const std::string & log);
+  void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  std::unordered_map<std::string, std::string> errors_;
+};
+
+#endif  // SERVICE_LOG_CHECKER_HPP_

common/component_interface_utils/CMakeLists.txt

@@ -3,5 +3,4 @@ project(component_interface_utils)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
-ament_export_dependencies(tier4_system_msgs)
 ament_auto_package()

common/component_interface_utils/package.xml

@@ -13,6 +13,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
   <build_depend>autoware_cmake</build_depend>
+  <build_depend>tier4_system_msgs</build_depend>
 
   <depend>autoware_adapi_v1_msgs</depend>
   <depend>rclcpp</depend>

launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml

@@ -36,6 +36,7 @@
 
         /autoware/system/node_alive_monitoring: default
         /autoware/system/emergency_stop_operation: default
+        /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" }
         /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "none", spf_at: "none" }
 
         /autoware/vehicle/node_alive_monitoring: default
@@ -46,5 +47,6 @@
 
         /autoware/system/node_alive_monitoring: default
         /autoware/system/emergency_stop_operation: default
+        /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" }
 
         /autoware/vehicle/node_alive_monitoring: default

launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml

@@ -36,6 +36,7 @@
 
         /autoware/system/node_alive_monitoring: default
         /autoware/system/emergency_stop_operation: default
+        /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" }
         # /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "error", spf_at: "none" }
 
         /autoware/vehicle/node_alive_monitoring: default
@@ -46,5 +47,6 @@
 
         /autoware/system/node_alive_monitoring: default
         /autoware/system/emergency_stop_operation: default
+        /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" }
 
         /autoware/vehicle/node_alive_monitoring: default

launch/tier4_system_launch/launch/system.launch.xml

@@ -25,6 +25,11 @@
       </include>
     </group>
 
+    <!-- Service Log Checker -->
+    <group>
+      <include file="$(find-pkg-share component_interface_tools)/launch/service_log_checker.launch.xml"/>
+    </group>
+
     <!-- Component State Monitor -->
     <group>
       <include file="$(find-pkg-share component_state_monitor)/launch/component_state_monitor.launch.py">

system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml

@@ -20,6 +20,12 @@
           contains: [": emergency_stop_operation"]
           timeout: 1.0
 
+        service_log_checker:
+          type: diagnostic_aggregator/GenericAnalyzer
+          path: service_log_checker
+          contains: ["service_log_checker"]
+          timeout: 5.0
+
         resource_monitoring:
           type: diagnostic_aggregator/AnalyzerGroup
           path: resource_monitoring

system/system_error_monitor/config/system_error_monitor.param.yaml

@@ -36,6 +36,7 @@
 
         /autoware/system/node_alive_monitoring: default
         /autoware/system/emergency_stop_operation: default
+        /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" }
         /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "none", spf_at: "none" }
 
         /autoware/vehicle/node_alive_monitoring: default
@@ -46,5 +47,6 @@
 
         /autoware/system/node_alive_monitoring: default
         /autoware/system/emergency_stop_operation: default
+        /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" }
 
         /autoware/vehicle/node_alive_monitoring: default

system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml

@@ -36,6 +36,7 @@
 
         /autoware/system/node_alive_monitoring: default
         /autoware/system/emergency_stop_operation: default
+        /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" }
         # /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "error", spf_at: "none" }
 
         /autoware/vehicle/node_alive_monitoring: default
@@ -46,5 +47,6 @@
 
         /autoware/system/node_alive_monitoring: default
         /autoware/system/emergency_stop_operation: default
+        /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" }
 
         /autoware/vehicle/node_alive_monitoring: default