diff --git a/diagnostic_aggregator/CMakeLists.txt b/diagnostic_aggregator/CMakeLists.txt index 12aac3b1f..a72335e6f 100644 --- a/diagnostic_aggregator/CMakeLists.txt +++ b/diagnostic_aggregator/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED) find_package(diagnostic_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) +find_package(rcl_interfaces REQUIRED) find_package(std_msgs REQUIRED) add_library(${PROJECT_NAME} SHARED @@ -67,6 +68,10 @@ add_executable(aggregator_node src/aggregator_node.cpp) target_link_libraries(aggregator_node ${PROJECT_NAME}) +# Add analyzer +add_executable(add_analyzer src/add_analyzer.cpp) +ament_target_dependencies(add_analyzer rclcpp rcl_interfaces) + # Testing macro if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -77,6 +82,7 @@ if(BUILD_TESTING) find_package(launch_testing_ament_cmake REQUIRED) file(TO_CMAKE_PATH "${CMAKE_INSTALL_PREFIX}/lib/${PROJECT_NAME}/aggregator_node" AGGREGATOR_NODE) + file(TO_CMAKE_PATH "${CMAKE_INSTALL_PREFIX}/lib/${PROJECT_NAME}/add_analyzer" ADD_ANALYZER) file(TO_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test/test_listener.py" TEST_LISTENER) set(create_analyzers_tests "primitive_analyzers" @@ -124,6 +130,27 @@ if(BUILD_TESTING) ) endforeach() + set(add_analyzers_tests + "all_analyzers") + + foreach(test_name ${add_analyzers_tests}) + file(TO_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test/default.yaml" PARAMETER_FILE) + file(TO_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test/${test_name}.yaml" ADD_PARAMETER_FILE) + file(TO_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test/expected_output/add_${test_name}" EXPECTED_OUTPUT) + + configure_file( + "test/add_analyzers.launch.py.in" + "test_add_${test_name}.launch.py" + @ONLY + ) + add_launch_test( + "${CMAKE_CURRENT_BINARY_DIR}/test_add_${test_name}.launch.py" + TARGET "test_add_${test_name}" + TIMEOUT 30 + ENV + ) + endforeach() + add_launch_test( test/test_critical_pub.py TIMEOUT 30 @@ -140,6 +167,11 @@ install( DESTINATION lib/${PROJECT_NAME} ) +install( + TARGETS add_analyzer + DESTINATION lib/${PROJECT_NAME} +) + install( TARGETS ${PROJECT_NAME} ${ANALYZERS} EXPORT ${PROJECT_NAME}Targets @@ -157,6 +189,7 @@ ament_python_install_package(${PROJECT_NAME}) # Install Example set(ANALYZER_PARAMS_FILEPATH "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/example_analyzers.yaml") +set(ADD_ANALYZER_PARAMS_FILEPATH "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/example_add_analyzers.yaml") configure_file(example/example.launch.py.in example.launch.py @ONLY) install( # launch descriptor FILES ${CMAKE_CURRENT_BINARY_DIR}/example.launch.py @@ -167,7 +200,7 @@ install( # example publisher DESTINATION lib/${PROJECT_NAME} ) install( # example aggregator configration - FILES example/example_analyzers.yaml + FILES example/example_analyzers.yaml example/example_add_analyzers.yaml DESTINATION share/${PROJECT_NAME} ) diff --git a/diagnostic_aggregator/README.md b/diagnostic_aggregator/README.md index 08178a066..fa0eea7a9 100644 --- a/diagnostic_aggregator/README.md +++ b/diagnostic_aggregator/README.md @@ -135,6 +135,33 @@ You can launch the `aggregator_node` like this (see [example.launch.py.in](examp ]) ``` +You can add analyzers at runtime using the `add_analyzer` node like this (see [example.launch.py.in](example/example.launch.py.in)): +``` + add_analyzer = launch_ros.actions.Node( + package='diagnostic_aggregator', + executable='add_analyzer', + output='screen', + parameters=[add_analyzer_params_filepath]) + return launch.LaunchDescription([ + add_analyzer, + ]) +``` +This node updates the parameters of the `aggregator_node` by calling the service `/analyzers/set_parameters_atomically`. +The `aggregator_node` will detect when a `parameter-event` has introduced new parameters to it. +When this happens the `aggregator_node` will reload all analyzers based on its new set of parameters. +Adding analyzers this way can be done at runtime and can be made conditional. + +In the example, `add_analyzer` will add an analyzer for diagnostics that are marked optional: +``` yaml +/**: + ros__parameters: + optional: + type: diagnostic_aggregator/GenericAnalyzer + path: Optional + startswith: [ '/optional' ] +``` +This will move the `/optional/runtime/analyzer` diagnostic from the "Other" to "Aggregation" where it will not go stale after 5 seconds and will be taken into account for the toplevel state. + # Basic analyzers The `diagnostic_aggregator` package provides a few basic analyzers that you can use to aggregate your diagnostics. diff --git a/diagnostic_aggregator/example/README.md b/diagnostic_aggregator/example/README.md index 10e9b2574..27c593be9 100644 --- a/diagnostic_aggregator/example/README.md +++ b/diagnostic_aggregator/example/README.md @@ -1,5 +1,7 @@ # Aggregator Example -This is a simple example to show the diagnostic_aggregator in action. It involves one python script producing dummy diagnostic data ([example_pub.py](./example_pub.py)), and one diagnostic aggregator configuration ([example.yaml](./example.yaml)) that provides analyzers aggregating it. +This is a simple example to show the diagnostic_aggregator and add_analyzer in action. It involves one python script producing dummy diagnostic data ([example_pub.py](./example_pub.py)), one diagnostic aggregator configuration ([example_analyzers.yaml](./example_analyzers.yaml)) and one add_analyzer configuration ([example_add_analyzers.yaml](./example_add_analyzers.yaml)). + +The aggregator will launch and load all the analyzers listed in ([example_analyzers.yaml](./example_analyzers.yaml)). Then the aggregator will be notified that there are additional analyzers that we also want to load in ([example_add_analyzers.yaml](./example_add_analyzers.yaml)). After this reload all analyzers will be active. Run the example with `ros2 launch diagnostic_aggregator example.launch.py` diff --git a/diagnostic_aggregator/example/example.launch.py.in b/diagnostic_aggregator/example/example.launch.py.in index 48cd62f66..81a749220 100644 --- a/diagnostic_aggregator/example/example.launch.py.in +++ b/diagnostic_aggregator/example/example.launch.py.in @@ -4,6 +4,7 @@ import launch import launch_ros.actions analyzer_params_filepath = "@ANALYZER_PARAMS_FILEPATH@" +add_analyzer_params_filepath = "@ADD_ANALYZER_PARAMS_FILEPATH@" def generate_launch_description(): @@ -12,11 +13,18 @@ def generate_launch_description(): executable='aggregator_node', output='screen', parameters=[analyzer_params_filepath]) + add_analyzer = launch_ros.actions.Node( + package='diagnostic_aggregator', + executable='add_analyzer', + output='screen', + parameters=[add_analyzer_params_filepath] + ) diag_publisher = launch_ros.actions.Node( package='diagnostic_aggregator', executable='example_pub.py') return launch.LaunchDescription([ aggregator, + add_analyzer, diag_publisher, launch.actions.RegisterEventHandler( event_handler=launch.event_handlers.OnProcessExit( diff --git a/diagnostic_aggregator/example/example_add_analyzers.yaml b/diagnostic_aggregator/example/example_add_analyzers.yaml new file mode 100644 index 000000000..1c6c264c7 --- /dev/null +++ b/diagnostic_aggregator/example/example_add_analyzers.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + optional: + type: diagnostic_aggregator/GenericAnalyzer + path: Optional + contains: [ '/optional' ] diff --git a/diagnostic_aggregator/example/example_pub.py b/diagnostic_aggregator/example/example_pub.py index 887dc18df..0c1b10436 100755 --- a/diagnostic_aggregator/example/example_pub.py +++ b/diagnostic_aggregator/example/example_pub.py @@ -81,6 +81,10 @@ def __init__(self): name='/sensors/front/cam', message='OK'), DiagnosticStatus(level=DiagnosticStatus.OK, name='/sensors/rear/cam', message='OK'), + + # Optional + DiagnosticStatus(level=DiagnosticStatus.OK, + name='/optional/runtime/analyzer', message='OK'), ] def timer_callback(self): diff --git a/diagnostic_aggregator/include/diagnostic_aggregator/aggregator.hpp b/diagnostic_aggregator/include/diagnostic_aggregator/aggregator.hpp index b901acdc0..5f48dd6b1 100644 --- a/diagnostic_aggregator/include/diagnostic_aggregator/aggregator.hpp +++ b/diagnostic_aggregator/include/diagnostic_aggregator/aggregator.hpp @@ -133,6 +133,8 @@ class Aggregator rclcpp::Service::SharedPtr add_srv_; /// DiagnosticArray, /diagnostics rclcpp::Subscription::SharedPtr diag_sub_; + /// ParameterEvent, /parameter_events + rclcpp::Subscription::SharedPtr param_sub_; /// DiagnosticArray, /diagnostics_agg rclcpp::Publisher::SharedPtr agg_pub_; /// DiagnosticStatus, /diagnostics_toplevel_state @@ -165,6 +167,16 @@ class Aggregator /// Records all ROS warnings. No warnings are repeated. std::set ros_warnings_; + /* + *!\brief Checks for new parameters to trigger reinitialization of the AnalyzerGroup and OtherAnalyzer + */ + void parameterCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr param_msg); + + /* + *!\brief (re)initializes the AnalyzerGroup and OtherAnalyzer + */ + void initAnalyzers(); + /* *!\brief Checks timestamp of message, and warns if timestamp is 0 (not set) */ diff --git a/diagnostic_aggregator/package.xml b/diagnostic_aggregator/package.xml index 677e04365..cfe473cd7 100644 --- a/diagnostic_aggregator/package.xml +++ b/diagnostic_aggregator/package.xml @@ -12,7 +12,7 @@ BSD-3-Clause http://www.ros.org/wiki/diagnostic_aggregator - + Kevin Watts Brice Rebsamen Arne Nordmann @@ -22,6 +22,7 @@ diagnostic_msgs pluginlib + rcl_interfaces rclcpp std_msgs diff --git a/diagnostic_aggregator/src/add_analyzer.cpp b/diagnostic_aggregator/src/add_analyzer.cpp new file mode 100644 index 000000000..42ff2b0aa --- /dev/null +++ b/diagnostic_aggregator/src/add_analyzer.cpp @@ -0,0 +1,110 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Nobleo Technology + * 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 copyright holder 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. + *********************************************************************/ + +/**< \author Martin Cornelis */ + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rcl_interfaces/srv/set_parameters_atomically.hpp" +#include "rcl_interfaces/msg/parameter.hpp" + +using namespace std::chrono_literals; + +class AddAnalyzer : public rclcpp::Node +{ +public: + AddAnalyzer() + : Node("add_analyzer_node", "", rclcpp::NodeOptions().allow_undeclared_parameters( + true).automatically_declare_parameters_from_overrides(true)) + { + client_ = this->create_client( + "/analyzers/set_parameters_atomically"); + } + + void send_request() + { + while (!client_->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO_ONCE(this->get_logger(), "service not available, waiting ..."); + } + auto request = std::make_shared(); + std::map parameters; + + if (!this->get_parameters("", parameters)) { + RCLCPP_ERROR(this->get_logger(), "Failed to retrieve parameters"); + } + for (const auto & [param_name, param] : parameters) { + // Find the suffix + size_t suffix_start = param_name.find_last_of('.'); + // Remove suffix if it exists + if (suffix_start != std::string::npos) { + std::string stripped_param_name = param_name.substr(0, suffix_start); + // Check in map if the stripped param name with the added suffix "path" exists + // This indicates the parameter is part of an analyzer description + if (parameters.count(stripped_param_name + ".path") > 0) { + auto parameter_msg = param.to_parameter_msg(); + request->parameters.push_back(parameter_msg); + } + } + } + + auto result = client_->async_send_request(request); + // Wait for the result. + if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(this->get_logger(), "Parameters succesfully set"); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to set parameters"); + } + } + +private: + rclcpp::Client::SharedPtr client_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto add_analyzer = std::make_shared(); + add_analyzer->send_request(); + rclcpp::shutdown(); + + return 0; +} diff --git a/diagnostic_aggregator/src/aggregator.cpp b/diagnostic_aggregator/src/aggregator.cpp index 64b716caf..4619ec297 100644 --- a/diagnostic_aggregator/src/aggregator.cpp +++ b/diagnostic_aggregator/src/aggregator.cpp @@ -59,7 +59,8 @@ using diagnostic_msgs::msg::DiagnosticStatus; Aggregator::Aggregator() : n_(std::make_shared( "analyzers", "", - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true))), + rclcpp::NodeOptions().allow_undeclared_parameters(true). + automatically_declare_parameters_from_overrides(true))), logger_(rclcpp::get_logger("Aggregator")), pub_rate_(1.0), history_depth_(1000), @@ -69,6 +70,36 @@ Aggregator::Aggregator() last_top_level_state_(DiagnosticStatus::STALE) { RCLCPP_DEBUG(logger_, "constructor"); + initAnalyzers(); + + diag_sub_ = n_->create_subscription( + "/diagnostics", rclcpp::SystemDefaultsQoS().keep_last(history_depth_), + std::bind(&Aggregator::diagCallback, this, _1)); + agg_pub_ = n_->create_publisher("/diagnostics_agg", 1); + toplevel_state_pub_ = + n_->create_publisher("/diagnostics_toplevel_state", 1); + + int publish_rate_ms = 1000 / pub_rate_; + publish_timer_ = n_->create_wall_timer( + std::chrono::milliseconds(publish_rate_ms), + std::bind(&Aggregator::publishData, this)); + + param_sub_ = n_->create_subscription( + "/parameter_events", 1, std::bind(&Aggregator::parameterCallback, this, _1)); +} + +void Aggregator::parameterCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr msg) +{ + if (msg->node == "/" + std::string(n_->get_name())) { + if (msg->new_parameters.size() != 0) { + base_path_ = ""; + initAnalyzers(); + } + } +} + +void Aggregator::initAnalyzers() +{ bool other_as_errors = false; std::map parameters; @@ -101,26 +132,17 @@ Aggregator::Aggregator() RCLCPP_DEBUG( logger_, "Aggregator critical publisher configured to: %s", (critical_ ? "true" : "false")); - analyzer_group_ = std::make_unique(); - if (!analyzer_group_->init(base_path_, "", n_)) { - RCLCPP_ERROR(logger_, "Analyzer group for diagnostic aggregator failed to initialize!"); - } - - // Last analyzer handles remaining data - other_analyzer_ = std::make_unique(other_as_errors); - other_analyzer_->init(base_path_); // This always returns true - - diag_sub_ = n_->create_subscription( - "/diagnostics", rclcpp::SystemDefaultsQoS().keep_last(history_depth_), - std::bind(&Aggregator::diagCallback, this, _1)); - agg_pub_ = n_->create_publisher("/diagnostics_agg", 1); - toplevel_state_pub_ = - n_->create_publisher("/diagnostics_toplevel_state", 1); + { // lock the mutex while analyzer_group_ and other_analyzer_ are being updated + std::lock_guard lock(mutex_); + analyzer_group_ = std::make_unique(); + if (!analyzer_group_->init(base_path_, "", n_)) { + RCLCPP_ERROR(logger_, "Analyzer group for diagnostic aggregator failed to initialize!"); + } - int publish_rate_ms = 1000 / pub_rate_; - publish_timer_ = n_->create_wall_timer( - std::chrono::milliseconds(publish_rate_ms), - std::bind(&Aggregator::publishData, this)); + // Last analyzer handles remaining data + other_analyzer_ = std::make_unique(other_as_errors); + other_analyzer_->init(base_path_); // This always returns true + } } void Aggregator::checkTimestamp(const DiagnosticArray::SharedPtr diag_msg) diff --git a/diagnostic_aggregator/test/add_analyzers.launch.py.in b/diagnostic_aggregator/test/add_analyzers.launch.py.in new file mode 100644 index 000000000..8e4eae8da --- /dev/null +++ b/diagnostic_aggregator/test/add_analyzers.launch.py.in @@ -0,0 +1,74 @@ +import os + +import unittest + +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch.events import matches_action +from launch.events.process import ShutdownProcess + +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing.util +import launch_testing_ros + + +def generate_test_description(): + os.environ['OSPL_VERBOSITY'] = '8' + os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}' + + aggregator_node = ExecuteProcess( + cmd=[ + "@AGGREGATOR_NODE@", + "--ros-args", + "--params-file", + "@PARAMETER_FILE@" + ], + name='aggregator_node', + emulate_tty=True, + output='screen') + + add_analyzer = ExecuteProcess( + cmd=[ + "@ADD_ANALYZER@", + "--ros-args", + "--params-file", + "@ADD_PARAMETER_FILE@" + ], + name='add_analyzer', + emulate_tty=True, + output='screen') + + launch_description = LaunchDescription() + launch_description.add_action(aggregator_node) + launch_description.add_action(add_analyzer) + launch_description.add_action(launch_testing.util.KeepAliveProc()) + launch_description.add_action(launch_testing.actions.ReadyToTest()) + return launch_description, locals() + +class TestAggregator(unittest.TestCase): + + def test_processes_output(self, proc_output, aggregator_node): + """Check aggregator logging output for expected strings.""" + + from launch_testing.tools.output import get_default_filtered_prefixes + output_filter = launch_testing_ros.tools.basic_output_filter( + filtered_prefixes=get_default_filtered_prefixes() + ['service not available, waiting...'], + filtered_rmw_implementation='@rmw_implementation@' + ) + proc_output.assertWaitFor( + expected_output=launch_testing.tools.expected_output_from_file(path="@EXPECTED_OUTPUT@"), + process=aggregator_node, + output_filter=output_filter, + timeout=15 + ) + + import time + time.sleep(1) + +@launch_testing.post_shutdown_test() +class TestAggregatorShutdown(unittest.TestCase): + + def test_last_process_exit_code(self, proc_info, aggregator_node): + launch_testing.asserts.assertExitCodes(proc_info, process=aggregator_node) diff --git a/diagnostic_aggregator/test/all_analyzers.yaml b/diagnostic_aggregator/test/all_analyzers.yaml index 84b330e34..4cb012a83 100644 --- a/diagnostic_aggregator/test/all_analyzers.yaml +++ b/diagnostic_aggregator/test/all_analyzers.yaml @@ -1,4 +1,4 @@ -analyzers: +/**: ros__parameters: path: BASIC prefix1: diff --git a/diagnostic_aggregator/test/analyzer_group.yaml b/diagnostic_aggregator/test/analyzer_group.yaml index 72bb5639d..14c938fff 100644 --- a/diagnostic_aggregator/test/analyzer_group.yaml +++ b/diagnostic_aggregator/test/analyzer_group.yaml @@ -1,4 +1,4 @@ -analyzers: +/**: ros__parameters: path: TEST primary: diff --git a/diagnostic_aggregator/test/default.yaml b/diagnostic_aggregator/test/default.yaml new file mode 100644 index 000000000..2da82b92f --- /dev/null +++ b/diagnostic_aggregator/test/default.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + path: BASIC + prefix0: + type: diagnostic_aggregator/GenericAnalyzer + path: Zeroth + contains: [ + 'contain0a', + 'contain0b' ] \ No newline at end of file diff --git a/diagnostic_aggregator/test/empty_root_path.yaml b/diagnostic_aggregator/test/empty_root_path.yaml index 391de4e99..b4b25509f 100644 --- a/diagnostic_aggregator/test/empty_root_path.yaml +++ b/diagnostic_aggregator/test/empty_root_path.yaml @@ -1,4 +1,4 @@ -analyzers: +/**: ros__parameters: primary: type: 'diagnostic_aggregator/AnalyzerGroup' diff --git a/diagnostic_aggregator/test/expected_output/add_all_analyzers.txt b/diagnostic_aggregator/test/expected_output/add_all_analyzers.txt new file mode 100644 index 000000000..9c5c2fc23 --- /dev/null +++ b/diagnostic_aggregator/test/expected_output/add_all_analyzers.txt @@ -0,0 +1,6 @@ +/BASIC/Zeroth +prefix0 +/BASIC/First +prefix1 +/BASIC/Third +prefix3 \ No newline at end of file diff --git a/diagnostic_aggregator/test/expected_stale_analyzers.yaml b/diagnostic_aggregator/test/expected_stale_analyzers.yaml index 9110f84d6..9546204d5 100644 --- a/diagnostic_aggregator/test/expected_stale_analyzers.yaml +++ b/diagnostic_aggregator/test/expected_stale_analyzers.yaml @@ -1,4 +1,4 @@ -analyzers: +/**: my_path: type: diagnostic_aggregator/GenericAnalyzer path: My Path diff --git a/diagnostic_aggregator/test/multiple_match_analyzers.yaml b/diagnostic_aggregator/test/multiple_match_analyzers.yaml index 46153c6a7..3f0ec91f4 100644 --- a/diagnostic_aggregator/test/multiple_match_analyzers.yaml +++ b/diagnostic_aggregator/test/multiple_match_analyzers.yaml @@ -1,4 +1,4 @@ -analyzers: +/**: my_path: type: diagnostic_aggregator/GenericAnalyzer path: Header1 diff --git a/diagnostic_aggregator/test/primitive_analyzers.yaml b/diagnostic_aggregator/test/primitive_analyzers.yaml index 9601cce77..fc98e2ee8 100644 --- a/diagnostic_aggregator/test/primitive_analyzers.yaml +++ b/diagnostic_aggregator/test/primitive_analyzers.yaml @@ -1,4 +1,4 @@ -analyzers: +/**: ros__parameters: log_level: debug primary: diff --git a/diagnostic_common_diagnostics/CMakeLists.txt b/diagnostic_common_diagnostics/CMakeLists.txt index 99569f20d..9ab1c21f9 100644 --- a/diagnostic_common_diagnostics/CMakeLists.txt +++ b/diagnostic_common_diagnostics/CMakeLists.txt @@ -11,6 +11,7 @@ install(PROGRAMS ${PROJECT_NAME}/cpu_monitor.py ${PROJECT_NAME}/ntp_monitor.py ${PROJECT_NAME}/ram_monitor.py + ${PROJECT_NAME}/sensors_monitor.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/diagnostic_common_diagnostics/README.md b/diagnostic_common_diagnostics/README.md index 41c500d17..3b410ee34 100644 --- a/diagnostic_common_diagnostics/README.md +++ b/diagnostic_common_diagnostics/README.md @@ -93,7 +93,20 @@ warning percentage threshold. Length of RAM readings queue. ## sensors_monitor.py -**To be ported** +The `sensors_monitor` module allows users to monitor the temperature, volt and fan speeds of their system in real-time. +It uses the [`LM Sensors` package](https://packages.debian.org/sid/utils/lm-sensors) to get the data. + +* Name of the node is "sensors_monitor_" + hostname. + +### Published Topics +#### /diagnostics +diagnostic_msgs/DiagnosticArray +The diagnostics information. + +### Parameters +#### ignore_fans +(default: false) +Whether to ignore the fan speed. ## tf_monitor.py **To be ported** diff --git a/diagnostic_common_diagnostics/diagnostic_common_diagnostics/sensors_monitor.py b/diagnostic_common_diagnostics/diagnostic_common_diagnostics/sensors_monitor.py new file mode 100755 index 000000000..c6733e119 --- /dev/null +++ b/diagnostic_common_diagnostics/diagnostic_common_diagnostics/sensors_monitor.py @@ -0,0 +1,255 @@ +#!/usr/bin/env python3 + +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, 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. + +from __future__ import division, with_statement + +from io import StringIO +import math +import re +import socket +import subprocess + +from diagnostic_msgs.msg import DiagnosticStatus + +import diagnostic_updater as DIAG + +import rclpy +from rclpy.node import Node + + +class Sensor(object): + + def __init__(self): + self.critical = None + self.min = None + self.max = None + self.input = None + self.name = None + self.type = None + self.high = None + self.alarm = None + + def __repr__(self): + return 'Sensor object (name: {}, type: {})'.format(self.name, self.type) + + def getCrit(self): + return self.critical + + def getMin(self): + return self.min + + def getMax(self): + return self.max + + def getInput(self): + return self.input + + def getName(self): + return self.name + + def getType(self): + return self.type + + def getHigh(self): + return self.high + + def getAlarm(self): + return self.alarm + + def __str__(self): + lines = [] + lines.append(str(self.name)) + lines.append('\t' + 'Type: ' + str(self.type)) + if self.input: + lines.append('\t' + 'Input: ' + str(self.input)) + if self.min: + lines.append('\t' + 'Min: ' + str(self.min)) + if self.max: + lines.append('\t' + 'Max: ' + str(self.max)) + if self.high: + lines.append('\t' + 'High: ' + str(self.high)) + if self.critical: + lines.append('\t' + 'Crit: ' + str(self.critical)) + lines.append('\t' + 'Alarm: ' + str(self.alarm)) + return '\n'.join(lines) + + +def parse_sensor_line(line): + sensor = Sensor() + line = line.lstrip() + [name, reading] = line.split(':') + + try: + [sensor.name, sensor.type] = name.rsplit(' ', 1) + except ValueError: + return None + + if sensor.name == 'Core': + sensor.name = name + sensor.type = 'Temperature' + elif sensor.name.find('Physical id') != -1: + sensor.name = name + sensor.type = 'Temperature' + + try: + [reading, params] = reading.lstrip().split('(') + except ValueError: + return None + + sensor.alarm = False + if line.find('ALARM') != -1: + sensor.alarm = True + + if reading.find('°C') == -1: + sensor.input = float(reading.split()[0]) + else: + sensor.input = float(reading.split('°C')[0]) + + params = params.split(',') + for param in params: + m = re.search('[0-9]+.[0-9]*', param) + if param.find('min') != -1: + sensor.min = float(m.group(0)) + elif param.find('max') != -1: + sensor.max = float(m.group(0)) + elif param.find('high') != -1: + sensor.high = float(m.group(0)) + elif param.find('crit') != -1: + sensor.critical = float(m.group(0)) + + return sensor + + +def _rads_to_rpm(rads): + return rads / (2 * math.pi) * 60 + + +def _rpm_to_rads(rpm): + return rpm * (2 * math.pi) / 60 + + +def parse_sensors_output(node: Node, output): + out = StringIO(output if isinstance(output, str) else output.decode('utf-8')) + + sensorList = [] + for line in out.readlines(): + # Check for a colon + if ':' in line and 'Adapter' not in line: + s = None + try: + s = parse_sensor_line(line) + except Exception as exc: + node.get_logger().warn( + 'Unable to parse line "%s", due to %s', line, exc + ) + if s is not None: + sensorList.append(s) + return sensorList + + +def get_sensors(): + p = subprocess.Popen( + 'sensors', stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True + ) + (o, e) = p.communicate() + if not p.returncode == 0: + return '' + if not o: + return '' + return o + + +class SensorsMonitor(object): + + def __init__(self, node: Node, hostname): + self.node = node + self.hostname = hostname + self.ignore_fans = node.declare_parameter('ignore_fans', False).value + node.get_logger().info('Ignore fanspeed warnings: %s' % self.ignore_fans) + + self.updater = DIAG.Updater(node) + self.updater.setHardwareID('none') + self.updater.add('%s Sensor Status' % self.hostname, self.monitor) + + def monitor(self, stat): + try: + stat.summary(DiagnosticStatus.OK, 'OK') + for sensor in parse_sensors_output(self.node, get_sensors()): + if sensor.getType() == 'Temperature': + if sensor.getInput() > sensor.getCrit(): + stat.mergeSummary( + DiagnosticStatus.ERROR, 'Critical Temperature' + ) + elif sensor.getInput() > sensor.getHigh(): + stat.mergeSummary(DiagnosticStatus.WARN, 'High Temperature') + stat.add( + ' '.join([sensor.getName(), sensor.getType()]), + str(sensor.getInput()), + ) + elif sensor.getType() == 'Voltage': + if sensor.getInput() < sensor.getMin(): + stat.mergeSummary(DiagnosticStatus.ERROR, 'Low Voltage') + elif sensor.getInput() > sensor.getMax(): + stat.mergeSummary(DiagnosticStatus.ERROR, 'High Voltage') + stat.add( + ' '.join([sensor.getName(), sensor.getType()]), + str(sensor.getInput()), + ) + elif sensor.getType() == 'Speed': + if not self.ignore_fans: + if sensor.getInput() < sensor.getMin(): + stat.mergeSummary(DiagnosticStatus.ERROR, 'No Fan Speed') + stat.add( + ' '.join([sensor.getName(), sensor.getType()]), + str(sensor.getInput()), + ) + except Exception: + import traceback + + self.node.get_logger().error('Unable to process lm-sensors data') + self.node.get_logger().error(traceback.format_exc()) + return stat + + +if __name__ == '__main__': + rclpy.init() + hostname = socket.gethostname() + hostname_clean = hostname.translate(hostname.maketrans('-', '_')) + node = rclpy.create_node('sensors_monitor_%s' % hostname_clean) + + monitor = SensorsMonitor(node, hostname) + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass diff --git a/diagnostic_common_diagnostics/package.xml b/diagnostic_common_diagnostics/package.xml index 8496593a0..cc9d820ef 100644 --- a/diagnostic_common_diagnostics/package.xml +++ b/diagnostic_common_diagnostics/package.xml @@ -19,6 +19,7 @@ ament_cmake_python diagnostic_updater + lm-sensors python3-ntplib python3-psutil