feat(fault injection): change for diagnostic graph aggregator (autowa…

…refoundation#6750) * feat: change to adapt diagnostic graph aggregator Signed-off-by: Keisuke Shima <[email protected]> * Change the configuration to adapt to both system_error_monitor and diagnostic_graph_aggregator Signed-off-by: Keisuke Shima <[email protected]> * style(pre-commit): autofix * pre-commit fix Signed-off-by: Keisuke Shima <[email protected]> * style(pre-commit): autofix * spell check fix Signed-off-by: Keisuke Shima <[email protected]> * clang-tidy fix Signed-off-by: Keisuke Shima <[email protected]> * style(pre-commit): autofix * pre-commit fix Signed-off-by: Keisuke Shima <[email protected]> * style(pre-commit): autofix * fix datatype Signed-off-by: Keisuke Shima <[email protected]> * cleanup code Signed-off-by: Keisuke Shima <[email protected]> --------- Signed-off-by: Keisuke Shima <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Tomohito ANDO <[email protected]>
tier4 · Apr 11, 2024 · 36db135 · 36db135
1 parent aab197d
commit 36db135
Show file tree

Hide file tree

Showing 7 changed files with 298 additions and 87 deletions.
diff --git a/simulator/fault_injection/README.md b/simulator/fault_injection/README.md
@@ -27,7 +27,9 @@ launch_test test/test_fault_injection_node.test.py
 
 ### Output
 
-None.
+| Name           | Type                                    | Description       |
+| -------------- | --------------------------------------- | ----------------- |
+| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Dummy diagnostics |
 
 ## Parameters
 

diff --git a/simulator/fault_injection/config/fault_injection.param.yaml b/simulator/fault_injection/config/fault_injection.param.yaml
@@ -1,37 +1,37 @@
 /**:
   ros__parameters:
     event_diag_list:
-      vehicle_is_out_of_lane: "lane_departure"
-      trajectory_deviation_is_high: "trajectory_deviation"
-      localization_matching_score_is_low: "ndt_scan_matcher"
-      localization_accuracy_is_low: "localization_accuracy"
-      map_version_is_different: "map_version"
-      trajectory_is_invalid: "trajectory_point_validation"
-      cpu_temperature_is_high: "CPU Temperature"
-      cpu_usage_is_high: "CPU Usage"
-      cpu_is_in_thermal_throttling: "CPU Thermal Throttling"
-      storage_temperature_is_high: "HDD Temperature"
-      storage_usage_is_high: "HDD Usage"
-      network_usage_is_high: "Network Usage"
-      clock_error_is_large: "NTP Offset"
-      gpu_temperature_is_high: "GPU Temperature"
-      gpu_usage_is_high: "GPU Usage"
-      gpu_memory_usage_is_high: "GPU Memory Usage"
-      gpu_is_in_thermal_throttling: "GPU Thermal Throttling"
-      driving_recorder_storage_error: "driving_recorder"
-      debug_data_logger_storage_error: "bagpacker"
-      emergency_stop_operation: "emergency_stop_operation"
-      vehicle_error_occurred: "vehicle_errors"
-      vehicle_ecu_connection_is_lost: "can_bus_connection"
-      obstacle_crash_sensor_is_activated: "obstacle_crash"
+      vehicle_is_out_of_lane: ": lane_departure"
+      trajectory_deviation_is_high: ": trajectory_deviation"
+      localization_matching_score_is_low: ": ndt_scan_matcher"
+      localization_accuracy_is_low: ": localization_error_ellipse"
+      map_version_is_different: ": map_version"
+      trajectory_is_invalid: ": trajectory_point_validation"
+      cpu_temperature_is_high: ": CPU Temperature"
+      cpu_usage_is_high: ": CPU Usage"
+      cpu_is_in_thermal_throttling: ": CPU Thermal Throttling"
+      storage_temperature_is_high: ": HDD Temperature"
+      storage_usage_is_high: ": HDD Usage"
+      network_usage_is_high: ": Network Usage"
+      clock_error_is_large: ": NTP Offset"
+      gpu_temperature_is_high: ": GPU Temperature"
+      gpu_usage_is_high: ": GPU Usage"
+      gpu_memory_usage_is_high: ": GPU Memory Usage"
+      gpu_is_in_thermal_throttling: ": GPU Thermal Throttling"
+      driving_recorder_storage_error: ": driving_recorder"
+      debug_data_logger_storage_error: ": bagpacker"
+      emergency_stop_operation: ": emergency_stop_operation"
+      vehicle_error_occurred: ": vehicle_errors"
+      vehicle_ecu_connection_is_lost: ": can_bus_connection"
+      obstacle_crash_sensor_is_activated: ": obstacle_crash"
       /control/command_gate/node_alive_monitoring: "vehicle_cmd_gate: heartbeat"
-      /control/autonomous_driving/node_alive_monitoring: "control_topic_status"
+      /control/autonomous_driving/node_alive_monitoring: ": control_topic_status"
       /control/external_command_selector/node_alive_monitoring: "external_cmd_selector: heartbeat"
-      /localization/node_alive_monitoring: "localization_topic_status"
-      /map/node_alive_monitoring: "map_topic_status"
-      /planning/node_alive_monitoring: "planning_topic_status"
-      /sensing/lidar/node_alive_monitoring: "lidar_topic_status"
-      /sensing/imu/node_alive_monitoring: "imu_connection"
-      /sensing/gnss/node_alive_monitoring: "gnss_connection"
-      /system/node_alive_monitoring: "system_topic_status"
-      /vehicle/node_alive_monitoring: "vehicle_topic_status"
+      /localization/node_alive_monitoring: ": localization_topic_status"
+      /map/node_alive_monitoring: ": map_topic_status"
+      /planning/node_alive_monitoring: ": planning_topic_status"
+      /sensing/lidar/node_alive_monitoring: ": lidar_topic_status"
+      /sensing/imu/node_alive_monitoring: ": imu_connection"
+      /sensing/gnss/node_alive_monitoring: ": gnss_connection"
+      /system/node_alive_monitoring: ": system_topic_status"
+      /vehicle/node_alive_monitoring: ": vehicle_topic_status"
diff --git a/simulator/fault_injection/include/fault_injection/fault_injection_diag_updater.hpp b/simulator/fault_injection/include/fault_injection/fault_injection_diag_updater.hpp
@@ -0,0 +1,244 @@
+// Copyright 2024 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.
+
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#ifndef FAULT_INJECTION__FAULT_INJECTION_DIAG_UPDATER_HPP_
+#define FAULT_INJECTION__FAULT_INJECTION_DIAG_UPDATER_HPP_
+
+#include <diagnostic_updater/diagnostic_updater.hpp>
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace fault_injection
+{
+class FaultInjectionDiagUpdater : public diagnostic_updater::DiagnosticTaskVector
+{
+public:
+  template <class NodeT>
+  explicit FaultInjectionDiagUpdater(NodeT node, double period = 1.0)
+  : FaultInjectionDiagUpdater(
+      node->get_node_base_interface(), node->get_node_clock_interface(),
+      node->get_node_logging_interface(), node->get_node_timers_interface(),
+      node->get_node_topics_interface(), period)
+  {
+  }
+
+  FaultInjectionDiagUpdater(
+    const std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> & base_interface,
+    const std::shared_ptr<rclcpp::node_interfaces::NodeClockInterface> & clock_interface,
+    const std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface> & logging_interface,
+    std::shared_ptr<rclcpp::node_interfaces::NodeTimersInterface> timers_interface,
+    std::shared_ptr<rclcpp::node_interfaces::NodeTopicsInterface> topics_interface,
+    double period = 1.0)
+  : base_interface_(base_interface),
+    timers_interface_(std::move(timers_interface)),
+    clock_(clock_interface->get_clock()),
+    period_(rclcpp::Duration::from_nanoseconds(static_cast<rcl_duration_value_t>(period * 1e9))),
+    publisher_(rclcpp::create_publisher<diagnostic_msgs::msg::DiagnosticArray>(
+      topics_interface, "/diagnostics", 1)),
+    logger_(logging_interface->get_logger()),
+    node_name_(base_interface->get_name())
+  {
+    reset_timer();
+  }
+
+  /**
+   * \brief Returns the interval between updates.
+   */
+  [[nodiscard]] auto get_period() const { return period_; }
+
+  /**
+   * \brief Sets the period as a rclcpp::Duration
+   */
+  void set_period(const rclcpp::Duration & period)
+  {
+    period_ = period;
+    reset_timer();
+  }
+
+  /**
+   * \brief Sets the period given a value in seconds
+   */
+  void set_period(double period)
+  {
+    set_period(rclcpp::Duration::from_nanoseconds(static_cast<rcl_duration_value_t>(period * 1e9)));
+  }
+
+  /**
+   * \brief Forces to send out an update for all known DiagnosticStatus.
+   */
+  void force_update() { update(); }
+
+  /**
+   * \brief Output a message on all the known DiagnosticStatus.
+   *
+   * Useful if something drastic is happening such as shutdown or a
+   * self-test.
+   *
+   * \param lvl Level of the diagnostic being output.
+   *
+   * \param msg Status message to output.
+   */
+  void broadcast(int lvl, const std::string & msg)
+  {
+    std::vector<diagnostic_msgs::msg::DiagnosticStatus> status_vec;
+
+    const std::vector<DiagnosticTaskInternal> & tasks = getTasks();
+    for (const auto & task : tasks) {
+      diagnostic_updater::DiagnosticStatusWrapper status;
+
+      status.name = task.getName();
+      status.summary(lvl, msg);
+
+      status_vec.push_back(status);
+    }
+
+    publish(status_vec);
+  }
+
+  void set_hardware_id_f(const char * format, ...)
+  {
+    va_list va;
+    const int k_buffer_size = 1000;
+    char buff[1000];  // @todo This could be done more elegantly.
+    va_start(va, format);
+    if (vsnprintf(buff, k_buffer_size, format, va) >= k_buffer_size) {
+      RCLCPP_DEBUG(logger_, "Really long string in diagnostic_updater::setHardwareIDf.");
+    }
+    hardware_id_ = std::string(buff);
+    va_end(va);
+  }
+
+  void set_hardware_id(const std::string & hardware_id) { hardware_id_ = hardware_id; }
+
+private:
+  void reset_timer()
+  {
+    update_timer_ = rclcpp::create_timer(
+      base_interface_, timers_interface_, clock_, period_,
+      std::bind(&FaultInjectionDiagUpdater::update, this));
+  }
+
+  /**
+   * \brief Causes the diagnostics to update if the inter-update interval
+   * has been exceeded.
+   */
+  void update()
+  {
+    if (rclcpp::ok()) {
+      std::vector<diagnostic_msgs::msg::DiagnosticStatus> status_vec;
+
+      std::unique_lock<std::mutex> lock(
+        lock_);  // Make sure no adds happen while we are processing here.
+      const std::vector<DiagnosticTaskInternal> & tasks = getTasks();
+      for (const auto & task : tasks) {
+        diagnostic_updater::DiagnosticStatusWrapper status;
+
+        status.name = task.getName();
+        status.level = 2;
+        status.message = "No message was set";
+        status.hardware_id = hardware_id_;
+
+        task.run(status);
+
+        status_vec.push_back(status);
+      }
+
+      publish(status_vec);
+    }
+  }
+
+  /**
+   * Publishes a single diagnostic status.
+   */
+  void publish(diagnostic_msgs::msg::DiagnosticStatus & stat)
+  {
+    std::vector<diagnostic_msgs::msg::DiagnosticStatus> status_vec;
+    status_vec.push_back(stat);
+    publish(status_vec);
+  }
+
+  /**
+   * Publishes a vector of diagnostic statuses.
+   */
+  void publish(std::vector<diagnostic_msgs::msg::DiagnosticStatus> & status_vec)
+  {
+    diagnostic_msgs::msg::DiagnosticArray msg;
+    msg.status = status_vec;
+    msg.header.stamp = clock_->now();
+    publisher_->publish(msg);
+  }
+
+  /**
+   * Causes a placeholder DiagnosticStatus to be published as soon as a
+   * diagnostic task is added to the Updater.
+   */
+  void addedTaskCallback(DiagnosticTaskInternal & task) override
+  {
+    diagnostic_updater::DiagnosticStatusWrapper stat;
+    stat.name = task.getName();
+    stat.summary(0, "Node starting up");
+    publish(stat);
+  }
+
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr base_interface_;
+  rclcpp::node_interfaces::NodeTimersInterface::SharedPtr timers_interface_;
+  rclcpp::Clock::SharedPtr clock_;
+  rclcpp::Duration period_;
+  rclcpp::TimerBase::SharedPtr update_timer_;
+  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr publisher_;
+  rclcpp::Logger logger_;
+
+  std::string hardware_id_;
+  std::string node_name_;
+};
+}  // namespace fault_injection
+
+#endif  // FAULT_INJECTION__FAULT_INJECTION_DIAG_UPDATER_HPP_
diff --git a/simulator/fault_injection/include/fault_injection/fault_injection_node.hpp b/simulator/fault_injection/include/fault_injection/fault_injection_node.hpp
@@ -16,8 +16,8 @@
 #define FAULT_INJECTION__FAULT_INJECTION_NODE_HPP_
 
 #include "fault_injection/diagnostic_storage.hpp"
