diff --git a/common/component_interface_tools/CMakeLists.txt b/common/component_interface_tools/CMakeLists.txt new file mode 100644 index 0000000000000..a5ebc29463bec --- /dev/null +++ b/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) diff --git a/common/component_interface_tools/launch/service_log_checker.launch.xml b/common/component_interface_tools/launch/service_log_checker.launch.xml new file mode 100644 index 0000000000000..f3099b3238096 --- /dev/null +++ b/common/component_interface_tools/launch/service_log_checker.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/common/component_interface_tools/package.xml b/common/component_interface_tools/package.xml new file mode 100644 index 0000000000000..a1eba3cb41e09 --- /dev/null +++ b/common/component_interface_tools/package.xml @@ -0,0 +1,28 @@ + + + + component_interface_tools + 0.1.0 + The component_interface_tools package + Takagi, Isamu + yabuta + Kah Hooi Tan + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + diagnostic_updater + fmt + rclcpp + tier4_system_msgs + yaml_cpp_vendor + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/component_interface_tools/src/service_log_checker.cpp b/common/component_interface_tools/src/service_log_checker.cpp new file mode 100644 index 0000000000000..ce89573356412 --- /dev/null +++ b/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 + +#include +#include + +#define FMT_HEADER_ONLY +#include + +ServiceLogChecker::ServiceLogChecker() : Node("service_log_checker"), diagnostics_(this) +{ + sub_ = create_subscription( + "/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(); + if (!success) { + const auto message = status["message"].as(); + const auto code = status["code"].as(); + 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(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); +} diff --git a/common/component_interface_tools/src/service_log_checker.hpp b/common/component_interface_tools/src/service_log_checker.hpp new file mode 100644 index 0000000000000..32c7f02e757c6 --- /dev/null +++ b/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 +#include + +#include + +#include +#include + +class ServiceLogChecker : public rclcpp::Node +{ +public: + ServiceLogChecker(); + +private: + using ServiceLog = tier4_system_msgs::msg::ServiceLog; + rclcpp::Subscription::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 errors_; +}; + +#endif // SERVICE_LOG_CHECKER_HPP_ diff --git a/common/component_interface_utils/CMakeLists.txt b/common/component_interface_utils/CMakeLists.txt index d753b4e92d218..d4f6343e381c7 100644 --- a/common/component_interface_utils/CMakeLists.txt +++ b/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() diff --git a/common/component_interface_utils/package.xml b/common/component_interface_utils/package.xml index 78620dab76cfe..90602a65bd1b0 100644 --- a/common/component_interface_utils/package.xml +++ b/common/component_interface_utils/package.xml @@ -13,6 +13,7 @@ ament_cmake_auto autoware_cmake + tier4_system_msgs autoware_adapi_v1_msgs rclcpp diff --git a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml b/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml index b9de5cc4f2e13..71dc2ac600685 100644 --- a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml +++ b/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 diff --git a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml b/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml index bf40c334f6498..9708456df4fe7 100644 --- a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml +++ b/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 diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index ff3ebae90e847..2400b8b449a6a 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -25,6 +25,11 @@ + + + + + diff --git a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml index 01e973b8ff26a..87cf767accc06 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml +++ b/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 diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml index b9de5cc4f2e13..71dc2ac600685 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/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 diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml index bf40c334f6498..9708456df4fe7 100644 --- a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml +++ b/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