diff --git a/simulator/fault_injection/README.md b/simulator/fault_injection/README.md index cbac41ebfb640..cb3d6fea96a7a 100644 --- 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 index 1a57b852f7361..e9e90a336348e 100644 --- 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 new file mode 100644 index 0000000000000..948c2b45f6615 --- /dev/null +++ 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 + +#include +#include +#include +#include + +namespace fault_injection +{ +class FaultInjectionDiagUpdater : public diagnostic_updater::DiagnosticTaskVector +{ +public: + template + 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 & base_interface, + const std::shared_ptr & clock_interface, + const std::shared_ptr & logging_interface, + std::shared_ptr timers_interface, + std::shared_ptr 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(period * 1e9))), + publisher_(rclcpp::create_publisher( + 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(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 status_vec; + + const std::vector & 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 status_vec; + + std::unique_lock lock( + lock_); // Make sure no adds happen while we are processing here. + const std::vector & 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 status_vec; + status_vec.push_back(stat); + publish(status_vec); + } + + /** + * Publishes a vector of diagnostic statuses. + */ + void publish(std::vector & 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::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 index 0f578ff343a17..999d18b38c7b0 100644 --- 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 #include #include @@ -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::SharedPtr sub_simulation_events_; - // Parameter Server - rcl_interfaces::msg::SetParametersResult onSetParam( - const std::vector & 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 readEventDiagList(); + std::vector read_event_diag_list(); - diagnostic_updater::Updater updater_; + FaultInjectionDiagUpdater updater_; DiagnosticStorage diagnostic_storage_; }; diff --git a/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp b/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp index 9b86fa2de4294..7b8f87400beab 100644 --- a/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp +++ b/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp @@ -43,30 +43,26 @@ using rosidl_generator_traits::to_yaml; FaultInjectionNode::FaultInjectionNode(rclcpp::NodeOptions node_options) : Node("fault_injection", node_options.automatically_declare_parameters_from_overrides(true)), - updater_(this) + updater_(this, 0.05) { - updater_.setHardwareID("fault_injection"); + updater_.set_hardware_id("fault_injection"); using std::placeholders::_1; - // Parameter Server - set_param_res_ = - this->add_on_set_parameters_callback(std::bind(&FaultInjectionNode::onSetParam, this, _1)); - // Subscriber sub_simulation_events_ = this->create_subscription( "~/input/simulation_events", rclcpp::QoS{rclcpp::KeepLast(10)}, - std::bind(&FaultInjectionNode::onSimulationEvents, this, _1)); + std::bind(&FaultInjectionNode::on_simulation_events, this, _1)); // Load all config - for (const auto & diag : readEventDiagList()) { + for (const auto & diag : read_event_diag_list()) { diagnostic_storage_.registerEvent(diag); updater_.add( - diag.diag_name, std::bind(&FaultInjectionNode::updateEventDiag, this, _1, diag.sim_name)); + diag.diag_name, std::bind(&FaultInjectionNode::update_event_diag, this, _1, diag.sim_name)); } } -void FaultInjectionNode::onSimulationEvents(const SimulationEvents::ConstSharedPtr msg) +void FaultInjectionNode::on_simulation_events(const SimulationEvents::ConstSharedPtr msg) { RCLCPP_DEBUG(this->get_logger(), "Received data: %s", to_yaml(*msg).c_str()); for (const auto & event : msg->fault_injection_events) { @@ -76,7 +72,7 @@ void FaultInjectionNode::onSimulationEvents(const SimulationEvents::ConstSharedP } } -void FaultInjectionNode::updateEventDiag( +void FaultInjectionNode::update_event_diag( diagnostic_updater::DiagnosticStatusWrapper & wrap, const std::string & event_name) { const auto diag = diagnostic_storage_.getDiag(event_name); @@ -86,30 +82,7 @@ void FaultInjectionNode::updateEventDiag( wrap.hardware_id = diag.hardware_id; } -rcl_interfaces::msg::SetParametersResult FaultInjectionNode::onSetParam( - const std::vector & params) -{ - rcl_interfaces::msg::SetParametersResult result; - - RCLCPP_DEBUG(this->get_logger(), "call onSetParam"); - - try { - double value; - if (tier4_autoware_utils::updateParam(params, "diagnostic_updater.period", value)) { - updater_.setPeriod(value); - } - } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { - result.successful = false; - result.reason = e.what(); - return result; - } - - result.successful = true; - result.reason = "success"; - return result; -} - -std::vector FaultInjectionNode::readEventDiagList() +std::vector FaultInjectionNode::read_event_diag_list() { // Expected parameter name is "event_diag_list.param_name". // In this case, depth is 2. diff --git a/simulator/fault_injection/test/config/test_event_diag.param.yaml b/simulator/fault_injection/test/config/test_event_diag.param.yaml index 83f58fcc455ee..d9006d239b924 100644 --- a/simulator/fault_injection/test/config/test_event_diag.param.yaml +++ b/simulator/fault_injection/test/config/test_event_diag.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: event_diag_list: - cpu_temperature: "CPU Temperature" - cpu_usage: "CPU Usage" - cpu_load_average: "CPU Load Average" + cpu_temperature: ": CPU Temperature" + cpu_usage: ": CPU Usage" + cpu_load_average: ": CPU Load Average" diff --git a/simulator/fault_injection/test/test_fault_injection_node.test.py b/simulator/fault_injection/test/test_fault_injection_node.test.py index 4556aaaca8d80..1437d69b1c91b 100644 --- a/simulator/fault_injection/test/test_fault_injection_node.test.py +++ b/simulator/fault_injection/test/test_fault_injection_node.test.py @@ -167,11 +167,11 @@ def test_receive_multiple_message_simultaneously(self): # Verify the latest message for stat in msg_buffer[-1].status: - if stat.name == "fault_injection: CPU Load Average": + if stat.name == ": CPU Load Average": self.assertEqual(stat.level, DiagnosticStatus.OK) - elif stat.name == "fault_injection: CPU Temperature": + elif stat.name == ": CPU Temperature": self.assertEqual(stat.level, DiagnosticStatus.ERROR) - elif stat.name == "fault_injection: CPU Usage": + elif stat.name == ": CPU Usage": self.assertEqual(stat.level, DiagnosticStatus.ERROR) else: self.fail(f"Unexpected status name: {stat.name}")