From adad56a5bab9c1bab3fa958a38772a01437ac263 Mon Sep 17 00:00:00 2001 From: Yuxuan Liu <619684051@qq.com> Date: Sat, 14 Oct 2023 06:32:59 +0800 Subject: [PATCH 1/2] feat(duplicated_node_checker): add packages to check duplication of node names in ros2 (#5286) * add implementation for duplicated node checking Signed-off-by: Owen-Liuyuxuan * update the default parameters of system_error_monitor to include results from duplication check Signed-off-by: Owen-Liuyuxuan * style(pre-commit): autofix * fix typo in readme Signed-off-by: Owen-Liuyuxuan * update license Signed-off-by: Owen-Liuyuxuan * change module to the system module Signed-off-by: Owen-Liuyuxuan * follow json schema: 1. update code to start without default 2. add schema/config/readme/launch accordingly Signed-off-by: Owen-Liuyuxuan * add duplicated node checker to launch Signed-off-by: Owen-Liuyuxuan * style(pre-commit): autofix * fix var name to config for uniform launch Signed-off-by: Owen-Liuyuxuan * Update system/duplicated_node_checker/README.md * Update system/duplicated_node_checker/README.md --------- Signed-off-by: Owen-Liuyuxuan Co-authored-by: Owen-Liuyuxuan Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> --- .../launch/system.launch.xml | 7 ++ system/duplicated_node_checker/CMakeLists.txt | 20 +++++ system/duplicated_node_checker/README.md | 37 +++++++++ .../config/duplicated_node_checker.param.yaml | 3 + .../duplicated_node_checker_core.hpp | 54 +++++++++++++ .../launch/duplicated_node_checker.launch.xml | 7 ++ system/duplicated_node_checker/package.xml | 24 ++++++ .../duplicated_node_checker.schema.json | 31 +++++++ .../src/duplicated_node_checker_core.cpp | 81 +++++++++++++++++++ .../diagnostic_aggregator/system.param.yaml | 6 ++ .../config/system_error_monitor.param.yaml | 1 + ...ror_monitor.planning_simulation.param.yaml | 1 + 12 files changed, 272 insertions(+) create mode 100644 system/duplicated_node_checker/CMakeLists.txt create mode 100644 system/duplicated_node_checker/README.md create mode 100644 system/duplicated_node_checker/config/duplicated_node_checker.param.yaml create mode 100644 system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp create mode 100644 system/duplicated_node_checker/launch/duplicated_node_checker.launch.xml create mode 100644 system/duplicated_node_checker/package.xml create mode 100644 system/duplicated_node_checker/schema/duplicated_node_checker.schema.json create mode 100644 system/duplicated_node_checker/src/duplicated_node_checker_core.cpp diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 016d613cfa72d..2de6a61547498 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -3,6 +3,7 @@ + @@ -84,6 +85,12 @@ + + + + + + diff --git a/system/duplicated_node_checker/CMakeLists.txt b/system/duplicated_node_checker/CMakeLists.txt new file mode 100644 index 0000000000000..6a8089fa85c96 --- /dev/null +++ b/system/duplicated_node_checker/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.14) +project(duplicated_node_checker) + +find_package(autoware_cmake REQUIRED) +autoware_package() +find_package(rclcpp REQUIRED) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/duplicated_node_checker_core.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "duplicated_node_checker::DuplicatedNodeChecker" + EXECUTABLE duplicated_node_checker_node +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/duplicated_node_checker/README.md b/system/duplicated_node_checker/README.md new file mode 100644 index 0000000000000..81b53e36c9f6d --- /dev/null +++ b/system/duplicated_node_checker/README.md @@ -0,0 +1,37 @@ +# Duplicated Node Checker + +## Purpose + +This node monitors the ROS 2 environments and detect duplication of node names in the environment. +The result is published as diagnostics. + +### Standalone Startup + +```bash +ros2 launch duplicated_node_checker duplicated_node_checker.launch.xml +``` + +## Inner-workings / Algorithms + +The types of topic status and corresponding diagnostic status are following. + +| Duplication status | Diagnostic status | Description | +| --------------------- | ----------------- | -------------------------- | +| `OK` | OK | No duplication is detected | +| `Duplicated Detected` | ERROR | Duplication is detected | + +## Inputs / Outputs + +### Output + +| Name | Type | Description | +| -------------- | --------------------------------- | ------------------- | +| `/diagnostics` | `diagnostic_msgs/DiagnosticArray` | Diagnostics outputs | + +## Parameters + +{{ json_to_markdown("system/duplicated_node_checker/schema/duplicated_node_checker.schema.json") }} + +## Assumptions / Known limits + +TBD. diff --git a/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml new file mode 100644 index 0000000000000..5b8c019de5a20 --- /dev/null +++ b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + update_rate: 10.0 diff --git a/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp b/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp new file mode 100644 index 0000000000000..ec086058e9041 --- /dev/null +++ b/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 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 DUPLICATED_NODE_CHECKER__DUPLICATED_NODE_CHECKER_CORE_HPP_ +#define DUPLICATED_NODE_CHECKER__DUPLICATED_NODE_CHECKER_CORE_HPP_ + +#include +#include + +#include +#include +#include + +namespace duplicated_node_checker +{ +class DuplicatedNodeChecker : public rclcpp::Node +{ +public: + explicit DuplicatedNodeChecker(const rclcpp::NodeOptions & node_options); + std::vector findIdenticalNames(const std::vector input_name_lists) + { + std::unordered_set unique_names; + std::vector identical_names; + for (auto name : input_name_lists) { + if (unique_names.find(name) != unique_names.end()) { + identical_names.push_back(name); + } else { + unique_names.insert(name); + } + } + return identical_names; + } + +private: + void onTimer(); + void produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); + + diagnostic_updater::Updater updater_{this}; + rclcpp::TimerBase::SharedPtr timer_; +}; +} // namespace duplicated_node_checker + +#endif // DUPLICATED_NODE_CHECKER__DUPLICATED_NODE_CHECKER_CORE_HPP_ diff --git a/system/duplicated_node_checker/launch/duplicated_node_checker.launch.xml b/system/duplicated_node_checker/launch/duplicated_node_checker.launch.xml new file mode 100644 index 0000000000000..87f41f045545d --- /dev/null +++ b/system/duplicated_node_checker/launch/duplicated_node_checker.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/system/duplicated_node_checker/package.xml b/system/duplicated_node_checker/package.xml new file mode 100644 index 0000000000000..e2ba225b16330 --- /dev/null +++ b/system/duplicated_node_checker/package.xml @@ -0,0 +1,24 @@ + + + + duplicated_node_checker + 1.0.0 + A package to find out nodes with common names + Shumpei Wakabayashi + yliuhb + Apache 2.0 + + ament_cmake + autoware_cmake + + diagnostic_updater + rclcpp + rclcpp_components + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/system/duplicated_node_checker/schema/duplicated_node_checker.schema.json b/system/duplicated_node_checker/schema/duplicated_node_checker.schema.json new file mode 100644 index 0000000000000..6ee933ecf1a77 --- /dev/null +++ b/system/duplicated_node_checker/schema/duplicated_node_checker.schema.json @@ -0,0 +1,31 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Duplicated Node Checker", + "type": "object", + "definitions": { + "duplicated_node_checker": { + "type": "object", + "properties": { + "update_rate": { + "type": "number", + "default": 10, + "exclusiveMinimum": 2, + "description": "The scanning and update frequency of the checker." + } + }, + "required": ["update_rate"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/duplicated_node_checker" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp new file mode 100644 index 0000000000000..c9aa483724532 --- /dev/null +++ b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp @@ -0,0 +1,81 @@ +// Copyright 2023 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 "duplicated_node_checker/duplicated_node_checker_core.hpp" + +#include +#include + +#include +#include +#include + +namespace duplicated_node_checker +{ + +DuplicatedNodeChecker::DuplicatedNodeChecker(const rclcpp::NodeOptions & node_options) +: Node("duplicated_node_checker", node_options) +{ + double update_rate = declare_parameter("update_rate"); + updater_.setHardwareID("duplicated_node_checker"); + updater_.add("duplicated_node_checker", this, &DuplicatedNodeChecker::produceDiagnostics); + + const auto period_ns = rclcpp::Rate(update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&DuplicatedNodeChecker::onTimer, this)); +} + +std::string get_fullname_from_name_ns_pair(std::pair name_and_ns_pair) +{ + std::string full_name; + const std::string & name = name_and_ns_pair.first; + const std::string & ns = name_and_ns_pair.second; + if (ns.back() == '/') { + full_name = ns + name; + } else { + full_name = ns + "/" + name; + } + return full_name; +} + +void DuplicatedNodeChecker::onTimer() +{ + updater_.force_update(); +} + +void DuplicatedNodeChecker::produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + using diagnostic_msgs::msg::DiagnosticStatus; + + std::vector node_names = this->get_node_names(); + std::vector identical_names = findIdenticalNames(node_names); + std::string msg; + int level; + if (identical_names.size() > 0) { + msg = "Error"; + level = DiagnosticStatus::ERROR; + for (auto name : identical_names) { + stat.add("Duplicated Node Name", name); + } + } else { + msg = "OK"; + level = DiagnosticStatus::OK; + } + stat.summary(level, msg); +} + +} // namespace duplicated_node_checker + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(duplicated_node_checker::DuplicatedNodeChecker) 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 87cf767accc06..f6e13012957aa 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml @@ -26,6 +26,12 @@ contains: ["service_log_checker"] timeout: 5.0 + duplicated_node_checker: + type: diagnostic_aggregator/GenericAnalyzer + path: duplicated_node_checker + contains: ["duplicated_node_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 312a2e6186c42..1778f6594f0c3 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.param.yaml @@ -39,6 +39,7 @@ /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/system/duplicated_node_checker: default /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 88431dc406fe3..c396e2e9f5ed8 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 @@ -38,6 +38,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/duplicated_node_checker: default # /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "error", spf_at: "none" } /autoware/vehicle/node_alive_monitoring: default From aafc0e8bb084f05e3369110e4989a0d3189714b8 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Mon, 23 Oct 2023 20:27:52 +0900 Subject: [PATCH 2/2] feat(duplicated_node_checker): add duplicated node names to msg (#5382) * add duplicated node names to msg Signed-off-by: kyoichi-sugahara * align with launcher repository Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../config/duplicated_node_checker.param.yaml | 1 + .../duplicated_node_checker_core.hpp | 1 + .../src/duplicated_node_checker_core.cpp | 9 ++++++++- .../system_error_monitor.planning_simulation.param.yaml | 1 + 4 files changed, 11 insertions(+), 1 deletion(-) diff --git a/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml index 5b8c019de5a20..54b4f691b6eb1 100644 --- a/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml +++ b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml @@ -1,3 +1,4 @@ /**: ros__parameters: update_rate: 10.0 + add_duplicated_node_names_to_msg: false # if true, duplicated node names are added to msg diff --git a/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp b/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp index ec086058e9041..9d806754f3c20 100644 --- a/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp +++ b/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp @@ -48,6 +48,7 @@ class DuplicatedNodeChecker : public rclcpp::Node diagnostic_updater::Updater updater_{this}; rclcpp::TimerBase::SharedPtr timer_; + bool add_duplicated_node_names_to_msg_; }; } // namespace duplicated_node_checker diff --git a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp index c9aa483724532..46c02d9d6e133 100644 --- a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp +++ b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp @@ -28,6 +28,7 @@ DuplicatedNodeChecker::DuplicatedNodeChecker(const rclcpp::NodeOptions & node_op : Node("duplicated_node_checker", node_options) { double update_rate = declare_parameter("update_rate"); + add_duplicated_node_names_to_msg_ = declare_parameter("add_duplicated_node_names_to_msg"); updater_.setHardwareID("duplicated_node_checker"); updater_.add("duplicated_node_checker", this, &DuplicatedNodeChecker::produceDiagnostics); @@ -63,8 +64,14 @@ void DuplicatedNodeChecker::produceDiagnostics(diagnostic_updater::DiagnosticSta std::string msg; int level; if (identical_names.size() > 0) { - msg = "Error"; level = DiagnosticStatus::ERROR; + msg = "Error: Duplicated nodes detected"; + if (add_duplicated_node_names_to_msg_) { + std::set unique_identical_names(identical_names.begin(), identical_names.end()); + for (const auto & name : unique_identical_names) { + msg += " " + name; + } + } for (auto name : identical_names) { stat.add("Duplicated Node Name", name); } 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 c396e2e9f5ed8..9784259490ec2 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 @@ -40,6 +40,7 @@ /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/system/duplicated_node_checker: default # /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "error", spf_at: "none" } + # /autoware/system/duplicated_node_checker: default /autoware/vehicle/node_alive_monitoring: default