Merge pull request #1020 from tier4/feat/duplicate_check

feat(duplicated_node_checker): add new package

launch/tier4_system_launch/launch/system.launch.xml

@@ -3,6 +3,7 @@
   <!-- Parameter files -->
   <arg name="component_state_monitor_topic_path"/>
   <arg name="emergency_handler_param_path"/>
+  <arg name="duplicated_node_checker_param_path"/>
   <arg name="mrm_comfortable_stop_operator_param_path"/>
   <arg name="mrm_emergency_stop_operator_param_path"/>
   <arg name="system_error_monitor_param_path"/>
@@ -84,6 +85,12 @@
       </include>
     </group>
 
+    <group>
+      <include file="$(find-pkg-share duplicated_node_checker)/launch/duplicated_node_checker.launch.xml">
+        <arg name="config_file" value="$(var duplicated_node_checker_param_path)"/>
+      </include>
+    </group>
+
     <!-- MRM Operator -->
     <group>
       <include file="$(find-pkg-share mrm_comfortable_stop_operator)/launch/mrm_comfortable_stop_operator.launch.py">

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
+)

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.

system/duplicated_node_checker/config/duplicated_node_checker.param.yaml

@@ -0,0 +1,4 @@
+/**:
+  ros__parameters:
+    update_rate: 10.0
+    add_duplicated_node_names_to_msg: false # if true, duplicated node names are added to msg

system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp

@@ -0,0 +1,55 @@
+// 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 <diagnostic_updater/diagnostic_updater.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <string>
+#include <unordered_set>
+#include <vector>
+
+namespace duplicated_node_checker
+{
+class DuplicatedNodeChecker : public rclcpp::Node
+{
+public:
+  explicit DuplicatedNodeChecker(const rclcpp::NodeOptions & node_options);
+  std::vector<std::string> findIdenticalNames(const std::vector<std::string> input_name_lists)
+  {
+    std::unordered_set<std::string> unique_names;
+    std::vector<std::string> 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_;
+  bool add_duplicated_node_names_to_msg_;
+};
+}  // namespace duplicated_node_checker
+
+#endif  // DUPLICATED_NODE_CHECKER__DUPLICATED_NODE_CHECKER_CORE_HPP_

system/duplicated_node_checker/launch/duplicated_node_checker.launch.xml

@@ -0,0 +1,7 @@
+<launch>
+  <arg name="config_file" default="$(find-pkg-share duplicated_node_checker)/config/duplicated_node_checker.param.yaml"/>
+
+  <node pkg="duplicated_node_checker" exec="duplicated_node_checker_node" name="duplicated_node_checker" output="screen">
+    <param from="$(var config_file)"/>
+  </node>
+</launch>

system/duplicated_node_checker/package.xml

@@ -0,0 +1,24 @@
+<?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>duplicated_node_checker</name>
+  <version>1.0.0</version>
+  <description>A package to find out nodes with common names</description>
+  <maintainer email="[email protected]">Shumpei Wakabayashi</maintainer>
+  <maintainer email="[email protected]">yliuhb</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>diagnostic_updater</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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": ["/**"]
+}

system/duplicated_node_checker/src/duplicated_node_checker_core.cpp

@@ -0,0 +1,88 @@
+// 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 <diagnostic_updater/diagnostic_updater.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <chrono>
+#include <string>
+#include <vector>
+
+namespace duplicated_node_checker
+{
+
+DuplicatedNodeChecker::DuplicatedNodeChecker(const rclcpp::NodeOptions & node_options)
+: Node("duplicated_node_checker", node_options)
+{
+  double update_rate = declare_parameter<double>("update_rate");
+  add_duplicated_node_names_to_msg_ = declare_parameter<bool>("add_duplicated_node_names_to_msg");
+  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<std::string, std::string> 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<std::string> node_names = this->get_node_names();
+  std::vector<std::string> identical_names = findIdenticalNames(node_names);
+  std::string msg;
+  int level;
+  if (identical_names.size() > 0) {
+    level = DiagnosticStatus::ERROR;
+    msg = "Error: Duplicated nodes detected";
+    if (add_duplicated_node_names_to_msg_) {
+      std::set<std::string> 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);
+    }
+  } else {
+    msg = "OK";
+    level = DiagnosticStatus::OK;
+  }
+  stat.summary(level, msg);
+}
+
+}  // namespace duplicated_node_checker
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(duplicated_node_checker::DuplicatedNodeChecker)

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

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
 

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

@@ -38,7 +38,9 @@
         /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/system/duplicated_node_checker: default
 
         /autoware/vehicle/node_alive_monitoring: default