+#include "fault_injection/fault_injection_diag_updater.hpp"
 
-#include <diagnostic_updater/diagnostic_updater.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <tier4_simulation_msgs/msg/simulation_events.hpp>
@@ -36,23 +36,15 @@ class FaultInjectionNode : public rclcpp::Node
 
 private:
   // Subscribers
-  void onSimulationEvents(const SimulationEvents::ConstSharedPtr msg);
+  void on_simulation_events(const SimulationEvents::ConstSharedPtr msg);
   rclcpp::Subscription<SimulationEvents>::SharedPtr sub_simulation_events_;
 
-  // Parameter Server
-  rcl_interfaces::msg::SetParametersResult onSetParam(
-    const std::vector<rclcpp::Parameter> & params);
-  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
-
-  void updateEventDiag(
+  void update_event_diag(
     diagnostic_updater::DiagnosticStatusWrapper & wrap, const std::string & event_name);
 
-  void addDiag(
-    const diagnostic_msgs::msg::DiagnosticStatus & status, diagnostic_updater::Updater & updater);
-
-  std::vector<DiagConfig> readEventDiagList();
+  std::vector<DiagConfig> read_event_diag_list();
 
-  diagnostic_updater::Updater updater_;
+  FaultInjectionDiagUpdater updater_;
 
   DiagnosticStorage diagnostic_storage_;
 };