├── .backportrc.json ├── .github ├── mergify.yml └── workflows │ ├── lint.yaml │ └── test.yaml ├── .gitignore ├── LICENSE ├── README.md ├── diagnostic_aggregator ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── diagnostic_aggregator │ └── __init__.py ├── doc │ └── rqt_robot_monitor.png ├── example │ ├── README.md │ ├── example.launch.py.in │ ├── example_add_analyzers.yaml │ ├── example_analyzers.yaml │ └── example_pub.py ├── include │ └── diagnostic_aggregator │ │ ├── aggregator.hpp │ │ ├── analyzer.hpp │ │ ├── analyzer_group.hpp │ │ ├── discard_analyzer.hpp │ │ ├── generic_analyzer.hpp │ │ ├── generic_analyzer_base.hpp │ │ ├── ignore_analyzer.hpp │ │ ├── other_analyzer.hpp │ │ ├── status_item.hpp │ │ └── visibility_control.hpp ├── mainpage.dox ├── package.xml ├── plugin_description.xml ├── src │ ├── add_analyzer.cpp │ ├── aggregator.cpp │ ├── aggregator_node.cpp │ ├── analyzer_group.cpp │ ├── discard_analyzer.cpp │ ├── generic_analyzer.cpp │ ├── ignore_analyzer.cpp │ └── status_item.cpp └── test │ ├── add_analyzers.launch.py.in │ ├── all_analyzers.yaml │ ├── analyzer_group.yaml │ ├── analyzer_loader.cpp │ ├── create_analyzers.launch.py.in │ ├── default.yaml │ ├── empty_root_path.yaml │ ├── expected_output │ ├── add_all_analyzers.txt │ ├── create_all_analyzers.txt │ ├── create_analyzer_group.txt │ ├── create_empty_root_path.txt │ ├── create_primitive_analyzers.txt │ ├── output_all_analyzers.txt │ ├── output_analyzer_group.txt │ ├── output_empty_root_path.txt │ └── output_primitive_analyzers.txt │ ├── expected_stale_analyzers.yaml │ ├── multiple_match_analyzers.yaml │ ├── primitive_analyzers.yaml │ ├── test_analyzers_output.launch.py.in │ ├── test_critical_pub.py │ ├── test_discard_behavior.py │ ├── test_listener.py │ └── test_pub.py ├── diagnostic_common_diagnostics ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── diagnostic_common_diagnostics │ ├── __init__.py │ ├── cpu_monitor.py │ ├── hd_monitor.py │ ├── ntp_monitor.py │ ├── ram_monitor.py │ └── sensors_monitor.py ├── mainpage.dox ├── package.xml └── test │ └── systemtest │ ├── test_cpu_monitor.py │ ├── test_hd_monitor_launchtest.py │ └── test_ntp_monitor_launchtest.py ├── diagnostic_remote_logging ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── diagnostic_remote_logging │ │ ├── influx_line_protocol.hpp │ │ └── influxdb.hpp ├── package.xml ├── src │ └── influxdb.cpp └── test │ └── influx_line_protocol.cpp ├── diagnostic_updater ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── diagnostic_updater │ ├── __init__.py │ ├── _diagnostic_status_wrapper.py │ ├── _diagnostic_updater.py │ ├── _publisher.py │ ├── _update_functions.py │ └── example.py ├── include │ └── diagnostic_updater │ │ ├── diagnostic_status_wrapper.hpp │ │ ├── diagnostic_updater.hpp │ │ ├── publisher.hpp │ │ └── update_functions.hpp ├── mainpage.dox ├── package.xml ├── src │ ├── diagnostic_updater.cpp │ └── example.cpp └── test │ ├── diagnostic_status_wrapper_test.cpp │ ├── diagnostic_updater_test.cpp │ ├── diagnostic_updater_test.py │ ├── dummy_process.py │ ├── status_msg_test.cpp │ ├── status_msg_test.py │ └── test_diagnostic_status_wrapper.py ├── diagnostics ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml └── self_test ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── example └── selftest_example.cpp ├── include └── self_test │ └── test_runner.hpp ├── mainpage.dox ├── package.xml ├── src └── run_selftest.cpp └── test ├── CMakeLists.txt ├── error_selftest.cpp ├── exception_selftest.cpp ├── no_id_selftest.cpp ├── nominal_selftest.cpp ├── selftest_fixture.hpp └── selftest_node.hpp /.backportrc.json: -------------------------------------------------------------------------------- 1 | { 2 | "repoName": "diagnostics", 3 | "repoOwner": "ros", 4 | "sourceBranch": "ros2", 5 | "targetBranchChoices": [ 6 | "ros2-humble", 7 | "ros2-jazzy", 8 | "ros2-kilted" 9 | ], 10 | "targetPRLabels": [ 11 | "automerge" 12 | ] 13 | } -------------------------------------------------------------------------------- /.github/mergify.yml: -------------------------------------------------------------------------------- 1 | pull_request_rules: 2 | - name: add ros2 label 3 | description: If targeting an ROS2 branch, add the label 4 | conditions: 5 | - base = ros2 6 | actions: 7 | label: 8 | add: 9 | - ros2 10 | - name: no PRs against distro-specific branches 11 | description: PRs against branches like ros-humble are closed automatically. Unless they are a backport. 12 | conditions: 13 | - and: 14 | - base ~= ^ros2-[a-z]+$ 15 | - not: 16 | head ~= ^backport\/.+$ 17 | actions: 18 | close: 19 | message: Please make all PRs against the `ros2` branch. They will be backported if possible. 20 | - name: Automatic merge 21 | description: Merge when PR passes all branch protection and has label automerge 22 | conditions: 23 | - label = automerge 24 | # we have to list all checks, apparently 25 | - check-success = Check licenses 26 | - check-success = Lint cppcheck 27 | - check-success = Lint cpplint 28 | - check-success = Lint flake8 29 | - check-success = Lint uncrustify 30 | - check-success = Lint xmllint 31 | - check-success ~= diagnostic_aggregator on [a-z]+ 32 | - check-success ~= diagnostic_common_diagnostics on [a-z]+ 33 | - check-success ~= diagnostic_remote_logging on [a-z]+ 34 | # - check-success ~= diagnostic_topic_monitor on [a-z]+ 35 | - check-success ~= diagnostic_updater on [a-z]+ 36 | - check-success ~= self_test on [a-z]+ 37 | actions: 38 | merge: -------------------------------------------------------------------------------- /.github/workflows/lint.yaml: -------------------------------------------------------------------------------- 1 | name: Lint diagnostics 2 | on: 3 | pull_request: 4 | push: 5 | branches: 6 | - ros2 7 | schedule: 8 | # Run every week at 20:00 on Sunday 9 | - cron: "0 20 * * 0" 10 | 11 | jobs: 12 | ament_lint: 13 | name: Lint ${{ matrix.linter }} 14 | strategy: 15 | fail-fast: false 16 | matrix: 17 | linter: [ 18 | cppcheck, 19 | cpplint, 20 | flake8, 21 | # pep257, TODO: enable when we fixed 22 | # Error: diagnostic_common_diagnostics/diagnostic_common_diagnostics/ntp_monitor.py:113 in public method `ntp_diag`: D417: Missing argument descriptions in the docstring (argument(s) st are missing descriptions in 'ntp_diag' docstring) 23 | # using ros-rolling-ament-pep257 amd64 0.18.0-1noble.20240426.150718 24 | uncrustify, 25 | xmllint, 26 | ] 27 | include: 28 | - distro: rolling 29 | os: ubuntu-24.04 30 | runs-on: ${{ matrix.os }} 31 | env: 32 | AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: 1 33 | steps: 34 | - uses: actions/checkout@v1 35 | - uses: ros-tooling/setup-ros@master 36 | with: 37 | required-ros-distributions: ${{ matrix.distro }} 38 | - uses: ros-tooling/action-ros-lint@master 39 | with: 40 | linter: ${{ matrix.linter }} 41 | package-name: | 42 | diagnostic_aggregator 43 | diagnostic_common_diagnostics 44 | diagnostic_remote_logging 45 | diagnostic_updater 46 | self_test 47 | 48 | check_licenses: 49 | name: Check licenses 50 | runs-on: ubuntu-latest 51 | steps: 52 | - uses: actions/checkout@v3 53 | - uses: boschresearch/ros_license_toolkit@2.0.2 54 | -------------------------------------------------------------------------------- /.github/workflows/test.yaml: -------------------------------------------------------------------------------- 1 | name: Test diagnostics 2 | on: 3 | pull_request: 4 | push: 5 | branches: 6 | - ros2 7 | schedule: 8 | # Run every week at 20:00 on Sunday 9 | - cron: "0 20 * * 0" 10 | 11 | jobs: 12 | build_and_test: 13 | name: ${{ matrix.package }} on ${{ matrix.distro }} 14 | strategy: 15 | fail-fast: false 16 | matrix: 17 | package: [ 18 | diagnostic_aggregator, 19 | diagnostic_common_diagnostics, 20 | diagnostic_remote_logging, 21 | diagnostic_updater, 22 | self_test, 23 | ] 24 | include: 25 | - distro: rolling 26 | os: 24.04 27 | runs-on: ubuntu-latest 28 | container: ubuntu:${{ matrix.os }} 29 | steps: 30 | - uses: ros-tooling/setup-ros@master 31 | with: 32 | required-ros-distributions: ${{ matrix.distro }} 33 | - uses: ros-tooling/action-ros-ci@master 34 | with: 35 | target-ros2-distro: ${{ matrix.distro }} 36 | package-name: ${{ matrix.package }} 37 | # vcs-repo-file-url: | 38 | # https://raw.githubusercontent.com/ros2/ros2/master/ros2.repos 39 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | *.project 3 | *.cproject 4 | *.pydevproject 5 | *~ 6 | 7 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2008 - 2012, Willow Garage, Inc. 2 | 2012 - 2022, Open Source Robotics Foundation and contributors 3 | 4 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | 6 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | 8 | 2. 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. 9 | 10 | 3. 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. 11 | 12 | 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 HOLDER 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. -------------------------------------------------------------------------------- /diagnostic_aggregator/diagnostic_aggregator/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros/diagnostics/7021f83f92251d08c075a31fc5d8ada1928ee247/diagnostic_aggregator/diagnostic_aggregator/__init__.py -------------------------------------------------------------------------------- /diagnostic_aggregator/doc/rqt_robot_monitor.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros/diagnostics/7021f83f92251d08c075a31fc5d8ada1928ee247/diagnostic_aggregator/doc/rqt_robot_monitor.png -------------------------------------------------------------------------------- /diagnostic_aggregator/example/README.md: -------------------------------------------------------------------------------- 1 | # Aggregator Example 2 | 3 | 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)). 4 | 5 | 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. 6 | 7 | Run the example with `ros2 launch diagnostic_aggregator example.launch.py` 8 | -------------------------------------------------------------------------------- /diagnostic_aggregator/example/example.launch.py.in: -------------------------------------------------------------------------------- 1 | """Launch analyzer loader with parameters from yaml.""" 2 | 3 | import launch 4 | import launch_ros.actions 5 | 6 | analyzer_params_filepath = "@ANALYZER_PARAMS_FILEPATH@" 7 | add_analyzer_params_filepath = "@ADD_ANALYZER_PARAMS_FILEPATH@" 8 | 9 | 10 | def generate_launch_description(): 11 | aggregator = launch_ros.actions.Node( 12 | package='diagnostic_aggregator', 13 | executable='aggregator_node', 14 | output='screen', 15 | parameters=[analyzer_params_filepath]) 16 | add_analyzer = launch_ros.actions.Node( 17 | package='diagnostic_aggregator', 18 | executable='add_analyzer', 19 | output='screen', 20 | parameters=[add_analyzer_params_filepath] 21 | ) 22 | diag_publisher = launch_ros.actions.Node( 23 | package='diagnostic_aggregator', 24 | executable='example_pub.py') 25 | return launch.LaunchDescription([ 26 | aggregator, 27 | add_analyzer, 28 | diag_publisher, 29 | launch.actions.RegisterEventHandler( 30 | event_handler=launch.event_handlers.OnProcessExit( 31 | target_action=aggregator, 32 | on_exit=[launch.actions.EmitEvent( 33 | event=launch.events.Shutdown())], 34 | )), 35 | ]) 36 | -------------------------------------------------------------------------------- /diagnostic_aggregator/example/example_add_analyzers.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | optional: 4 | type: diagnostic_aggregator/GenericAnalyzer 5 | path: Optional 6 | contains: [ '/optional' ] 7 | -------------------------------------------------------------------------------- /diagnostic_aggregator/example/example_analyzers.yaml: -------------------------------------------------------------------------------- 1 | analyzers: 2 | ros__parameters: 3 | path: Aggregation 4 | arms: 5 | type: diagnostic_aggregator/GenericAnalyzer 6 | path: Arms 7 | startswith: [ '/arms' ] 8 | legs: 9 | type: diagnostic_aggregator/GenericAnalyzer 10 | path: Legs 11 | startswith: [ '/legs' ] 12 | sensors: 13 | type: diagnostic_aggregator/GenericAnalyzer 14 | path: Sensors 15 | startswith: [ '/sensors' ] 16 | motors: 17 | type: diagnostic_aggregator/GenericAnalyzer 18 | path: Motors 19 | contains: [ '/motor' ] 20 | topology: 21 | type: 'diagnostic_aggregator/AnalyzerGroup' 22 | path: Topology 23 | analyzers: 24 | left: 25 | type: diagnostic_aggregator/GenericAnalyzer 26 | path: Left 27 | contains: [ '/left' ] 28 | right: 29 | type: diagnostic_aggregator/GenericAnalyzer 30 | path: Right 31 | contains: [ '/right' ] 32 | -------------------------------------------------------------------------------- /diagnostic_aggregator/example/example_pub.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | # Software License Agreement (BSD License) 5 | # 6 | # Copyright (c) 2009, Willow Garage, Inc. 7 | # All rights reserved. 8 | # 9 | # Redistribution and use in source and binary forms, with or without 10 | # modification, are permitted provided that the following conditions 11 | # are met: 12 | # 13 | # * Redistributions of source code must retain the above copyright 14 | # notice, this list of conditions and the following disclaimer. 15 | # * Redistributions in binary form must reproduce the above 16 | # copyright notice, this list of conditions and the following 17 | # disclaimer in the documentation and/or other materials provided 18 | # with the distribution. 19 | # * Neither the name of the Willow Garage nor the names of its 20 | # contributors may be used to endorse or promote products derived 21 | # from this software without specific prior written permission. 22 | # 23 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | # POSSIBILITY OF SUCH DAMAGE. 35 | # 36 | 37 | # \author Kevin Watts 38 | # \brief Publishes diagnostic messages for diagnostic aggregator unit test 39 | 40 | from random import random 41 | 42 | from diagnostic_msgs.msg import DiagnosticArray 43 | from diagnostic_msgs.msg import DiagnosticStatus 44 | import rclpy 45 | from rclpy.clock import ROSClock 46 | from rclpy.node import Node 47 | from rclpy.qos import qos_profile_system_default 48 | 49 | PKG = 'diagnostic_aggregator' 50 | 51 | 52 | class DiagnosticTalker(Node): 53 | 54 | def __init__(self): 55 | super().__init__('diagnostic_talker') 56 | self.i = 0 57 | self.pub = self.create_publisher(DiagnosticArray, 58 | '/diagnostics', 59 | qos_profile_system_default) 60 | timer_period = 1.0 61 | self.tmr = self.create_timer(timer_period, self.timer_callback) 62 | 63 | self.array = DiagnosticArray() 64 | self.array.status = [ 65 | # Motors 66 | DiagnosticStatus(level=DiagnosticStatus.OK, 67 | name='/arms/left/motor', message='OK'), 68 | DiagnosticStatus(level=DiagnosticStatus.OK, 69 | name='/arms/right/motor', message='OK'), 70 | DiagnosticStatus(level=DiagnosticStatus.OK, 71 | name='/legs/left/motor', message='OK'), 72 | DiagnosticStatus(level=DiagnosticStatus.OK, 73 | name='/legs/right/motor', message='OK'), 74 | 75 | # Sensors 76 | DiagnosticStatus(level=DiagnosticStatus.OK, 77 | name='/sensors/left/cam', message='OK'), 78 | DiagnosticStatus(level=DiagnosticStatus.OK, 79 | name='/sensors/right/cam', message='OK'), 80 | DiagnosticStatus(level=DiagnosticStatus.OK, 81 | name='/sensors/front/cam', message='OK'), 82 | DiagnosticStatus(level=DiagnosticStatus.OK, 83 | name='/sensors/rear/cam', message='OK'), 84 | 85 | # Optional 86 | DiagnosticStatus(level=DiagnosticStatus.OK, 87 | name='/optional/runtime/analyzer', message='OK'), 88 | ] 89 | 90 | def timer_callback(self): 91 | self.array.header.stamp = ROSClock().now().to_msg() 92 | 93 | # Random diagnostics status 94 | level = random() 95 | self.array.status[1].level = DiagnosticStatus.OK 96 | self.array.status[1].message = 'OK' 97 | self.array.status[3].level = DiagnosticStatus.OK 98 | self.array.status[3].message = 'OK' 99 | self.array.status[4].level = DiagnosticStatus.OK 100 | self.array.status[4].message = 'OK' 101 | if level > .5: 102 | self.array.status[1].level = DiagnosticStatus.WARN 103 | self.array.status[1].message = 'Warning' 104 | if level > .7: 105 | self.array.status[3].level = DiagnosticStatus.WARN 106 | self.array.status[3].message = 'Warning' 107 | if level > .95: 108 | self.array.status[4].level = DiagnosticStatus.ERROR 109 | self.array.status[4].message = 'Error' 110 | 111 | self.pub.publish(self.array) 112 | 113 | 114 | def main(args=None): 115 | rclpy.init(args=args) 116 | 117 | node = DiagnosticTalker() 118 | rclpy.spin(node) 119 | 120 | node.destroy_node() 121 | rclpy.try_shutdown() 122 | 123 | 124 | if __name__ == '__main__': 125 | main() 126 | -------------------------------------------------------------------------------- /diagnostic_aggregator/include/diagnostic_aggregator/discard_analyzer.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2010, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /*! 36 | * \author Kevin Watts 37 | */ 38 | 39 | #ifndef DIAGNOSTIC_AGGREGATOR__DISCARD_ANALYZER_HPP_ 40 | #define DIAGNOSTIC_AGGREGATOR__DISCARD_ANALYZER_HPP_ 41 | 42 | #include 43 | #include 44 | 45 | #include "diagnostic_aggregator/generic_analyzer.hpp" 46 | #include "diagnostic_aggregator/visibility_control.hpp" 47 | 48 | #include "diagnostic_msgs/msg/diagnostic_status.h" 49 | 50 | namespace diagnostic_aggregator 51 | { 52 | /*! 53 | *\brief DiscardAnalyzer is does not report any values. It is a subclass of GenericAnalyzer 54 | * 55 | * DiscardAnalyzer is a subclass of GenericAnalyzer. It will ignore any value that it matches. 56 | * It takes the any of the parameters of a GenericAnalyzer. 57 | * 58 | * It is useful for configuring an aggregator_node to ignore certain values in the diagnostics. 59 | * 60 | *\verbatim 61 | * 62 | * 63 | * 64 | * 65 | * 66 | * 67 | *\endverbatim 68 | * 69 | * 70 | */ 71 | class DiscardAnalyzer : public GenericAnalyzer 72 | { 73 | public: 74 | /*! 75 | *\brief Default constructor loaded by pluginlib 76 | */ 77 | DIAGNOSTIC_AGGREGATOR_PUBLIC 78 | DiscardAnalyzer(); 79 | 80 | DIAGNOSTIC_AGGREGATOR_PUBLIC 81 | virtual ~DiscardAnalyzer(); 82 | 83 | /* 84 | *\brief Always reports an empty vector 85 | */ 86 | DIAGNOSTIC_AGGREGATOR_PUBLIC 87 | virtual std::vector> report(); 88 | }; 89 | 90 | } // namespace diagnostic_aggregator 91 | 92 | #endif // DIAGNOSTIC_AGGREGATOR__DISCARD_ANALYZER_HPP_ 93 | -------------------------------------------------------------------------------- /diagnostic_aggregator/include/diagnostic_aggregator/ignore_analyzer.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2010, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /*! 36 | * \author Kevin Watts 37 | */ 38 | 39 | #ifndef DIAGNOSTIC_AGGREGATOR__IGNORE_ANALYZER_HPP_ 40 | #define DIAGNOSTIC_AGGREGATOR__IGNORE_ANALYZER_HPP_ 41 | 42 | #include 43 | #include 44 | #include 45 | 46 | #include "diagnostic_aggregator/generic_analyzer.hpp" 47 | #include "diagnostic_aggregator/visibility_control.hpp" 48 | 49 | #include "diagnostic_msgs/msg/diagnostic_status.h" 50 | 51 | #include "rclcpp/rclcpp.hpp" 52 | 53 | namespace diagnostic_aggregator 54 | { 55 | /*! 56 | *\brief IgnoreAnalyzer ignores all analyzer parameters and does nothing 57 | * 58 | * IgnoreAnalyzer is used to get rid of an Analyzer that is no longer part of a robot configuration. 59 | * 60 | *\verbatim 61 | * 62 | * 63 | * 64 | * 65 | * 66 | * 67 | *\endverbatim 68 | * 69 | * 70 | */ 71 | class IgnoreAnalyzer : public Analyzer 72 | { 73 | public: 74 | /*! 75 | *\brief Default constructor loaded by pluginlib 76 | */ 77 | DIAGNOSTIC_AGGREGATOR_PUBLIC 78 | IgnoreAnalyzer(); 79 | 80 | DIAGNOSTIC_AGGREGATOR_PUBLIC 81 | virtual ~IgnoreAnalyzer(); 82 | 83 | DIAGNOSTIC_AGGREGATOR_PUBLIC 84 | bool init( 85 | const std::string & base_path, const std::string & breadcrumb, 86 | const rclcpp::Node::SharedPtr node); 87 | 88 | bool match(const std::string & name) 89 | { 90 | (void)name; 91 | 92 | return false; 93 | } 94 | 95 | bool analyze(std::shared_ptr item) 96 | { 97 | (void)item; 98 | 99 | return false; 100 | } 101 | 102 | /* 103 | *\brief Always reports an empty vector 104 | */ 105 | DIAGNOSTIC_AGGREGATOR_PUBLIC 106 | virtual std::vector> report(); 107 | 108 | std::string getPath() const {return "";} 109 | std::string getName() const {return "";} 110 | }; 111 | 112 | } // namespace diagnostic_aggregator 113 | 114 | #endif // DIAGNOSTIC_AGGREGATOR__IGNORE_ANALYZER_HPP_ 115 | -------------------------------------------------------------------------------- /diagnostic_aggregator/include/diagnostic_aggregator/other_analyzer.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2009, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /*! 36 | * \author Kevin Watts 37 | */ 38 | 39 | #ifndef DIAGNOSTIC_AGGREGATOR__OTHER_ANALYZER_HPP_ 40 | #define DIAGNOSTIC_AGGREGATOR__OTHER_ANALYZER_HPP_ 41 | 42 | #include 43 | #include 44 | #include 45 | 46 | #include "diagnostic_aggregator/generic_analyzer_base.hpp" 47 | #include "diagnostic_aggregator/visibility_control.hpp" 48 | 49 | #include "rclcpp/rclcpp.hpp" 50 | 51 | namespace diagnostic_aggregator 52 | { 53 | /* 54 | *\brief OtherAnalyzer analyzes any messages that haven't been analyzed by other Analyzers 55 | * 56 | * OtherAnalyzer is not loaded as a plugin. It is created by the Aggregator, and called 57 | * seperately. The aggregator will call analyze() on any message not handled by other 58 | * analyzers. 59 | * 60 | * Stale items will be discarded after 5 seconds of no updates. 61 | * 62 | * OtherAnalyzer is designed to be used internally by the Aggregator only. 63 | * 64 | */ 65 | class OtherAnalyzer : public GenericAnalyzerBase 66 | { 67 | public: 68 | /*! 69 | *\brief Default constructor. OtherAnalyzer isn't loaded by pluginlib 70 | */ 71 | explicit OtherAnalyzer(bool other_as_errors = false) 72 | : other_as_errors_(other_as_errors) 73 | { 74 | RCLCPP_DEBUG(rclcpp::get_logger("OtherAnalyzer"), "constructor"); 75 | } 76 | 77 | virtual ~OtherAnalyzer() 78 | { 79 | RCLCPP_DEBUG(rclcpp::get_logger("OtherAnalyzer"), "destructor"); 80 | } 81 | 82 | /* 83 | *\brief Initialized with the base path only. 84 | * 85 | *\param path Base path of Aggregator 86 | *\param breadcrumb Prefix for parameter getter. 87 | */ 88 | bool init(const std::string & path, const std::string & breadcrumb = "") 89 | { 90 | (void)breadcrumb; 91 | 92 | nice_name_ = "Other"; 93 | path_ = path; 94 | return GenericAnalyzerBase::init(path_, "", 5.0, -1, true); 95 | } 96 | 97 | /* 98 | *\brief OtherAnalyzer cannot be initialized with a NodeHandle 99 | * 100 | *\return False, since NodeHandle initialization isn't valid 101 | */ 102 | bool init( 103 | const std::string & base_path, const std::string & breadcrumb, 104 | const rclcpp::Node::SharedPtr node) 105 | { 106 | (void)base_path; 107 | (void)breadcrumb; 108 | (void)node; 109 | 110 | RCLCPP_ERROR( 111 | rclcpp::get_logger( 112 | "generic_analyzer_base"), 113 | R"(OtherAnalyzer was attempted to initialize with a NodeHandle. 114 | This analyzer cannot be used as a plugin.)"); 115 | return false; 116 | } 117 | 118 | /* 119 | *\brief match() isn't called by aggregator for OtherAnalyzer 120 | * 121 | *\return True, since match() will never by called by Aggregator 122 | */ 123 | bool match(const std::string & name) 124 | { 125 | (void)name; 126 | 127 | return true; 128 | } 129 | 130 | /* 131 | *\brief Reports diagnostics, but doesn't report anything if it doesn't have data 132 | * 133 | */ 134 | std::vector> report() 135 | { 136 | std::vector> processed = 137 | GenericAnalyzerBase::report(); 138 | 139 | // We don't report anything if there's no "Other" items 140 | if (processed.size() == 1) { 141 | processed.clear(); 142 | } else if (other_as_errors_ && processed.size() > 1) { 143 | // "Other" items are considered an error. 144 | std::vector>::iterator it = 145 | processed.begin(); 146 | for (; it != processed.end(); ++it) { 147 | if ((*it)->name == path_) { 148 | (*it)->level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; 149 | (*it)->message = "Unanalyzed items found in \"Other\""; 150 | break; 151 | } 152 | } 153 | } 154 | 155 | return processed; 156 | } 157 | 158 | private: 159 | bool other_as_errors_; 160 | }; 161 | 162 | } // namespace diagnostic_aggregator 163 | 164 | #endif // DIAGNOSTIC_AGGREGATOR__OTHER_ANALYZER_HPP_ 165 | -------------------------------------------------------------------------------- /diagnostic_aggregator/include/diagnostic_aggregator/visibility_control.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Karsten Knese 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef DIAGNOSTIC_AGGREGATOR__VISIBILITY_CONTROL_HPP_ 36 | #define DIAGNOSTIC_AGGREGATOR__VISIBILITY_CONTROL_HPP_ 37 | 38 | #ifdef __cplusplus 39 | extern "C" 40 | { 41 | #endif 42 | 43 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 44 | // https://gcc.gnu.org/wiki/Visibility 45 | 46 | #if defined _WIN32 || defined __CYGWIN__ 47 | #ifdef __GNUC__ 48 | #define DIAGNOSTIC_AGGREGATOR_EXPORT __attribute__ ((dllexport)) 49 | #define DIAGNOSTIC_AGGREGATOR_IMPORT __attribute__ ((dllimport)) 50 | #else 51 | #define DIAGNOSTIC_AGGREGATOR_EXPORT __declspec(dllexport) 52 | #define DIAGNOSTIC_AGGREGATOR_IMPORT __declspec(dllimport) 53 | #endif 54 | #ifdef DIAGNOSTIC_AGGREGATOR_BUILDING_DLL 55 | #define DIAGNOSTIC_AGGREGATOR_PUBLIC DIAGNOSTIC_AGGREGATOR_EXPORT 56 | #else 57 | #define DIAGNOSTIC_AGGREGATOR_PUBLIC DIAGNOSTIC_AGGREGATOR_IMPORT 58 | #endif 59 | #define DIAGNOSTIC_AGGREGATOR_PUBLIC_TYPE DIAGNOSTIC_AGGREGATOR_PUBLIC 60 | #define DIAGNOSTIC_AGGREGATOR_LOCAL 61 | #else 62 | #define DIAGNOSTIC_AGGREGATOR_EXPORT __attribute__ ((visibility("default"))) 63 | #define DIAGNOSTIC_AGGREGATOR_IMPORT 64 | #if __GNUC__ >= 4 65 | #define DIAGNOSTIC_AGGREGATOR_PUBLIC __attribute__ ((visibility("default"))) 66 | #define DIAGNOSTIC_AGGREGATOR_LOCAL __attribute__ ((visibility("hidden"))) 67 | #else 68 | #define DIAGNOSTIC_AGGREGATOR_PUBLIC 69 | #define DIAGNOSTIC_AGGREGATOR_LOCAL 70 | #endif 71 | #define DIAGNOSTIC_AGGREGATOR_PUBLIC_TYPE 72 | #endif 73 | 74 | #ifdef __cplusplus 75 | } 76 | #endif 77 | 78 | #endif // DIAGNOSTIC_AGGREGATOR__VISIBILITY_CONTROL_HPP_ 79 | -------------------------------------------------------------------------------- /diagnostic_aggregator/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b diagnostic_aggregator aggregates and performs basic analysis on the diagnostics of a robot. This package consists of the base node, or aggregator, and several analyzers to collect and process diagnostics data. 6 | 7 | \section codeapi Code API 8 | 9 | The Aggregator class loads "analyzers" which perform analysis on the incoming diagnostics data. Each analyzer is initialized according to the private parameters of the Aggregator. The aggregator will publish the diagnostic_msgs/DiagnosticArray message on the /diagnostics_agg topic at 1 Hz. 10 | 11 | \subsection analyzers Analyzers 12 | 13 | The Aggregator will create analyzers to store and process the diagnostic data. Each analyzer inherits from the pure virtual base class '''Analyzer'''. Analyzers should be in packages that depend directly on "pluginlib" and "diagnostic_aggregator". 14 | 15 | See Analyzer for more information on the base class. 16 | 17 | \subsubsection generic_analyzer GenericAnalyzer 18 | 19 | \b generic_analyzer holds the GenericAnalyzer class, which is the most basic of the Analyzer's. It is used by the diagnostic_aggregator/Aggregator to store, process and republish diagnostics data. The GenericAnalyzer is loaded by the pluginlib as a Analyzer plugin. It is the most basic of all Analyzer's. 20 | 21 | \subsubsection analyzer_group AnalyzerGroup 22 | 23 | \b analyzer_group holds the AnalyzerGroup class, which can hold a group of diagnostic analyzers. These "sub-analyzers" are loaded in the same way that the Aggregator loads analyzers. 24 | 25 | \section rosapi ROS API 26 | 27 | Nodes: 28 | - \b aggregator_node 29 | - \b analyzer_loader 30 | 31 |
32 | 33 | \subsection aggregator_node aggregator_node 34 | 35 | aggregator_node subscribes to "/diagnostics" and publishes an aggregated set of data to "/diagnostics_agg". The aggregator will load diagnostic analyzers (like the GenericAnalyzer above) as plugins. The analyzers are specified in the launch file as private parameters in the "~analyzers" namespace. 36 | 37 | \subsubsection topics ROS topics 38 | 39 | Subscribes to: 40 | - \b "/diagnostics": [diagnostics_msgs/DiagnosticArray] 41 | 42 | Publishes to: 43 | - \b "/diagnostics_agg": [diagnostics_msgs/DiagnosticArray] 44 | 45 | \subsubsection parameters ROS parameters 46 | 47 | Reads the following parameters from the parameter server 48 | 49 | - \b "~pub_rate" : \b double [optional] Rate that output diagnostics published 50 | - \b "~base_path" : \b double [optional] Prepended to all analyzed output 51 | - \b "~analyzers" : \b {} Configuration for loading analyzers 52 | - \b "~critical" : \b bool [optional] React immediately to a degradation in diagnostic state 53 | 54 | \subsection analyzer_loader analyzer_loader 55 | 56 | analyzer_loader loads diagnostic analyzers and verifies that they have initialized. It is used as a unit or regression test to verify that analyzer parameters work. 57 | 58 | \subsubsection parameters ROS parameters 59 | 60 | Reads the following parameters from the parameter server 61 | 62 | - \b "~analyzers" : \b {} Configuration for loading and testing analyzers 63 | 64 | 65 | */ 66 | -------------------------------------------------------------------------------- /diagnostic_aggregator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | diagnostic_aggregator 5 | 4.4.6 6 | diagnostic_aggregator 7 | Austin Hendrix 8 | Brice Rebsamen 9 | Christian Henkel 10 | Ralph Lange 11 | 12 | BSD-3-Clause 13 | 14 | http://www.ros.org/wiki/diagnostic_aggregator 15 | 16 | Kevin Watts 17 | Brice Rebsamen 18 | Arne Nordmann 19 | 20 | ament_cmake 21 | ament_cmake_python 22 | 23 | diagnostic_msgs 24 | pluginlib 25 | rcl_interfaces 26 | rclcpp 27 | std_msgs 28 | 29 | rclpy 30 | 31 | ament_cmake_gtest 32 | ament_cmake_pytest 33 | ament_lint_auto 34 | ament_lint_common 35 | launch_pytest 36 | launch_testing_ament_cmake 37 | launch_testing_ros 38 | 39 | 40 | 41 | ament_cmake 42 | 43 | 44 | -------------------------------------------------------------------------------- /diagnostic_aggregator/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | GenericAnalyzer is default diagnostic analyzer. 5 | 6 | 7 | 8 | 9 | DiscardAnalyzer will discard (not report) any values that it matches. 10 | 11 | 12 | 13 | 14 | IgnoreAnalyzer will ignore all parameters and discard all. 15 | 16 | 17 | 18 | 19 | AnalyzerGroup is a way of grouping diagnostic analyzers. It is used internally by the aggregator, and can also be used as a plugin. 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /diagnostic_aggregator/src/add_analyzer.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2024, Nobleo Technology 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /**< \author Martin Cornelis */ 36 | 37 | #include 38 | 39 | #include "rclcpp/rclcpp.hpp" 40 | #include "rcl_interfaces/srv/set_parameters_atomically.hpp" 41 | #include "rcl_interfaces/msg/parameter.hpp" 42 | 43 | using namespace std::chrono_literals; 44 | 45 | class AddAnalyzer : public rclcpp::Node 46 | { 47 | public: 48 | AddAnalyzer() 49 | : Node("add_analyzer_node", "", rclcpp::NodeOptions().allow_undeclared_parameters( 50 | true).automatically_declare_parameters_from_overrides(true)) 51 | { 52 | client_ = this->create_client( 53 | "/analyzers/set_parameters_atomically"); 54 | } 55 | 56 | void send_request() 57 | { 58 | while (!client_->wait_for_service(1s)) { 59 | if (!rclcpp::ok()) { 60 | RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); 61 | return; 62 | } 63 | RCLCPP_INFO_ONCE(this->get_logger(), "service not available, waiting ..."); 64 | } 65 | auto request = std::make_shared(); 66 | std::map parameters; 67 | 68 | if (!this->get_parameters("", parameters)) { 69 | RCLCPP_ERROR(this->get_logger(), "Failed to retrieve parameters"); 70 | } 71 | for (const auto & [param_name, param] : parameters) { 72 | // Find the suffix 73 | size_t suffix_start = param_name.find_last_of('.'); 74 | // Remove suffix if it exists 75 | if (suffix_start != std::string::npos) { 76 | std::string stripped_param_name = param_name.substr(0, suffix_start); 77 | // Check in map if the stripped param name with the added suffix "path" exists 78 | // This indicates the parameter is part of an analyzer description 79 | if (parameters.count(stripped_param_name + ".path") > 0) { 80 | auto parameter_msg = param.to_parameter_msg(); 81 | request->parameters.push_back(parameter_msg); 82 | } 83 | } 84 | } 85 | 86 | auto result = client_->async_send_request(request); 87 | // Wait for the result. 88 | if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == 89 | rclcpp::FutureReturnCode::SUCCESS) 90 | { 91 | RCLCPP_INFO(this->get_logger(), "Parameters succesfully set"); 92 | } else { 93 | RCLCPP_ERROR(this->get_logger(), "Failed to set parameters"); 94 | } 95 | } 96 | 97 | private: 98 | rclcpp::Client::SharedPtr client_; 99 | }; 100 | 101 | int main(int argc, char ** argv) 102 | { 103 | rclcpp::init(argc, argv); 104 | 105 | auto add_analyzer = std::make_shared(); 106 | add_analyzer->send_request(); 107 | rclcpp::shutdown(); 108 | 109 | return 0; 110 | } 111 | -------------------------------------------------------------------------------- /diagnostic_aggregator/src/aggregator_node.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2009, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /**< \author Kevin Watts */ 36 | 37 | #include "diagnostic_aggregator/aggregator.hpp" 38 | 39 | #include 40 | 41 | #include "rclcpp/rclcpp.hpp" 42 | 43 | int main(int argc, char ** argv) 44 | { 45 | rclcpp::init(argc, argv); 46 | 47 | rclcpp::executors::SingleThreadedExecutor exec; 48 | auto agg = std::make_shared(); 49 | exec.add_node(agg->get_node()); 50 | exec.spin(); 51 | 52 | rclcpp::shutdown(); 53 | 54 | return 0; 55 | } 56 | -------------------------------------------------------------------------------- /diagnostic_aggregator/src/discard_analyzer.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2010, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /**< \author Kevin Watts */ 36 | 37 | #include "diagnostic_aggregator/discard_analyzer.hpp" 38 | 39 | #include 40 | #include 41 | 42 | PLUGINLIB_EXPORT_CLASS(diagnostic_aggregator::DiscardAnalyzer, diagnostic_aggregator::Analyzer) 43 | 44 | namespace diagnostic_aggregator 45 | { 46 | using std::vector; 47 | 48 | DiscardAnalyzer::DiscardAnalyzer() {} 49 | 50 | DiscardAnalyzer::~DiscardAnalyzer() {} 51 | 52 | vector> DiscardAnalyzer::report() 53 | { 54 | vector> processed; 55 | 56 | return processed; 57 | } 58 | 59 | } // namespace diagnostic_aggregator 60 | -------------------------------------------------------------------------------- /diagnostic_aggregator/src/ignore_analyzer.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2010, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /**< \author Kevin Watts */ 36 | 37 | #include "diagnostic_aggregator/ignore_analyzer.hpp" 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | PLUGINLIB_EXPORT_CLASS(diagnostic_aggregator::IgnoreAnalyzer, diagnostic_aggregator::Analyzer) 44 | 45 | namespace diagnostic_aggregator 46 | { 47 | using std::vector; 48 | 49 | IgnoreAnalyzer::IgnoreAnalyzer() {} 50 | 51 | IgnoreAnalyzer::~IgnoreAnalyzer() {} 52 | 53 | bool IgnoreAnalyzer::init( 54 | const std::string & base_path, const std::string & breadcrumb, const rclcpp::Node::SharedPtr node) 55 | { 56 | (void)base_path; 57 | (void)breadcrumb; 58 | (void)node; 59 | 60 | return true; 61 | } 62 | 63 | vector> IgnoreAnalyzer::report() 64 | { 65 | vector> processed; 66 | 67 | return processed; 68 | } 69 | 70 | } // namespace diagnostic_aggregator 71 | -------------------------------------------------------------------------------- /diagnostic_aggregator/src/status_item.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2009, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /**!< \author Kevin Watts */ 36 | 37 | #include "diagnostic_aggregator/status_item.hpp" 38 | 39 | #include 40 | #include 41 | 42 | namespace diagnostic_aggregator 43 | { 44 | using std::string; 45 | 46 | using rclcpp::get_logger; 47 | 48 | StatusItem::StatusItem(const diagnostic_msgs::msg::DiagnosticStatus * status) 49 | : clock_(new rclcpp::Clock()) 50 | { 51 | level_ = valToLevel(status->level); 52 | name_ = status->name; 53 | message_ = status->message; 54 | hw_id_ = status->hardware_id; 55 | values_ = status->values; 56 | 57 | output_name_ = getOutputName(name_); 58 | 59 | update_time_ = clock_->now(); 60 | } 61 | 62 | StatusItem::StatusItem(const string item_name, const string message, const DiagnosticLevel level) 63 | : clock_(new rclcpp::Clock()) 64 | { 65 | RCLCPP_DEBUG(rclcpp::get_logger("StatusItem"), "StatusItem constructor from string"); 66 | name_ = item_name; 67 | message_ = message; 68 | level_ = level; 69 | hw_id_ = ""; 70 | 71 | output_name_ = getOutputName(name_); 72 | 73 | update_time_ = clock_->now(); 74 | RCLCPP_DEBUG(rclcpp::get_logger("StatusItem"), "StatusItem constructor from string"); 75 | } 76 | 77 | StatusItem::~StatusItem() {} 78 | 79 | bool StatusItem::update(const diagnostic_msgs::msg::DiagnosticStatus * status) 80 | { 81 | if (name_ != status->name) { 82 | RCLCPP_ERROR( 83 | get_logger("status_item"), "Incorrect name when updating StatusItem. Expected %s, got %s", 84 | name_.c_str(), status->name.c_str()); 85 | return false; 86 | } 87 | 88 | double update_interval = (clock_->now() - update_time_).seconds(); 89 | if (update_interval < 0) { 90 | RCLCPP_WARN( 91 | get_logger("status_item"), 92 | "StatusItem is being updated with older data. Negative update time: %f", update_interval); 93 | } 94 | 95 | level_ = valToLevel(status->level); 96 | message_ = status->message; 97 | hw_id_ = status->hardware_id; 98 | values_ = status->values; 99 | 100 | update_time_ = clock_->now(); 101 | 102 | return true; 103 | } 104 | 105 | std::shared_ptr StatusItem::toStatusMsg( 106 | const std::string & path, bool stale) const 107 | { 108 | std::shared_ptr status( 109 | new diagnostic_msgs::msg::DiagnosticStatus()); 110 | 111 | if (path == "/") { 112 | status->name = "/" + output_name_; 113 | } else { 114 | status->name = path + "/" + output_name_; 115 | } 116 | 117 | status->level = level_; 118 | status->message = message_; 119 | status->hardware_id = hw_id_; 120 | status->values = values_; 121 | 122 | if (stale) { 123 | status->level = Level_Stale; 124 | } 125 | 126 | return status; 127 | } 128 | 129 | } // namespace diagnostic_aggregator 130 | -------------------------------------------------------------------------------- /diagnostic_aggregator/test/add_analyzers.launch.py.in: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import unittest 4 | 5 | from launch import LaunchDescription 6 | from launch.actions import ExecuteProcess 7 | from launch.events import matches_action 8 | from launch.events.process import ShutdownProcess 9 | 10 | import launch_testing 11 | import launch_testing.actions 12 | import launch_testing.asserts 13 | import launch_testing.util 14 | import launch_testing_ros 15 | 16 | 17 | def generate_test_description(): 18 | os.environ['OSPL_VERBOSITY'] = '8' 19 | os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}' 20 | 21 | aggregator_node = ExecuteProcess( 22 | cmd=[ 23 | "@AGGREGATOR_NODE@", 24 | "--ros-args", 25 | "--params-file", 26 | "@PARAMETER_FILE@" 27 | ], 28 | name='aggregator_node', 29 | emulate_tty=True, 30 | output='screen') 31 | 32 | add_analyzer = ExecuteProcess( 33 | cmd=[ 34 | "@ADD_ANALYZER@", 35 | "--ros-args", 36 | "--params-file", 37 | "@ADD_PARAMETER_FILE@" 38 | ], 39 | name='add_analyzer', 40 | emulate_tty=True, 41 | output='screen') 42 | 43 | launch_description = LaunchDescription() 44 | launch_description.add_action(aggregator_node) 45 | launch_description.add_action(add_analyzer) 46 | launch_description.add_action(launch_testing.util.KeepAliveProc()) 47 | launch_description.add_action(launch_testing.actions.ReadyToTest()) 48 | return launch_description, locals() 49 | 50 | class TestAggregator(unittest.TestCase): 51 | 52 | def test_processes_output(self, proc_output, aggregator_node): 53 | """Check aggregator logging output for expected strings.""" 54 | 55 | from launch_testing.tools.output import get_default_filtered_prefixes 56 | output_filter = launch_testing_ros.tools.basic_output_filter( 57 | filtered_prefixes=get_default_filtered_prefixes() + ['service not available, waiting...'], 58 | filtered_rmw_implementation='@rmw_implementation@' 59 | ) 60 | proc_output.assertWaitFor( 61 | expected_output=launch_testing.tools.expected_output_from_file(path="@EXPECTED_OUTPUT@"), 62 | process=aggregator_node, 63 | output_filter=output_filter, 64 | timeout=15 65 | ) 66 | 67 | import time 68 | time.sleep(1) 69 | 70 | @launch_testing.post_shutdown_test() 71 | class TestAggregatorShutdown(unittest.TestCase): 72 | 73 | def test_last_process_exit_code(self, proc_info, aggregator_node): 74 | launch_testing.asserts.assertExitCodes(proc_info, process=aggregator_node) 75 | -------------------------------------------------------------------------------- /diagnostic_aggregator/test/all_analyzers.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | path: BASIC 4 | prefix1: 5 | type: diagnostic_aggregator/GenericAnalyzer 6 | path: First 7 | remove_prefix: [ 'prefix1' ] 8 | find_and_remove_prefix: [ 'find1_items'] 9 | startswith: [ 10 | 'pref1' ] 11 | contains: [ 12 | 'contains1a', 13 | 'contains1b' ] 14 | name: [ 15 | 'name1' ] 16 | expected: [ 17 | 'prefix1: expected1a', 18 | 'prefix1: expected1b', 19 | 'prefix1: expected1c', 20 | 'prefix1: expected1d' ] 21 | prefix2: 22 | type: diagnostic_aggregator/GenericAnalyzer 23 | path: Second 24 | contains: [ 25 | 'contain2a', 26 | 'contain2b' ] 27 | prefix3: 28 | type: diagnostic_aggregator/DiscardAnalyzer 29 | path: Third 30 | contains: [ 31 | 'nocontains'] 32 | prefix4: 33 | type: diagnostic_aggregator/IgnoreAnalyzer 34 | path: Fourth 35 | -------------------------------------------------------------------------------- /diagnostic_aggregator/test/analyzer_group.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | path: TEST 4 | primary: 5 | type: 'diagnostic_aggregator/AnalyzerGroup' 6 | path: Group01 7 | analyzers: 8 | primary: 9 | type: 'diagnostic_aggregator/GenericAnalyzer' 10 | path: Primary 11 | startswith: [ 'primary' ] 12 | secondary: 13 | type: 'diagnostic_aggregator/GenericAnalyzer' 14 | path: Secondary 15 | startswith: [ 'secondary' ] 16 | -------------------------------------------------------------------------------- /diagnostic_aggregator/test/analyzer_loader.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2010, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /**< \author Kevin Watts */ 36 | 37 | /**< \author Loads analyzer params, verifies that they are valid */ 38 | 39 | #include 40 | 41 | #include 42 | #include 43 | 44 | #include "diagnostic_aggregator/analyzer_group.hpp" 45 | 46 | #include "rclcpp/rclcpp.hpp" 47 | 48 | // Uses AnalyzerGroup to load analyzers 49 | TEST(AnalyzerLoader, analyzerLoading) 50 | { 51 | auto nh = std::make_shared( 52 | "analyzers", "", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); 53 | 54 | diagnostic_aggregator::AnalyzerGroup analyzer_group; 55 | std::string path = "/BASE/PATH"; 56 | 57 | EXPECT_TRUE(analyzer_group.init(path, "", nh)); 58 | } 59 | 60 | int main(int argc, char ** argv) 61 | { 62 | testing::InitGoogleTest(&argc, argv); 63 | rclcpp::init(argc, argv); 64 | 65 | return RUN_ALL_TESTS(); 66 | } 67 | -------------------------------------------------------------------------------- /diagnostic_aggregator/test/create_analyzers.launch.py.in: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import unittest 4 | 5 | from launch import LaunchDescription 6 | from launch.actions import ExecuteProcess 7 | from launch.events import matches_action 8 | from launch.events.process import ShutdownProcess 9 | 10 | import launch_testing 11 | import launch_testing.actions 12 | import launch_testing.asserts 13 | import launch_testing.util 14 | import launch_testing_ros 15 | 16 | 17 | def generate_test_description(): 18 | os.environ['OSPL_VERBOSITY'] = '8' 19 | os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}' 20 | 21 | aggregator_node = ExecuteProcess( 22 | cmd=[ 23 | "@AGGREGATOR_NODE@", 24 | "--ros-args", 25 | "--params-file", 26 | "@PARAMETER_FILE@" 27 | ], 28 | name='aggregator_node', 29 | emulate_tty=True, 30 | output='screen') 31 | 32 | launch_description = LaunchDescription() 33 | launch_description.add_action(aggregator_node) 34 | launch_description.add_action(launch_testing.util.KeepAliveProc()) 35 | launch_description.add_action(launch_testing.actions.ReadyToTest()) 36 | return launch_description, locals() 37 | 38 | class TestAggregator(unittest.TestCase): 39 | 40 | def test_processes_output(self, proc_output, aggregator_node): 41 | """Check aggregator logging output for expected strings.""" 42 | 43 | from launch_testing.tools.output import get_default_filtered_prefixes 44 | output_filter = launch_testing_ros.tools.basic_output_filter( 45 | filtered_prefixes=get_default_filtered_prefixes() + ['service not available, waiting...'], 46 | filtered_rmw_implementation='@rmw_implementation@' 47 | ) 48 | proc_output.assertWaitFor( 49 | expected_output=launch_testing.tools.expected_output_from_file(path="@EXPECTED_OUTPUT@"), 50 | process=aggregator_node, 51 | output_filter=output_filter, 52 | timeout=15 53 | ) 54 | 55 | import time 56 | time.sleep(1) 57 | 58 | @launch_testing.post_shutdown_test() 59 | class TestAggregatorShutdown(unittest.TestCase): 60 | 61 | def test_last_process_exit_code(self, proc_info, aggregator_node): 62 | launch_testing.asserts.assertExitCodes(proc_info, process=aggregator_node) 63 | -------------------------------------------------------------------------------- /diagnostic_aggregator/test/default.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | path: BASIC 4 | prefix0: 5 | type: diagnostic_aggregator/GenericAnalyzer 6 | path: Zeroth 7 | contains: [ 8 | 'contain0a', 9 | 'contain0b' ] -------------------------------------------------------------------------------- /diagnostic_aggregator/test/empty_root_path.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | primary: 4 | type: 'diagnostic_aggregator/AnalyzerGroup' 5 | path: Group01 6 | analyzers: 7 | primary: 8 | type: 'diagnostic_aggregator/GenericAnalyzer' 9 | path: Primary 10 | startswith: [ 'primary' ] 11 | secondary: 12 | type: 'diagnostic_aggregator/GenericAnalyzer' 13 | path: Secondary 14 | startswith: [ 'secondary' ] 15 | -------------------------------------------------------------------------------- /diagnostic_aggregator/test/expected_output/add_all_analyzers.txt: -------------------------------------------------------------------------------- 1 | /BASIC/Zeroth 2 | prefix0 3 | /BASIC/First 4 | prefix1 5 | /BASIC/Third 6 | prefix3 -------------------------------------------------------------------------------- /diagnostic_aggregator/test/expected_output/create_all_analyzers.txt: -------------------------------------------------------------------------------- 1 | /BASIC/First 2 | prefix1 3 | /BASIC/Third 4 | prefix3 -------------------------------------------------------------------------------- /diagnostic_aggregator/test/expected_output/create_analyzer_group.txt: -------------------------------------------------------------------------------- 1 | /TEST/Group01/Primary 2 | primary.analyzers.primary 3 | /TEST/Group01/Secondary 4 | primary.analyzers.secondary -------------------------------------------------------------------------------- /diagnostic_aggregator/test/expected_output/create_empty_root_path.txt: -------------------------------------------------------------------------------- 1 | /Group01/Primary 2 | /Group01/Secondary -------------------------------------------------------------------------------- /diagnostic_aggregator/test/expected_output/create_primitive_analyzers.txt: -------------------------------------------------------------------------------- 1 | diagnostic_aggregator/GenericAnalyzer 'Primary' 2 | /Primary 3 | diagnostic_aggregator/GenericAnalyzer 'Secondary' 4 | /Secondary 5 | -------------------------------------------------------------------------------- /diagnostic_aggregator/test/expected_output/output_all_analyzers.txt: -------------------------------------------------------------------------------- 1 | diagnostic_msgs.msg.DiagnosticStatus 2 | name='/BASIC/First/expected1c', message='Missing' 3 | diagnostic_msgs.msg.DiagnosticStatus 4 | name='/BASIC/First/expected1d', message='Missing' 5 | diagnostic_msgs.msg.DiagnosticStatus 6 | name='/BASIC/First/expected1c', message='Missing' 7 | diagnostic_msgs.msg.DiagnosticStatus 8 | name='/BASIC/First/expected1d', message='Missing' 9 | -------------------------------------------------------------------------------- /diagnostic_aggregator/test/expected_output/output_analyzer_group.txt: -------------------------------------------------------------------------------- 1 | diagnostic_msgs.msg.DiagnosticStatus 2 | name='/TEST/Group01/Primary', message='Stale' 3 | diagnostic_msgs.msg.DiagnosticStatus 4 | name='/TEST/Group01/Secondary', message='Stale' 5 | diagnostic_msgs.msg.DiagnosticStatus 6 | name='/TEST/Group01', message='Stale' 7 | diagnostic_msgs.msg.DiagnosticStatus 8 | name='/TEST/Group01/Primary', message='Stale' 9 | diagnostic_msgs.msg.DiagnosticStatus 10 | name='/TEST/Group01/Secondary', message='Stale' 11 | diagnostic_msgs.msg.DiagnosticStatus 12 | name='/TEST/Group01', message='Stale' 13 | -------------------------------------------------------------------------------- /diagnostic_aggregator/test/expected_output/output_empty_root_path.txt: -------------------------------------------------------------------------------- 1 | diagnostic_msgs.msg.DiagnosticStatus 2 | name='/Group01/Primary', message='Stale' 3 | diagnostic_msgs.msg.DiagnosticStatus 4 | name='/Group01/Secondary', message='Stale' 5 | diagnostic_msgs.msg.DiagnosticStatus 6 | name='/Group01', message='Stale' 7 | -------------------------------------------------------------------------------- /diagnostic_aggregator/test/expected_output/output_primitive_analyzers.txt: -------------------------------------------------------------------------------- 1 | diagnostic_msgs.msg.DiagnosticStatus 2 | Primary', message='Stale' 3 | diagnostic_msgs.msg.DiagnosticStatus 4 | Secondary', message='Stale' 5 | diagnostic_msgs.msg.DiagnosticStatus 6 | Primary', message='Stale' 7 | diagnostic_msgs.msg.DiagnosticStatus 8 | Secondary', message='Stale' 9 | -------------------------------------------------------------------------------- /diagnostic_aggregator/test/expected_stale_analyzers.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | my_path: 3 | type: diagnostic_aggregator/GenericAnalyzer 4 | path: My Path 5 | discard_stale: true 6 | expected: [ 7 | 'expected1', 8 | 'expected2', 9 | 'expected3', 10 | 'expected4', 11 | 'expected5'] 12 | startswith: 'startsw' -------------------------------------------------------------------------------- /diagnostic_aggregator/test/multiple_match_analyzers.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | my_path: 3 | type: diagnostic_aggregator/GenericAnalyzer 4 | path: Header1 5 | expected: 'multi' 6 | startswith: 'Something' 7 | other_path: 8 | type: diagnostic_aggregator/GenericAnalyzer 9 | path: Header2 10 | expected: 'multi' 11 | -------------------------------------------------------------------------------- /diagnostic_aggregator/test/primitive_analyzers.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | log_level: debug 4 | primary: 5 | type: 'diagnostic_aggregator/GenericAnalyzer' 6 | path: Primary 7 | startswith: [ 'primary' ] 8 | secondary: 9 | type: 'diagnostic_aggregator/GenericAnalyzer' 10 | path: Secondary 11 | startswith: [ 'secondary' ] 12 | -------------------------------------------------------------------------------- /diagnostic_aggregator/test/test_analyzers_output.launch.py.in: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import unittest 4 | 5 | from launch import LaunchDescription 6 | from launch.actions import ExecuteProcess 7 | from launch.events import matches_action 8 | from launch.events.process import ShutdownProcess 9 | 10 | import launch_testing 11 | import launch_testing.actions 12 | import launch_testing.asserts 13 | import launch_testing.util 14 | import launch_testing_ros 15 | 16 | 17 | test_listener = ExecuteProcess( 18 | cmd=[ 19 | "@PYTHON_EXECUTABLE@", 20 | "@TEST_LISTENER@" 21 | ], 22 | name='test_listener', 23 | emulate_tty=True, 24 | output='screen') 25 | 26 | def generate_test_description(): 27 | global test_listener 28 | 29 | os.environ['OSPL_VERBOSITY'] = '8' 30 | os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}' 31 | 32 | aggregator_node = ExecuteProcess( 33 | cmd=[ 34 | "@AGGREGATOR_NODE@", 35 | "--ros-args", 36 | "--params-file", 37 | "@PARAMETER_FILE@" 38 | ], 39 | name='aggregator_node', 40 | emulate_tty=True, 41 | output='screen') 42 | 43 | launch_description = LaunchDescription() 44 | launch_description.add_action(aggregator_node) 45 | launch_description.add_action(test_listener) 46 | launch_description.add_action(launch_testing.util.KeepAliveProc()) 47 | launch_description.add_action(launch_testing.actions.ReadyToTest()) 48 | return launch_description, locals() 49 | 50 | class TestAggregator(unittest.TestCase): 51 | 52 | def test_processes_output(self, launch_service, proc_info, proc_output): 53 | """Check aggregator logging output for expected strings.""" 54 | global test_listener 55 | 56 | from launch_testing.tools.output import get_default_filtered_prefixes 57 | output_filter = launch_testing_ros.tools.basic_output_filter( 58 | filtered_prefixes=get_default_filtered_prefixes() + ['service not available, waiting...'], 59 | filtered_rmw_implementation='@rmw_implementation@' 60 | ) 61 | 62 | proc_info.assertWaitForStartup(process=test_listener, timeout=2) 63 | proc_output.assertWaitFor( 64 | expected_output=launch_testing.tools.expected_output_from_file(path="@EXPECTED_OUTPUT@"), 65 | process=test_listener, 66 | output_filter=output_filter, 67 | timeout=15 68 | ) 69 | 70 | import time 71 | time.sleep(1) 72 | 73 | @launch_testing.post_shutdown_test() 74 | class TestAggregatorShutdown(unittest.TestCase): 75 | 76 | def test_last_process_exit_code(self, proc_info, aggregator_node): 77 | launch_testing.asserts.assertExitCodes(proc_info, process=aggregator_node) 78 | -------------------------------------------------------------------------------- /diagnostic_aggregator/test/test_critical_pub.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | from launch_testing.actions import ReadyToTest 7 | import pytest 8 | import rclpy 9 | 10 | 11 | @pytest.mark.launch_test 12 | def generate_test_description(): 13 | # Launch the aggregator 14 | parameters = [{'analyzers.test.type': 'diagnostic_aggregator/GenericAnalyzer'}, 15 | {'analyzers.test.path': 'Test'}, 16 | {'analyzers.test.contains': ['test']}, 17 | {'critical': True}] 18 | 19 | aggregator_cmd = Node( 20 | package='diagnostic_aggregator', 21 | executable='aggregator_node', 22 | name='diagnostic_aggregator', 23 | parameters=parameters) 24 | 25 | ld = LaunchDescription() 26 | 27 | # Launch the node 28 | ld.add_action(aggregator_cmd) 29 | ld.add_action(ReadyToTest()) 30 | return ld 31 | 32 | 33 | class TestProcessOutput(unittest.TestCase): 34 | 35 | @ classmethod 36 | def setUpClass(cls): 37 | # Initialize the ROS context for the test node 38 | rclpy.init() 39 | 40 | @ classmethod 41 | def tearDownClass(cls): 42 | # Shutdown the ROS context 43 | rclpy.shutdown() 44 | 45 | def setUp(self): 46 | # Create a ROS node for tests 47 | self.node = rclpy.create_node('test_node') 48 | self.publisher = self.node.create_publisher(DiagnosticArray, '/diagnostics', 1) 49 | self.subscriber = self.node.create_subscription( 50 | DiagnosticStatus, 51 | '/diagnostics_toplevel_state', 52 | lambda msg: self.received_state.append(msg.level), 53 | 1) 54 | self.received_state = [] 55 | 56 | def tearDown(self): 57 | self.node.destroy_node() 58 | 59 | def publish_message(self, level): 60 | msg = DiagnosticArray() 61 | msg.status.append(DiagnosticStatus()) 62 | msg.status[0].level = level 63 | msg.status[0].name = 'test status' 64 | while msg.status[0].level not in self.received_state: 65 | self.received_state.clear() 66 | self.publisher.publish(msg) 67 | rclpy.spin_once(self.node) 68 | return self.node.get_clock().now() 69 | 70 | def critical_publisher_test( 71 | self, initial_state=DiagnosticStatus.OK, new_state=DiagnosticStatus.ERROR 72 | ): 73 | # Publish the ok message and wait till the toplevel state is received 74 | time_0 = self.publish_message(initial_state) 75 | 76 | assert (self.received_state[0] == initial_state), \ 77 | ('Received state is not the same as the sent state:' 78 | + f"'{self.received_state[0]}' != '{initial_state}'") 79 | self.received_state.clear() 80 | 81 | # Publish the ok message and expect the toplevel state after 1 second period 82 | time_1 = self.publish_message(initial_state) 83 | assert (time_1 - time_0 > rclpy.duration.Duration(seconds=0.99)), \ 84 | 'OK message received too early' 85 | assert (self.received_state[0] == initial_state), \ 86 | ('Received state is not the same as the sent state:' 87 | + f"'{self.received_state[0]}' != '{initial_state}'") 88 | self.received_state.clear() 89 | 90 | # Publish the message and expect the critical error message immediately 91 | time_2 = self.publish_message(new_state) 92 | 93 | assert (time_2 - time_1 < rclpy.duration.Duration(seconds=0.1)), \ 94 | 'Critical error message not received within 0.1 second' 95 | assert (self.received_state[0] == new_state), \ 96 | ('Received state is not the same as the sent state:' 97 | + f"'{self.received_state[0]}' != '{new_state}'") 98 | self.received_state.clear() 99 | 100 | # Next error message should be sent at standard 1 second rate 101 | time_3 = self.publish_message(new_state) 102 | 103 | assert (time_3 - time_1 > rclpy.duration.Duration(seconds=0.99)), \ 104 | 'Periodic error message received too early' 105 | assert (self.received_state[0] == new_state), \ 106 | ('Received state is not the same as the sent state:' 107 | + f"'{self.received_state[0]}' != '{new_state}'") 108 | 109 | def test_critical_publisher_ok_error(self): 110 | self.critical_publisher_test( 111 | initial_state=DiagnosticStatus.OK, new_state=DiagnosticStatus.ERROR 112 | ) 113 | 114 | def test_critical_publisher_ok_warn(self): 115 | self.critical_publisher_test( 116 | initial_state=DiagnosticStatus.OK, new_state=DiagnosticStatus.WARN 117 | ) 118 | 119 | def test_critical_publisher_warn_error(self): 120 | self.critical_publisher_test( 121 | initial_state=DiagnosticStatus.WARN, new_state=DiagnosticStatus.ERROR 122 | ) 123 | -------------------------------------------------------------------------------- /diagnostic_aggregator/test/test_listener.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2020, Arne Nordmann 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of the Willow Garage nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | import os 34 | 35 | from diagnostic_msgs.msg import DiagnosticArray 36 | import rclpy 37 | from rclpy.node import Node 38 | 39 | 40 | class Listener(Node): 41 | 42 | def __init__(self): 43 | super().__init__('test_listener') 44 | self.sub = self.create_subscription( 45 | DiagnosticArray, 46 | 'diagnostics_agg', 47 | self.test_listener_callback, 10) 48 | 49 | def test_listener_callback(self, msg): 50 | for status in msg.status: 51 | self.get_logger().info('%s' % status) 52 | 53 | 54 | def main(args=None): 55 | rclpy.init(args=args) 56 | os.environ['OSPL_VERBOSITY'] = '8' 57 | os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}' 58 | 59 | node = Listener() 60 | try: 61 | rclpy.spin(node) 62 | except KeyboardInterrupt: 63 | pass 64 | 65 | node.destroy_node() 66 | rclpy.try_shutdown() 67 | 68 | 69 | if __name__ == '__main__': 70 | main() 71 | -------------------------------------------------------------------------------- /diagnostic_aggregator/test/test_pub.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | # Software License Agreement (BSD License) 5 | # 6 | # Copyright (c) 2009, Willow Garage, Inc. 7 | # All rights reserved. 8 | # 9 | # Redistribution and use in source and binary forms, with or without 10 | # modification, are permitted provided that the following conditions 11 | # are met: 12 | # 13 | # * Redistributions of source code must retain the above copyright 14 | # notice, this list of conditions and the following disclaimer. 15 | # * Redistributions in binary form must reproduce the above 16 | # copyright notice, this list of conditions and the following 17 | # disclaimer in the documentation and/or other materials provided 18 | # with the distribution. 19 | # * Neither the name of the Willow Garage nor the names of its 20 | # contributors may be used to endorse or promote products derived 21 | # from this software without specific prior written permission. 22 | # 23 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | # POSSIBILITY OF SUCH DAMAGE. 35 | # 36 | 37 | # \author Kevin Watts 38 | # \brief Publishes diagnostic messages for diagnostic aggregator unit test 39 | 40 | from diagnostic_msgs.msg import DiagnosticArray 41 | from diagnostic_msgs.msg import DiagnosticStatus 42 | import rclpy 43 | from rclpy.clock import ROSClock 44 | from rclpy.node import Node 45 | from rclpy.qos import qos_profile_system_default 46 | 47 | PKG = 'diagnostic_aggregator' 48 | 49 | 50 | class DiagnosticTalker(Node): 51 | 52 | def __init__(self): 53 | super().__init__('diagnostic_talker') 54 | self.i = 0 55 | self.pub = self.create_publisher(DiagnosticArray, 56 | '/diagnostics', 57 | qos_profile_system_default) 58 | timer_period = 1.0 59 | self.tmr = self.create_timer(timer_period, self.timer_callback) 60 | 61 | self.array = DiagnosticArray() 62 | self.array.status = [ 63 | # GenericAnalyzer prefix1 64 | DiagnosticStatus( 65 | level=DiagnosticStatus.OK, 66 | name='pref1a', 67 | message='OK'), 68 | DiagnosticStatus( 69 | level=DiagnosticStatus.WARN, 70 | name='pref1b', 71 | message='Warning'), 72 | DiagnosticStatus( 73 | level=DiagnosticStatus.OK, 74 | name='contains1a', 75 | message='OK'), 76 | DiagnosticStatus( 77 | level=DiagnosticStatus.OK, 78 | name='prefix1: contains1b', 79 | message='OK'), 80 | DiagnosticStatus( 81 | level=DiagnosticStatus.OK, 82 | name='name1', 83 | message='OK'), 84 | DiagnosticStatus( 85 | level=DiagnosticStatus.OK, 86 | name='prefix1: expected1a', 87 | message='OK'), 88 | DiagnosticStatus( 89 | level=DiagnosticStatus.OK, 90 | name='prefix1: expected1b', 91 | message='OK'), 92 | DiagnosticStatus( 93 | level=DiagnosticStatus.OK, 94 | name='prefix1: expected1c', 95 | message='OK'), 96 | DiagnosticStatus( 97 | level=DiagnosticStatus.OK, 98 | name='prefix1: expected1d', 99 | message='OK'), 100 | DiagnosticStatus( 101 | level=DiagnosticStatus.OK, 102 | name='find1_items: find_remove1a', 103 | message='OK'), 104 | DiagnosticStatus( 105 | level=DiagnosticStatus.OK, 106 | name='find1_items: find_remove1b', 107 | message='OK'), 108 | 109 | # GenericAnalyzer prefix2 110 | DiagnosticStatus( 111 | level=DiagnosticStatus.OK, 112 | name='contain2a', 113 | message='OK'), 114 | DiagnosticStatus( 115 | level=DiagnosticStatus.OK, 116 | name='contain2b', 117 | message='OK'), 118 | DiagnosticStatus( 119 | level=DiagnosticStatus.OK, 120 | name='name2', 121 | message='OK'), 122 | 123 | # OtherAnalyzer for Other 124 | DiagnosticStatus( 125 | level=DiagnosticStatus.ERROR, 126 | name='other1', 127 | message='Error'), 128 | DiagnosticStatus( 129 | level=DiagnosticStatus.OK, 130 | name='other2', 131 | message='OK'), 132 | DiagnosticStatus( 133 | level=DiagnosticStatus.OK, 134 | name='other3', 135 | message='OK')] 136 | 137 | def timer_callback(self): 138 | self.array.header.stamp = ROSClock().now().to_msg() 139 | self.pub.publish(self.array) 140 | 141 | 142 | def main(args=None): 143 | rclpy.init(args=args) 144 | 145 | node = DiagnosticTalker() 146 | rclpy.spin(node) 147 | 148 | node.destroy_node() 149 | rclpy.try_shutdown() 150 | 151 | 152 | if __name__ == '__main__': 153 | main() 154 | -------------------------------------------------------------------------------- /diagnostic_common_diagnostics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | project(diagnostic_common_diagnostics) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | endif() 8 | 9 | # Find dependencies 10 | find_package(ament_cmake REQUIRED) 11 | find_package(ament_cmake_python REQUIRED) 12 | 13 | ament_python_install_package(${PROJECT_NAME}) 14 | 15 | install(PROGRAMS 16 | ${PROJECT_NAME}/cpu_monitor.py 17 | ${PROJECT_NAME}/ntp_monitor.py 18 | ${PROJECT_NAME}/ram_monitor.py 19 | ${PROJECT_NAME}/sensors_monitor.py 20 | ${PROJECT_NAME}/hd_monitor.py 21 | DESTINATION lib/${PROJECT_NAME} 22 | ) 23 | 24 | if(BUILD_TESTING) 25 | find_package(ament_lint_auto REQUIRED) 26 | find_package(launch_testing_ament_cmake REQUIRED) 27 | ament_lint_auto_find_test_dependencies() 28 | 29 | find_package(ament_cmake_pytest REQUIRED) 30 | ament_add_pytest_test( 31 | test_cpu_monitor 32 | test/systemtest/test_cpu_monitor.py 33 | TIMEOUT 10) 34 | # SKIPPING FLAKY TEST 35 | # add_launch_test( 36 | # test/systemtest/test_ntp_monitor_launchtest.py 37 | # TARGET ntp_monitor_launchtest 38 | # TIMEOUT 20) 39 | add_launch_test( 40 | test/systemtest/test_hd_monitor_launchtest.py 41 | TARGET hd_monitor_launchtest 42 | TIMEOUT 20) 43 | endif() 44 | 45 | ament_package() 46 | -------------------------------------------------------------------------------- /diagnostic_common_diagnostics/README.md: -------------------------------------------------------------------------------- 1 | General information about this repository, including legal information, build instructions and known issues/limitations, are given in [README.md](../README.md) in the repository root. 2 | 3 | # The diagnostic_common_diagnostics package 4 | This package provides generic nodes to monitor a Linux host. 5 | 6 | Currently only the NTP monitor is ported to ROS2. 7 | 8 | # Nodes 9 | 10 | ## cpu_monitor.py 11 | The `cpu_monitor` module allows users to monitor the CPU usage of their system in real-time. 12 | It publishes the usage percentage in a diagnostic message. 13 | 14 | * Name of the node is "cpu_monitor_" + hostname. 15 | * Uses the following args: 16 | * warning_percentage: If the CPU usage is > warning_percentage, a WARN status will be publised. 17 | * window: the maximum length of the used collections.deque for queuing CPU readings. 18 | 19 | ### Published Topics 20 | #### /diagnostics 21 | diagnostic_msgs/DiagnosticArray 22 | The diagnostics information. 23 | 24 | ### Parameters 25 | #### warning_percentage 26 | (default: 90) 27 | warning percentage threshold. 28 | 29 | #### window 30 | (default: 1) 31 | Length of CPU readings queue. 32 | 33 | ## ntp_monitor.py 34 | Runs 'ntpdate' to check if the system clock is synchronized with the NTP server. 35 | * If the offset is smaller than `offset-tolerance`, an `OK` status will be published. 36 | * If the offset is larger than the configured `offset-tolerance`, a `WARN` status will be published, 37 | * if it is bigger than `error-offset-tolerance`, an `ERROR` status will be published. 38 | * If there was an error running `ntpdate`, an `ERROR` status will be published. 39 | 40 | ### Published Topics 41 | #### /diagnostics 42 | diagnostic_msgs/DiagnosticArray 43 | The diagnostics information. 44 | 45 | ### Parameters 46 | #### ntp_hostname 47 | (default: "pool.ntp.org") 48 | Hostname of NTP server. 49 | 50 | #### offset-tolerance" 51 | (default: 500) 52 | Allowed offset from NTP host. Above this is a warning. 53 | 54 | #### error-offset-tolerance 55 | (default: 5000000) 56 | If the offset from the NTP host exceeds this value, it is reported as an error instead of warning. 57 | 58 | #### self_offset-tolerance 59 | (default: 500) 60 | Offset from self 61 | 62 | #### diag-hostname 63 | Computer name in diagnostics output (ex: 'c1') 64 | 65 | #### no-self-test 66 | (default: True) 67 | Disable self test. 68 | 69 | ## hd_monitor.py 70 | Runs 'shutil.disk_usage' to check if there is enough space left on a given device. With default parameters, the following thresholds are used: 71 | * Above 5% of free space left, an `OK` status will be published. 72 | * Between 5% and 1%, a `WARN` status will be published, 73 | * Below 1%, an `ERROR` status will be published. 74 | 75 | ### Published Topics 76 | #### /diagnostics 77 | diagnostic_msgs/DiagnosticArray 78 | The diagnostics information. 79 | 80 | ### Parameters 81 | #### path 82 | (default: home directory "~") 83 | Path in which to check remaining space. 84 | 85 | #### free_percent_low 86 | (default: 5%) 87 | Warning threshold. 88 | 89 | #### free_percent_crit 90 | (default: 1%) 91 | Error threshold. 92 | 93 | ## ram_monitor.py 94 | The `ram_monitor` module allows users to monitor the RAM usage of their system in real-time. 95 | It publishes the usage percentage in a diagnostic message. 96 | 97 | * Name of the node is "ram_monitor_" + hostname. 98 | * Uses the following args: 99 | * warning_percentage: If the RAM usage is > warning_percentage, a WARN status will be published. 100 | * window: the maximum length of the used collections.deque for queuing RAM readings. 101 | 102 | ### Published Topics 103 | #### /diagnostics 104 | diagnostic_msgs/DiagnosticArray 105 | The diagnostics information. 106 | 107 | ### Parameters 108 | #### warning_percentage 109 | (default: 90) 110 | warning percentage threshold. 111 | 112 | #### window 113 | (default: 1) 114 | Length of RAM readings queue. 115 | 116 | ## sensors_monitor.py 117 | The `sensors_monitor` module allows users to monitor the temperature, volt and fan speeds of their system in real-time. 118 | It uses the [`LM Sensors` package](https://packages.debian.org/sid/utils/lm-sensors) to get the data. 119 | 120 | * Name of the node is "sensors_monitor_" + hostname. 121 | 122 | ### Published Topics 123 | #### /diagnostics 124 | diagnostic_msgs/DiagnosticArray 125 | The diagnostics information. 126 | 127 | ### Parameters 128 | #### ignore_fans 129 | (default: false) 130 | Whether to ignore the fan speed. 131 | 132 | ## tf_monitor.py 133 | **To be ported** 134 | -------------------------------------------------------------------------------- /diagnostic_common_diagnostics/diagnostic_common_diagnostics/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros/diagnostics/7021f83f92251d08c075a31fc5d8ada1928ee247/diagnostic_common_diagnostics/diagnostic_common_diagnostics/__init__.py -------------------------------------------------------------------------------- /diagnostic_common_diagnostics/diagnostic_common_diagnostics/cpu_monitor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Software License Agreement (BSD License) 5 | # 6 | # Copyright (c) 2017, TNO IVS, Helmond, Netherlands 7 | # All rights reserved. 8 | # 9 | # Redistribution and use in source and binary forms, with or without 10 | # modification, are permitted provided that the following conditions 11 | # are met: 12 | # 13 | # * Redistributions of source code must retain the above copyright 14 | # notice, this list of conditions and the following disclaimer. 15 | # * Redistributions in binary form must reproduce the above 16 | # copyright notice, this list of conditions and the following 17 | # disclaimer in the documentation and/or other materials provided 18 | # with the distribution. 19 | # * Neither the name of the TNO IVS nor the names of its 20 | # contributors may be used to endorse or promote products derived 21 | # from this software without specific prior written permission. 22 | # 23 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | # POSSIBILITY OF SUCH DAMAGE. 35 | 36 | # \author Rein Appeldoorn 37 | 38 | from collections import deque 39 | import socket 40 | import traceback 41 | 42 | from diagnostic_msgs.msg import DiagnosticStatus 43 | 44 | from diagnostic_updater import DiagnosticTask, Updater 45 | 46 | import psutil 47 | 48 | import rclpy 49 | from rclpy.node import Node 50 | 51 | 52 | class CpuTask(DiagnosticTask): 53 | 54 | def __init__(self, warning_percentage=90, window=1): 55 | DiagnosticTask.__init__(self, 'CPU Information') 56 | 57 | self._warning_percentage = int(warning_percentage) 58 | self._readings = deque(maxlen=window) 59 | 60 | def _get_average_reading(self): 61 | def avg(lst): 62 | return float(sum(lst)) / len(lst) if lst else float('nan') 63 | 64 | return [avg(cpu_percentages) 65 | for cpu_percentages in zip(*self._readings)] 66 | 67 | def run(self, stat): 68 | self._readings.append(psutil.cpu_percent(percpu=True)) 69 | cpu_percentages = self._get_average_reading() 70 | cpu_average = sum(cpu_percentages) / len(cpu_percentages) 71 | 72 | stat.add('CPU Load Average', f'{cpu_average:.2f}') 73 | 74 | warn = False 75 | for idx, cpu_percentage in enumerate(cpu_percentages): 76 | stat.add(f'CPU {idx} Load', f'{cpu_percentage:.2f}') 77 | if cpu_percentage > self._warning_percentage: 78 | warn = True 79 | 80 | if warn: 81 | stat.summary(DiagnosticStatus.WARN, 82 | f'At least one CPU exceeds {self._warning_percentage} percent') 83 | else: 84 | stat.summary(DiagnosticStatus.OK, 85 | f'CPU Average {cpu_average:.2f} percent') 86 | 87 | return stat 88 | 89 | 90 | def main(args=None): 91 | rclpy.init(args=args) 92 | 93 | # Create the node 94 | hostname = socket.gethostname() 95 | # Every invalid symbol is replaced by underscore. 96 | # isalnum() alone also allows invalid symbols depending on the locale 97 | cleaned_hostname = ''.join( 98 | c if (c.isascii() and c.isalnum()) else '_' for c in hostname) 99 | node = Node(f'cpu_monitor_{cleaned_hostname}') 100 | 101 | # Declare and get parameters 102 | node.declare_parameter('warning_percentage', 90) 103 | node.declare_parameter('window', 1) 104 | 105 | warning_percentage = node.get_parameter( 106 | 'warning_percentage').get_parameter_value().integer_value 107 | window = node.get_parameter('window').get_parameter_value().integer_value 108 | 109 | # Create diagnostic updater with default updater rate of 1 hz 110 | updater = Updater(node) 111 | updater.setHardwareID(hostname) 112 | updater.add(CpuTask(warning_percentage=warning_percentage, window=window)) 113 | 114 | rclpy.spin(node) 115 | 116 | 117 | if __name__ == '__main__': 118 | try: 119 | main() 120 | except KeyboardInterrupt: 121 | pass 122 | except Exception: 123 | traceback.print_exc() 124 | -------------------------------------------------------------------------------- /diagnostic_common_diagnostics/diagnostic_common_diagnostics/ram_monitor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Software License Agreement (BSD License) 4 | # 5 | # Copyright (c) 2017, TNO IVS, Helmond, Netherlands 6 | # All rights reserved. 7 | # 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions 10 | # are met: 11 | # 12 | # * Redistributions of source code must retain the above copyright 13 | # notice, this list of conditions and the following disclaimer. 14 | # * Redistributions in binary form must reproduce the above 15 | # copyright notice, this list of conditions and the following 16 | # disclaimer in the documentation and/or other materials provided 17 | # with the distribution. 18 | # * Neither the name of the TNO IVS nor the names of its 19 | # contributors may be used to endorse or promote products derived 20 | # from this software without specific prior written permission. 21 | # 22 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | # POSSIBILITY OF SUCH DAMAGE. 34 | 35 | # \author Rein Appeldoorn 36 | 37 | import collections 38 | import socket 39 | 40 | from diagnostic_msgs.msg import DiagnosticStatus 41 | 42 | from diagnostic_updater import DiagnosticTask, Updater 43 | 44 | import psutil 45 | 46 | import rclpy 47 | 48 | 49 | class RamTask(DiagnosticTask): 50 | 51 | def __init__(self, warning_percentage, window): 52 | DiagnosticTask.__init__(self, 'RAM Information') 53 | self._warning_percentage = int(warning_percentage) 54 | self._readings = collections.deque(maxlen=window) 55 | 56 | def run(self, stat): 57 | self._readings.append(psutil.virtual_memory().percent) 58 | ram_average = sum(self._readings) / len(self._readings) 59 | 60 | stat.add('RAM Load Average', f'{ram_average:.2f}') 61 | 62 | if ram_average > self._warning_percentage: 63 | stat.summary( 64 | DiagnosticStatus.WARN, 65 | f'RAM Average exceeds {self._warning_percentage:d} percent', 66 | ) 67 | else: 68 | stat.summary(DiagnosticStatus.OK, f'RAM Average {ram_average:.2f} percent') 69 | 70 | return stat 71 | 72 | 73 | def main(): 74 | hostname = socket.gethostname() 75 | # Every invalid symbol is replaced by underscore. 76 | # isalnum() alone also allows invalid symbols depending on the locale 77 | cleaned_hostname = ''.join( 78 | c if (c.isascii() and c.isalnum()) else '_' for c in hostname) 79 | rclpy.init() 80 | node = rclpy.create_node(f'ram_monitor_{cleaned_hostname}') 81 | 82 | updater = Updater(node) 83 | updater.setHardwareID(hostname) 84 | updater.add( 85 | RamTask( 86 | node.declare_parameter('warning_percentage', 90).value, 87 | node.declare_parameter('window', 1).value, 88 | ) 89 | ) 90 | 91 | rclpy.spin(node) 92 | 93 | 94 | if __name__ == '__main__': 95 | try: 96 | main() 97 | except KeyboardInterrupt: 98 | pass 99 | -------------------------------------------------------------------------------- /diagnostic_common_diagnostics/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b diagnostic_common_diagnostics contains a few common diagnostic nodes 6 | 7 | - cpu_monitor publishes diagnostic messages with the CPU usage of the system. 8 | - hd_monitor publishes diagnostic messages related to the available space on a given storage device. 9 | - ntp_monitor publishes diagnostic messages for how well the NTP time sync is working. 10 | - tf_monitor used to publish diagnostic messages reporting on the health of 11 | the TF tree. It is based on tfwtf. It is not ported to ROS2. 12 | 13 | 14 | \section codeapi Code API 15 | 16 | 26 | 27 | 28 | */ 29 | -------------------------------------------------------------------------------- /diagnostic_common_diagnostics/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | diagnostic_common_diagnostics 5 | 4.4.6 6 | diagnostic_common_diagnostics 7 | Austin Hendrix 8 | Brice Rebsamen 9 | Christian Henkel 10 | Ralph Lange 11 | 12 | BSD-3-Clause 13 | 14 | http://ros.org/wiki/diagnostic_common_diagnostics 15 | 16 | Brice Rebsamen 17 | 18 | ament_cmake 19 | ament_cmake_python 20 | 21 | diagnostic_updater 22 | lm-sensors 23 | python3-ntplib 24 | python3-psutil 25 | rclpy 26 | 27 | ament_lint_auto 28 | 32 | 33 | 34 | ament_cmake_xmllint 35 | ament_cmake_lint_cmake 36 | 37 | ament_cmake_pytest 38 | launch_testing_ament_cmake 39 | 40 | 41 | ament_cmake 42 | 43 | 44 | -------------------------------------------------------------------------------- /diagnostic_common_diagnostics/test/systemtest/test_cpu_monitor.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2023, Robert Bosch GmbH 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the Willow Garage nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | 34 | import time 35 | import unittest 36 | 37 | from diagnostic_common_diagnostics.cpu_monitor import CpuTask 38 | 39 | from diagnostic_msgs.msg import DiagnosticStatus 40 | 41 | from diagnostic_updater import DiagnosticArray, Updater 42 | from diagnostic_updater import DiagnosticStatusWrapper 43 | 44 | import rclpy 45 | from rclpy.node import Node 46 | 47 | 48 | class TestCPUMonitor(unittest.TestCase): 49 | 50 | @classmethod 51 | def setUpClass(cls): 52 | rclpy.init(args=None) 53 | 54 | @classmethod 55 | def tearDownClass(cls): 56 | if rclpy.ok(): 57 | rclpy.shutdown() 58 | 59 | def setUp(self): 60 | # In this case is recommended for accuracy that psutil.cpu_percent() 61 | # function be called with at least 0.1 seconds between calls. 62 | time.sleep(0.1) 63 | 64 | def diagnostics_callback(self, msg): 65 | self.message_recieved = True 66 | self.assertEqual(len(msg.status), 1) 67 | 68 | def test_ok(self): 69 | warning_percentage = 100 70 | task = CpuTask(warning_percentage) 71 | stat = DiagnosticStatusWrapper() 72 | task.run(stat) 73 | self.assertEqual(task.name, 'CPU Information') 74 | self.assertEqual(stat.level, DiagnosticStatus.OK) 75 | self.assertIn(str('CPU Average'), stat.message) 76 | 77 | # Check for at least 1 CPU Load Average and 1 CPU Load 78 | self.assertGreaterEqual(len(stat.values), 2) 79 | 80 | def test_warn(self): 81 | warning_percentage = -1 82 | task = CpuTask(warning_percentage) 83 | stat = DiagnosticStatusWrapper() 84 | task.run(stat) 85 | print(f'Raw readings: {task._readings}') 86 | self.assertEqual(task.name, 'CPU Information') 87 | self.assertEqual(stat.level, DiagnosticStatus.WARN) 88 | self.assertIn(str('At least one CPU exceeds'), stat.message) 89 | 90 | # Check for at least 1 CPU Load Average and 1 CPU Load 91 | self.assertGreaterEqual(len(stat.values), 2) 92 | 93 | def test_updater(self): 94 | self.message_recieved = False 95 | 96 | node = Node('cpu_monitor_test') 97 | updater = Updater(node) 98 | updater.setHardwareID('test_id') 99 | updater.add(CpuTask()) 100 | 101 | node.create_subscription( 102 | DiagnosticArray, '/diagnostics', self.diagnostics_callback, 10) 103 | 104 | start_time = time.time() 105 | timeout = 5.0 # Timeout in seconds 106 | 107 | while not self.message_recieved: 108 | rclpy.spin_once(node) 109 | time.sleep(0.1) 110 | elapsed_time = time.time() - start_time 111 | if elapsed_time >= timeout: 112 | self.fail('No diagnostics received') 113 | 114 | 115 | if __name__ == '__main__': 116 | unittest.main() 117 | -------------------------------------------------------------------------------- /diagnostic_common_diagnostics/test/systemtest/test_hd_monitor_launchtest.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # Software License Agreement (BSD License) 4 | # 5 | # Copyright (c) 2023, Robert Bosch GmbH 6 | # All rights reserved. 7 | # 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions 10 | # are met: 11 | # 12 | # * Redistributions of source code must retain the above copyright 13 | # notice, this list of conditions and the following disclaimer. 14 | # * Redistributions in binary form must reproduce the above 15 | # copyright notice, this list of conditions and the following 16 | # disclaimer in the documentation and/or other materials provided 17 | # with the distribution. 18 | # * Neither the name of the Willow Garage nor the names of its 19 | # contributors may be used to endorse or promote products derived 20 | # from this software without specific prior written permission. 21 | # 22 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | # POSSIBILITY OF SUCH DAMAGE. 34 | 35 | import unittest 36 | 37 | from diagnostic_msgs.msg import DiagnosticArray 38 | 39 | import launch 40 | 41 | import launch_ros 42 | 43 | import launch_testing 44 | 45 | from launch_testing_ros import WaitForTopics 46 | 47 | import pytest 48 | 49 | import rclpy 50 | 51 | 52 | @pytest.mark.launch_test 53 | def generate_test_description(): 54 | """Launch the hd_monitor node and return a launch description.""" 55 | return launch.LaunchDescription( 56 | [ 57 | launch_ros.actions.Node( 58 | package='diagnostic_common_diagnostics', 59 | executable='hd_monitor.py', 60 | name='hd_monitor', 61 | output='screen', 62 | parameters=[{'free_percent_low': 10, 'free_percent_crit': 5}], 63 | ), 64 | launch_testing.actions.ReadyToTest(), 65 | ] 66 | ) 67 | 68 | 69 | class TestHDMonitor(unittest.TestCase): 70 | """Test if the hd_monitor node is publishing diagnostics.""" 71 | 72 | def __init__(self, methodName: str = 'runTest') -> None: 73 | super().__init__(methodName) 74 | self.received_messages = [] 75 | 76 | def _received_message(self, msg): 77 | self.received_messages.append(msg) 78 | 79 | def _get_min_level(self): 80 | levels = [ 81 | int.from_bytes(status.level, 'little') 82 | for diag in self.received_messages 83 | for status in diag.status 84 | ] 85 | if len(levels) == 0: 86 | return -1 87 | return min(levels) 88 | 89 | def test_topic_published(self): 90 | """Test if the hd_monitor node is publishing diagnostics.""" 91 | with WaitForTopics([('/diagnostics', DiagnosticArray)], timeout=5): 92 | print('Topic found') 93 | 94 | rclpy.init() 95 | test_node = rclpy.create_node('test_node') 96 | test_node.create_subscription( 97 | DiagnosticArray, '/diagnostics', self._received_message, 1 98 | ) 99 | 100 | while len(self.received_messages) < 10: 101 | rclpy.spin_once(test_node, timeout_sec=1) 102 | if (min_level := self._get_min_level()) == 0: 103 | break 104 | 105 | test_node.destroy_node() 106 | rclpy.shutdown() 107 | print(f'Got {len(self.received_messages)} messages:') 108 | for msg in self.received_messages: 109 | print(msg) 110 | self.assertEqual(min_level, 0) 111 | -------------------------------------------------------------------------------- /diagnostic_common_diagnostics/test/systemtest/test_ntp_monitor_launchtest.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # Software License Agreement (BSD License) 4 | # 5 | # Copyright (c) 2023, Robert Bosch GmbH 6 | # All rights reserved. 7 | # 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions 10 | # are met: 11 | # 12 | # * Redistributions of source code must retain the above copyright 13 | # notice, this list of conditions and the following disclaimer. 14 | # * Redistributions in binary form must reproduce the above 15 | # copyright notice, this list of conditions and the following 16 | # disclaimer in the documentation and/or other materials provided 17 | # with the distribution. 18 | # * Neither the name of the Willow Garage nor the names of its 19 | # contributors may be used to endorse or promote products derived 20 | # from this software without specific prior written permission. 21 | # 22 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | # POSSIBILITY OF SUCH DAMAGE. 34 | 35 | import unittest 36 | 37 | from diagnostic_msgs.msg import DiagnosticArray 38 | import launch 39 | import launch_ros 40 | import launch_testing 41 | from launch_testing_ros import WaitForTopics 42 | import pytest 43 | import rclpy 44 | 45 | 46 | @pytest.mark.launch_test 47 | def generate_test_description(): 48 | """Launch the ntp_monitor node and return a launch description.""" 49 | return launch.LaunchDescription([ 50 | launch_ros.actions.Node( 51 | package='diagnostic_common_diagnostics', 52 | executable='ntp_monitor.py', 53 | name='ntp_monitor', 54 | output='screen', 55 | arguments=['--offset-tolerance', '100000', 56 | '--error-offset-tolerance', '200000', 57 | '--ntp_hostname', 'ntp.ubuntu.com'] 58 | # 100s, 200s, we are not testing if your clock is correct 59 | ), 60 | launch_testing.actions.ReadyToTest() 61 | ]) 62 | 63 | 64 | class TestNtpMonitor(unittest.TestCase): 65 | """Test if the ntp_monitor node is publishing diagnostics.""" 66 | 67 | def __init__(self, methodName: str = 'runTest') -> None: 68 | super().__init__(methodName) 69 | self.received_messages = [] 70 | 71 | def _received_message(self, msg): 72 | self.received_messages.append(msg) 73 | 74 | def _get_min_level(self): 75 | levels = [ 76 | int.from_bytes(status.level, 'little') 77 | for diag in self.received_messages 78 | for status in diag.status] 79 | if len(levels) == 0: 80 | return -1 81 | return min(levels) 82 | 83 | def test_topic_published(self): 84 | """Test if the ntp_monitor node is publishing diagnostics.""" 85 | with WaitForTopics( 86 | [('/diagnostics', DiagnosticArray)], 87 | timeout=5 88 | ): 89 | print('Topic found') 90 | 91 | rclpy.init() 92 | test_node = rclpy.create_node('test_node') 93 | test_node.create_subscription( 94 | DiagnosticArray, 95 | '/diagnostics', 96 | self._received_message, 97 | 1 98 | ) 99 | 100 | while len(self.received_messages) < 10: 101 | rclpy.spin_once(test_node, timeout_sec=1) 102 | if (min_level := self._get_min_level()) == 0: 103 | break 104 | 105 | test_node.destroy_node() 106 | rclpy.shutdown() 107 | print(f'Got {len(self.received_messages)} messages:') 108 | for msg in self.received_messages: 109 | print(msg) 110 | self.assertEqual(min_level, 0) 111 | -------------------------------------------------------------------------------- /diagnostic_remote_logging/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package diagnostic_remote_logging 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 4.4.6 (2025-05-26) 6 | ------------------ 7 | * C++17 and cmake 3.20 everywhere (`#510 `_) 8 | * fix: Missing link to libcurl (`#505 `_) 9 | * Contributors: Christian Henkel, Moritz Schauer 10 | 11 | 4.4.5 (2025-05-26) 12 | ------------------ 13 | * Use target_link_libraries instead of ament_target_dependencies (`#507 `_) 14 | * Contributors: Christoph Fröhlich 15 | 16 | 4.4.4 (2025-05-12) 17 | ------------------ 18 | * Kilted dep fix (`#474 `_) 19 | * Contributors: David V. Lu, Christian Henkel 20 | 21 | 4.4.3 (2025-04-03) 22 | ------------------ 23 | * cleanup (`#450 `_) 24 | * Grafana Integration (`#425 `_) 25 | * First working version of remote_logging 26 | * Added more error handling, and skipping values when new line is present in stat 27 | * Changed default telegraf url to reflect the change to influxdb_v2_listener 28 | * Made node composable and changed name to influx to better reflect use cases 29 | * Added README 30 | --------- 31 | Co-authored-by: Daan Wijffels 32 | Co-authored-by: Thiever Base 33 | Co-authored-by: Christian Henkel <6976069+ct2034@users.noreply.github.com> 34 | * Contributors: Christian Henkel, Daan Wijffels 35 | 36 | 4.4.2 (2025-02-10) 37 | ------------------ 38 | 39 | 4.3.1 (2024-07-30) 40 | ------------------ 41 | 42 | 4.3.0 (2024-06-27 14:08) 43 | ------------------------ 44 | 45 | 3.2.1 (2024-06-27 11:00) 46 | ------------------------ 47 | 48 | 3.2.0 (2024-03-22) 49 | ------------------ 50 | 51 | 3.1.2 (2023-03-24) 52 | ------------------ 53 | 54 | 3.1.1 (2023-03-16) 55 | ------------------ 56 | 57 | 3.1.0 (2023-01-30) 58 | ------------------ 59 | 60 | 3.0.0 (2022-06-10) 61 | ------------------ 62 | 63 | 2.1.3 (2021-08-03) 64 | ------------------ 65 | 66 | 2.1.2 (2021-03-03) 67 | ------------------ 68 | 69 | 2.1.1 (2021-01-28) 70 | ------------------ 71 | 72 | 2.1.0 (2021-01-12) 73 | ------------------ 74 | 75 | 2.0.2 (2020-06-03) 76 | ------------------ 77 | 78 | 2.0.0 (2019-09-03) 79 | ------------------ 80 | 81 | 1.9.3 (2018-05-02) 82 | ------------------ 83 | 84 | 1.9.2 (2017-07-15 20:34) 85 | ------------------------ 86 | 87 | 1.9.1 (2017-07-15 16:38) 88 | ------------------------ 89 | 90 | 1.9.0 (2017-04-25) 91 | ------------------ 92 | 93 | 1.8.10 (2016-06-14) 94 | ------------------- 95 | 96 | 1.8.9 (2016-03-02) 97 | ------------------ 98 | 99 | 1.8.8 (2015-08-06) 100 | ------------------ 101 | 102 | 1.8.7 (2015-01-09) 103 | ------------------ 104 | 105 | 1.8.6 (2014-12-10) 106 | ------------------ 107 | 108 | 1.8.5 (2014-07-29) 109 | ------------------ 110 | 111 | 1.8.4 (2014-07-24) 112 | ------------------ 113 | 114 | 1.8.3 (2014-04-23) 115 | ------------------ 116 | 117 | 1.8.2 (2014-04-08) 118 | ------------------ 119 | 120 | 1.8.1 (2014-04-07) 121 | ------------------ 122 | 123 | 1.8.0 (2013-04-03) 124 | ------------------ 125 | 126 | 1.7.10 (2013-02-22) 127 | ------------------- 128 | 129 | 1.7.9 (2012-12-14) 130 | ------------------ 131 | 132 | 1.7.8 (2012-12-06) 133 | ------------------ 134 | 135 | 1.7.7 (2012-11-10) 136 | ------------------ 137 | 138 | 1.7.6 (2012-11-07 23:32) 139 | ------------------------ 140 | 141 | 1.7.5 (2012-11-07 21:53) 142 | ------------------------ 143 | 144 | 1.7.4 (2012-11-07 20:18) 145 | ------------------------ 146 | 147 | 1.7.3 (2012-11-04) 148 | ------------------ 149 | 150 | 1.7.2 (2012-10-30 22:31) 151 | ------------------------ 152 | 153 | 1.7.1 (2012-10-30 15:30) 154 | ------------------------ 155 | 156 | 1.7.0 (2012-10-29) 157 | ------------------ 158 | -------------------------------------------------------------------------------- /diagnostic_remote_logging/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | project(diagnostic_remote_logging) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | endif() 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-Wall -Wextra -Wpedantic) 10 | endif() 11 | 12 | set(dependencies 13 | ament_cmake 14 | rclcpp 15 | rclcpp_components 16 | diagnostic_msgs 17 | CURL 18 | ) 19 | 20 | foreach(dep ${dependencies}) 21 | find_package(${dep} REQUIRED) 22 | endforeach(dep) 23 | 24 | include_directories( 25 | src/ 26 | include/ 27 | ) 28 | add_library(influx_component SHARED src/influxdb.cpp) 29 | 30 | target_link_libraries(influx_component PUBLIC 31 | ${diagnostic_msgs_TARGETS} 32 | rclcpp::rclcpp 33 | rclcpp_components::component 34 | CURL::libcurl 35 | ) 36 | 37 | ament_export_dependencies(influx_component ${dependencies}) 38 | 39 | target_compile_features(influx_component PUBLIC c_std_99 cxx_std_17) 40 | 41 | rclcpp_components_register_node( 42 | influx_component 43 | PLUGIN "InfluxDB" 44 | EXECUTABLE influx 45 | ) 46 | ament_export_targets(export_influx_component) 47 | install(TARGETS influx_component 48 | EXPORT export_influx_component 49 | ARCHIVE DESTINATION lib 50 | LIBRARY DESTINATION lib 51 | RUNTIME DESTINATION bin 52 | ) 53 | 54 | if(BUILD_TESTING) 55 | find_package(ament_cmake_gtest REQUIRED) 56 | ament_add_gtest(unit_tests test/influx_line_protocol.cpp) 57 | target_include_directories(unit_tests PRIVATE include) 58 | target_link_libraries(unit_tests 59 | influx_component 60 | ${diagnostic_msgs_TARGETS} 61 | rclcpp::rclcpp 62 | rclcpp_components::component 63 | rclcpp_components::component_manager 64 | ) 65 | endif() 66 | ament_package() 67 | -------------------------------------------------------------------------------- /diagnostic_remote_logging/README.md: -------------------------------------------------------------------------------- 1 | General information about this repository, including legal information and known issues/limitations, are given in [README.md](../README.md) in the repository root. 2 | 3 | # The diagnostic_remote_logging package 4 | 5 | This package provides the `influx` node, which listens to diagnostic messages and integrates with InfluxDB v2 for monitoring and visualization. Specifically, it subscribes to the [`diagnostic_msgs/DiagnosticArray`](https://index.ros.org/p/diagnostic_msgs) messages on the `/diagnostics_agg` topic and the [`diagnostic_msgs/DiagnosticStatus`](https://index.ros.org/p/diagnostic_msgs) messages on the `/diagnostics_toplevel_state` topic. The node processes these messages, sending their statistics and levels to an [`InfluxDB`](http://influxdb.com) database, enabling use with tools like [`Grafana`](https://grafana.com). 6 | 7 | As of now we only support InfluxDB v2, for support with older versions please use a proxy like [`Telegraf`](https://www.influxdata.com/time-series-platform/telegraf/). See section [Telegraf](#using-a-telegraf-proxy) for an example on how to setup. 8 | 9 | ## Node Configuration 10 | 11 | You can send data to [`InfluxDB`](http://influxdb.com) in two ways: directly to the database or via a proxy like [`Telegraf`](https://www.influxdata.com/time-series-platform/telegraf/). While both methods are valid, using a proxy is generally recommended due to the following benefits: 12 | 13 | - **Efficient Data Transmission**: Telegraf aggregates multiple measurements and sends them in a single request, reducing bandwidth usage and minimizing database load. 14 | - **Enhanced Reliability**: Provides buffering in case of connection issues, ensuring no data is lost. 15 | - **Comprehensive Metric Collection**: Telegraf can send additional system metrics (e.g., RAM, CPU, network usage) with minimal configuration. 16 | - **Data Filtering and Transformation**: Supports preprocessing, such as filtering or transforming data, before sending it to InfluxDB. 17 | 18 | To use either method, ensure you have a running instance of InfluxDB. The simplest way to set this up is through [`InfluxDB Cloud`](https://cloud2.influxdata.com/signup). 19 | 20 | ### Parameters 21 | 22 | The `influx` node supports several parameters. Below is an example configuration: 23 | 24 | ```yaml 25 | /influx: 26 | ros__parameters: 27 | connection: 28 | url: http://localhost:8086/api/v2/write 29 | token: 30 | bucket: 31 | organization: 32 | send: 33 | agg: true 34 | top_level_state: true 35 | ``` 36 | 37 | - `send.agg`: Enables or disables subscription to the `/diagnostics_agg` topic. 38 | - `send.top_level_state`: Enables or disables subscription to the `/diagnostics_toplevel_state` topic. 39 | 40 | #### InfluxDB Configuration 41 | 42 | Set the following parameters in your configuration to match your InfluxDB instance: 43 | 44 | - `connection.url`: The URL of your InfluxDB write API endpoint. 45 | - `connection.token`: Your InfluxDB authentication token. 46 | - `connection.bucket`: The target bucket in InfluxDB. 47 | - `connection.organization`: The name of your InfluxDB organization. 48 | 49 | ### Starting the node 50 | 51 | Afterward all configurations are set run the node with the following command: 52 | 53 | ```bash 54 | ros2 run diagnostic_remote_logging influx --ros-args --params-file 55 | ``` 56 | 57 | ## Using a Telegraf Proxy 58 | 59 | To configure Telegraf as a proxy for InfluxDB: 60 | 61 | 1. Ensure Telegraf is set up to send data to your InfluxDB instance via its configuration file (`/etc/telegraf/telegraf.conf`). Check [this link](https://docs.influxdata.com/influxdb/cloud/write-data/no-code/use-telegraf/manual-config/) for an example. 62 | 2. Add the following to the telegraf configuration file to enable the InfluxDB v2 listener: 63 | 64 | ```toml 65 | [[inputs.influxdb_v2_listener]] 66 | service_address = ":8086" 67 | ``` 68 | 69 | 3. Update the `influx` node configuration to point to the appropriate URL. For example, if Telegraf is running on the same host as the `influx` node, the default `http://localhost:8086/api/v2/write` should work. 70 | 71 | 4. Leave the following parameters empty in the `influx` node configuration when using Telegraf as a proxy: 72 | 73 | - `connection.token` 74 | - `connection.bucket` 75 | - `connection.organization` 76 | 77 | 5. Afterwards run the node with the following command: 78 | 79 | ```bash 80 | ros2 run diagnostic_remote_logging influx --ros-args --params-file 81 | ``` 82 | -------------------------------------------------------------------------------- /diagnostic_remote_logging/include/diagnostic_remote_logging/influx_line_protocol.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2025, Daan Wijffels 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /** 36 | * \author Daan Wijffels 37 | */ 38 | 39 | #ifndef DIAGNOSTIC_REMOTE_LOGGING__INFLUX_LINE_PROTOCOL_HPP_ 40 | #define DIAGNOSTIC_REMOTE_LOGGING__INFLUX_LINE_PROTOCOL_HPP_ 41 | 42 | #include 43 | #include 44 | #include 45 | 46 | #include "rclcpp/rclcpp.hpp" 47 | #include "diagnostic_msgs/msg/diagnostic_array.hpp" 48 | 49 | std::string toInfluxTimestamp(const rclcpp::Time & time) 50 | { 51 | uint64_t seconds = static_cast(time.seconds()); 52 | uint64_t nanoseconds = static_cast(time.nanoseconds()) % 1000000000; 53 | 54 | // Convert to strings 55 | std::string secStr = std::to_string(seconds); 56 | std::string nanosecStr = std::to_string(nanoseconds); 57 | 58 | // Zero-pad nanoseconds to 9 digits 59 | nanosecStr = std::string(9 - nanosecStr.length(), '0') + nanosecStr; 60 | 61 | return secStr + nanosecStr; 62 | } 63 | 64 | std::string escapeSpace(const std::string & input) 65 | { 66 | std::string result; 67 | for (char c : input) { 68 | if (c == ' ') { 69 | result += '\\'; // Add a backslash before the space 70 | } 71 | result += c; // Add the original character 72 | } 73 | return result; 74 | } 75 | 76 | bool is_number(const std::string & s) 77 | { 78 | std::istringstream iss(s); 79 | double d; 80 | return iss >> std::noskipws >> d && iss.eof(); 81 | } 82 | 83 | std::string formatValues(const std::vector & values) 84 | { 85 | std::string formatted; 86 | for (const auto & kv : values) { 87 | if (kv.value.find("\n") != std::string::npos) { 88 | // If the value contains a newline, skip it 89 | // InfluxDB uses this to separate measurements 90 | continue; 91 | } 92 | 93 | formatted += escapeSpace(kv.key) + "="; 94 | 95 | if (is_number(kv.value)) { 96 | formatted += kv.value; 97 | } else { 98 | formatted += "\"" + kv.value + "\""; 99 | } 100 | formatted += ","; 101 | } 102 | if (!formatted.empty()) { 103 | formatted.pop_back(); // Remove the last comma 104 | } 105 | return formatted; 106 | } 107 | 108 | std::pair splitHardwareID(const std::string & input) 109 | { 110 | size_t first_slash_pos = input.find('/'); 111 | 112 | // If no slash is found, treat the entire input as the node_name 113 | if (first_slash_pos == std::string::npos) { 114 | return {"none", input}; 115 | } 116 | 117 | size_t second_slash_pos = input.find('/', first_slash_pos + 1); 118 | 119 | // If the second slash is found, extract the "ns" and "node" parts 120 | if (second_slash_pos != std::string::npos) { 121 | std::string ns = input.substr(first_slash_pos + 1, second_slash_pos - first_slash_pos - 1); 122 | std::string node = input.substr(second_slash_pos + 1); 123 | return {ns, node}; 124 | } 125 | 126 | // If no second slash is found, everything after the first slash is the node 127 | std::string node = input.substr(first_slash_pos + 1); 128 | return {"none", node}; // ns is empty, node is the remaining string 129 | } 130 | 131 | void statusToInfluxLineProtocol( 132 | std::string & output, 133 | const diagnostic_msgs::msg::DiagnosticStatus & status, 134 | const std::string & timestamp_str) 135 | { 136 | // hardware_id is empty for analyzer groups, so skip them 137 | if (status.hardware_id.empty()) { 138 | return; 139 | } 140 | 141 | auto [ns, identifier] = splitHardwareID(status.hardware_id); 142 | output += escapeSpace(identifier) + ",ns=" + escapeSpace(ns) + 143 | " level=" + std::to_string(status.level) + ",message=\"" + status.message + "\""; 144 | auto formatted_key_values = formatValues(status.values); 145 | if (!formatted_key_values.empty()) { 146 | output += "," + formatted_key_values; 147 | } 148 | output += " " + timestamp_str + "\n"; 149 | } 150 | 151 | std::string diagnosticStatusToInfluxLineProtocol( 152 | const diagnostic_msgs::msg::DiagnosticStatus::SharedPtr & msg, const rclcpp::Time & time) 153 | { 154 | std::string output = 155 | msg->name + " level=" + std::to_string(msg->level) + " " + toInfluxTimestamp(time) + "\n"; 156 | return output; 157 | } 158 | 159 | std::string diagnosticArrayToInfluxLineProtocol( 160 | const diagnostic_msgs::msg::DiagnosticArray::SharedPtr & diag_msg) 161 | { 162 | std::string output; 163 | std::string timestamp = toInfluxTimestamp(diag_msg->header.stamp); 164 | 165 | for (auto & status : diag_msg->status) { 166 | statusToInfluxLineProtocol(output, status, timestamp); 167 | } 168 | 169 | return output; 170 | } 171 | 172 | #endif // DIAGNOSTIC_REMOTE_LOGGING__INFLUX_LINE_PROTOCOL_HPP_ 173 | -------------------------------------------------------------------------------- /diagnostic_remote_logging/include/diagnostic_remote_logging/influxdb.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2025, Daan Wijffels 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /** 36 | * \author Daan Wijffels 37 | */ 38 | 39 | #ifndef DIAGNOSTIC_REMOTE_LOGGING__INFLUXDB_HPP_ 40 | #define DIAGNOSTIC_REMOTE_LOGGING__INFLUXDB_HPP_ 41 | 42 | #include 43 | #include 44 | 45 | #include "diagnostic_remote_logging/influx_line_protocol.hpp" 46 | 47 | #include "rclcpp/rclcpp.hpp" 48 | #include "diagnostic_msgs/msg/diagnostic_array.hpp" 49 | 50 | class InfluxDB : public rclcpp::Node 51 | { 52 | public: 53 | explicit InfluxDB(const rclcpp::NodeOptions & opt); 54 | ~InfluxDB(); 55 | 56 | private: 57 | rclcpp::Subscription::SharedPtr diag_sub_; 58 | rclcpp::Subscription::SharedPtr top_level_sub_; 59 | 60 | std::string post_url_, influx_token_; 61 | CURL * curl_; 62 | 63 | void setupConnection(const std::string & telegraf_url); 64 | 65 | void diagnosticsCallback(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg); 66 | void topLevelCallback(const diagnostic_msgs::msg::DiagnosticStatus::SharedPtr msg); 67 | 68 | bool sendToInfluxDB(const std::string & data); 69 | }; 70 | 71 | #endif // DIAGNOSTIC_REMOTE_LOGGING__INFLUXDB_HPP_ 72 | -------------------------------------------------------------------------------- /diagnostic_remote_logging/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | diagnostic_remote_logging 5 | 4.4.6 6 | diagnostic_remote_logging 7 | Daan Wijffels 8 | BSD-3-Clause 9 | 10 | ament_cmake 11 | 12 | diagnostic_msgs 13 | rclcpp_components 14 | curl 15 | 16 | ament_lint_auto 17 | ament_lint_common 18 | ament_cmake_gtest 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /diagnostic_updater/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | project(diagnostic_updater) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_C_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) 13 | 14 | find_package(ament_cmake REQUIRED) 15 | find_package(ament_cmake_ros REQUIRED) 16 | find_package(ament_cmake_python REQUIRED) 17 | find_package(diagnostic_msgs REQUIRED) 18 | find_package(rclcpp REQUIRED) 19 | find_package(rclpy REQUIRED) 20 | find_package(std_msgs REQUIRED) 21 | 22 | add_library(${PROJECT_NAME} 23 | src/diagnostic_updater.cpp 24 | ) 25 | target_include_directories( 26 | ${PROJECT_NAME} 27 | PUBLIC 28 | $ 29 | $ 30 | ) 31 | target_link_libraries( 32 | ${PROJECT_NAME} 33 | PUBLIC 34 | ${diagnostic_msgs_TARGETS} 35 | rclcpp::rclcpp 36 | ) 37 | 38 | install( 39 | TARGETS ${PROJECT_NAME} 40 | EXPORT ${PROJECT_NAME}Targets 41 | LIBRARY DESTINATION lib 42 | ARCHIVE DESTINATION lib 43 | RUNTIME DESTINATION bin 44 | INCLUDES DESTINATION include 45 | ) 46 | 47 | add_executable(example src/example.cpp) 48 | target_link_libraries(example 49 | PUBLIC 50 | ${PROJECT_NAME} 51 | ${diagnostic_msgs_TARGETS} 52 | ${std_msgs_TARGETS} 53 | rclcpp::rclcpp 54 | ) 55 | 56 | if(BUILD_TESTING) 57 | find_package(ament_lint_auto REQUIRED) 58 | set(ament_cmake_copyright_FOUND TRUE) 59 | set(ament_cmake_flake8_FOUND TRUE) # skip for https://github.com/ros-tooling/action-ros-lint/issues/354 60 | ament_lint_auto_find_test_dependencies() 61 | 62 | find_package(ament_cmake_gtest REQUIRED) 63 | find_package(rclcpp_lifecycle REQUIRED) 64 | ament_add_gtest(diagnostic_updater_test test/diagnostic_updater_test.cpp) 65 | target_include_directories(diagnostic_updater_test 66 | PUBLIC 67 | $ 68 | $ 69 | ) 70 | target_link_libraries(diagnostic_updater_test ${PROJECT_NAME}) 71 | target_link_libraries(diagnostic_updater_test 72 | rclcpp_lifecycle::rclcpp_lifecycle 73 | ) 74 | 75 | ament_add_gtest(diagnostic_status_wrapper_test test/diagnostic_status_wrapper_test.cpp) 76 | target_include_directories(diagnostic_status_wrapper_test 77 | PUBLIC 78 | $ 79 | $ 80 | ) 81 | target_link_libraries(diagnostic_status_wrapper_test 82 | ${diagnostic_msgs_TARGETS} 83 | ${std_msgs_TARGETS} 84 | rclcpp::rclcpp 85 | rclcpp_lifecycle::rclcpp_lifecycle 86 | ) 87 | # SKIPPING FLAKY TEST 88 | # ament_add_gtest(status_msg_test test/status_msg_test.cpp) 89 | # target_include_directories(status_msg_test 90 | # PUBLIC 91 | # $ 92 | # $ 93 | # ) 94 | # target_link_libraries(status_msg_test ${PROJECT_NAME}) 95 | 96 | find_package(ament_cmake_pytest REQUIRED) 97 | ament_add_pytest_test(diagnostic_updater_test.py "test/diagnostic_updater_test.py") 98 | ament_add_pytest_test(test_DiagnosticStatusWrapper.py "test/test_diagnostic_status_wrapper.py") 99 | # ament_add_pytest_test(status_msg_test.py "test/status_msg_test.py") 100 | endif() 101 | 102 | ament_python_install_package(${PROJECT_NAME}) 103 | install( 104 | FILES ${PROJECT_NAME}/example.py 105 | DESTINATION lib/${PROJECT_NAME} 106 | ) 107 | 108 | install( 109 | TARGETS example 110 | DESTINATION lib/${PROJECT_NAME} 111 | ) 112 | 113 | install( 114 | DIRECTORY include/ 115 | DESTINATION include 116 | ) 117 | 118 | ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) 119 | 120 | ament_export_include_directories(include) 121 | ament_export_libraries(${PROJECT_NAME}) 122 | ament_export_dependencies(ament_cmake) 123 | ament_export_dependencies(ament_cmake_python) 124 | ament_export_dependencies(diagnostic_msgs) 125 | ament_export_dependencies(rclcpp) 126 | ament_export_dependencies(rclpy) 127 | ament_export_dependencies(std_msgs) 128 | 129 | ament_package() 130 | -------------------------------------------------------------------------------- /diagnostic_updater/README.md: -------------------------------------------------------------------------------- 1 | General information about this repository, including legal information, build instructions and known issues/limitations, are given in [README.md](../README.md) in the repository root. 2 | # The diagnostic_updater package 3 | 4 | This package is used to implement the collection of diagnostics information. 5 | 6 | ## Overview 7 | It can for example update the state of sensors or actors of the robot. 8 | Common tasks include 9 | * Publish the status of a sensor topic from a device driver 10 | * Report that a hardware device is closed 11 | * Send an error if a value is out bounds (e.g. temperature) 12 | 13 | ## Example 14 | The file [example.cpp](src/example.cpp) contains an example of how to use the diagnostic_updater. 15 | 16 | ## C++ and Python API 17 | The main classes are: 18 | 19 | ### DiagnosticStatusWrapper 20 | This class is used to create a diagnostic message. 21 | It simplifies the creation of the message by providing methods to set the level, name, message and values. 22 | There is also the possibility to merge multiple DiagnosticStatusWrapper into one. 23 | 24 | ### Updater 25 | This class is used to collect the diagnostic messages and to publish them. 26 | 27 | ### DiagnosedPublisher 28 | A ROS publisher with included diagnostics. 29 | It diagnoses the frequency of the published messages. 30 | 31 | -------------------------------------------------------------------------------- /diagnostic_updater/diagnostic_updater/__init__.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # 3 | # Software License Agreement (BSD License) 4 | # 5 | # Copyright (c) 2012, Willow Garage, Inc. 6 | # All rights reserved. 7 | # 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions 10 | # are met: 11 | # 12 | # * Redistributions of source code must retain the above copyright 13 | # notice, this list of conditions and the following disclaimer. 14 | # * Redistributions in binary form must reproduce the above 15 | # copyright notice, this list of conditions and the following 16 | # disclaimer in the documentation and/or other materials provided 17 | # with the distribution. 18 | # * Neither the name of the Willow Garage nor the names of its 19 | # contributors may be used to endorse or promote products derived 20 | # from this software without specific prior written permission. 21 | # 22 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | # POSSIBILITY OF SUCH DAMAGE. 34 | 35 | # -*- coding: utf-8 -*- 36 | from ._diagnostic_status_wrapper import * # noqa 37 | from ._diagnostic_updater import * # noqa 38 | from ._publisher import * # noqa 39 | from ._update_functions import * # noqa 40 | -------------------------------------------------------------------------------- /diagnostic_updater/diagnostic_updater/_diagnostic_status_wrapper.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2012, Willow Garage, Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of Willow Garage, Inc. nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | # -*- coding: utf-8 -*- 34 | 35 | """ 36 | diagnostic_updater for Python. 37 | 38 | @author Brice Rebsamen 39 | """ 40 | 41 | from diagnostic_msgs.msg import DiagnosticStatus 42 | from diagnostic_msgs.msg import KeyValue 43 | 44 | 45 | class DiagnosticStatusWrapper(DiagnosticStatus): 46 | """ 47 | Wrapper for the DiagnosticStatus that makes it easier to update. 48 | 49 | This class handles common string formatting and vector handling issues 50 | for filling the diagnostic_msgs::DiagnosticStatus message. It is a 51 | subclass of diagnostic_msgs::DiagnosticStatus, so it can be passed 52 | directly to diagnostic publish calls. 53 | """ 54 | 55 | def __init__(self, *args, **kwds): 56 | """ 57 | Construct a new status wrapper. 58 | 59 | Any message fields that are implicitly/explicitly 60 | set to None will be assigned a default value. The recommend 61 | use is keyword arguments as this is more robust to future message 62 | changes. You cannot mix in-order arguments and keyword arguments. 63 | The available fields are: 64 | level,name,message,hardware_id,values 65 | @param args: complete set of field values, in .msg order 66 | @param kwds: use keyword arguments corresponding to message field names 67 | to set specific fields. 68 | """ 69 | DiagnosticStatus.__init__(self, *args, **kwds) 70 | 71 | def summary(self, *args): 72 | """ 73 | Fill out the level and message fields of the DiagnosticStatus. 74 | 75 | Usage: 76 | summary(diagnostic_status): Copies the summary from a DiagnosticStatus 77 | message. 78 | summary(lvl,msg): sets from lvl and messages 79 | """ 80 | if len(args) == 1: 81 | self.level = args[0].level 82 | self.message = args[0].message 83 | elif len(args) == 2: 84 | self.level = args[0] 85 | self.message = str(args[1]) 86 | 87 | def clearSummary(self): 88 | """Clear the summary, setting the level to zero and the message to.""" 89 | self.summary(DiagnosticStatus.OK, '') 90 | 91 | def mergeSummary(self, *args): 92 | """ 93 | Merge a level and message with the existing ones. 94 | 95 | It is sometimes useful to merge two DiagnosticStatus messages. In that 96 | case, the key value pairs can be unioned, but the level and summary 97 | message have to be merged more intelligently. This function does the 98 | merge in an intelligent manner, combining the summary in *this, with 99 | the one that is passed in. 100 | The combined level is the greater of the two levels to be merged. 101 | If both levels are non-zero (not OK), the messages are combined with a 102 | semicolon separator. If only one level is zero, and the other is 103 | non-zero, the message for the zero level is discarded. If both are 104 | zero, the new message is ignored. 105 | Usage: 106 | mergeSummary(diagnostic_status): merge from a DiagnosticStatus message 107 | mergeSummary(lvl,msg): sets from lvl and msg 108 | """ 109 | if len(args) == 1: 110 | lvl = args[0].level 111 | msg = args[0].message 112 | elif len(args) == 2: 113 | lvl = args[0] 114 | msg = args[1] 115 | 116 | if (lvl > DiagnosticStatus.OK) == (self.level > DiagnosticStatus.OK): 117 | if len(self.message) > 0: 118 | self.message += '; ' 119 | self.message += msg 120 | elif lvl > self.level: 121 | self.message = msg 122 | 123 | if lvl > self.level: 124 | self.level = lvl 125 | 126 | def add(self, key, val): 127 | """ 128 | Add a key-value pair. 129 | 130 | This method adds a key-value pair. Any type that has a << stream 131 | operator can be passed as the second argument. Formatting is done 132 | using a std::stringstream. 133 | @type key string 134 | @param key Key to be added. 135 | @type value string 136 | @param value Value to be added. 137 | """ 138 | key_ = KeyValue() 139 | key_.key = key 140 | key_.value = val 141 | self.values.append(key_) 142 | -------------------------------------------------------------------------------- /diagnostic_updater/diagnostic_updater/_publisher.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2012, Willow Garage, Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of Willow Garage, Inc. nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | # -*- coding: utf-8 -*- 34 | 35 | """ 36 | diagnostic_updater for Python. 37 | 38 | @author Brice Rebsamen 39 | """ 40 | 41 | from rclpy.time import Time 42 | 43 | from ._diagnostic_updater import CompositeDiagnosticTask 44 | from ._update_functions import FrequencyStatus 45 | from ._update_functions import TimeStampStatus 46 | 47 | 48 | class HeaderlessTopicDiagnostic(CompositeDiagnosticTask): 49 | """ 50 | Class to facilitate making diagnostics for a topic with FrequencyStatus. 51 | 52 | The word "headerless" in the class name refers to the fact that it is 53 | mainly designed for use with messages that do not have a header, and 54 | that cannot therefore be checked using a TimeStampStatus. 55 | """ 56 | 57 | def __init__(self, name, diag, freq): 58 | """ 59 | Construct a HeaderlessTopicDiagnostic. 60 | 61 | @param name The name of the topic that is being diagnosed. 62 | @param diag The diagnostic_updater that the CompositeDiagnosticTask 63 | should add itself to. 64 | @param freq The parameters for the FrequencyStatus class that will be 65 | computing statistics. 66 | """ 67 | CompositeDiagnosticTask.__init__(self, name + ' topic status') 68 | self.diag = diag 69 | self.freq = FrequencyStatus(freq) 70 | self.addTask(self.freq) 71 | self.diag.add(self) 72 | 73 | def tick(self): 74 | """Signals that a publication has occurred.""" 75 | self.freq.tick() 76 | 77 | def clear_window(self): 78 | """Clear the frequency statistics.""" 79 | self.freq.clear() 80 | 81 | 82 | class TopicDiagnostic(HeaderlessTopicDiagnostic): 83 | """ 84 | A class to facilitate making diagnostics for a topic using. 85 | 86 | a FrequencyStatus and TimeStampStatus. 87 | """ 88 | 89 | def __init__(self, name, diag, freq, stamp): 90 | """ 91 | Construct a TopicDiagnostic. 92 | 93 | @param name The name of the topic that is being diagnosed. 94 | @param diag The diagnostic_updater that the CompositeDiagnosticTask 95 | should add itself to. 96 | @param freq The parameters for the FrequencyStatus class that will be 97 | computing statistics. 98 | @param stamp The parameters for the TimeStampStatus class that will be 99 | computing statistics. 100 | """ 101 | HeaderlessTopicDiagnostic.__init__(self, name, diag, freq) 102 | self.stamp = TimeStampStatus(stamp) 103 | self.addTask(self.stamp) 104 | 105 | def tick(self, stamp): 106 | """ 107 | Collect statistics and publishes the message. 108 | 109 | @param stamp Timestamp to use for interval computation by the 110 | TimeStampStatus class. 111 | """ 112 | self.stamp.tick(stamp) 113 | HeaderlessTopicDiagnostic.tick(self) 114 | 115 | 116 | class DiagnosedPublisher(TopicDiagnostic): 117 | """ 118 | A TopicDiagnostic combined with a ros::Publisher. 119 | 120 | For a standard ros::Publisher, this class allows the ros::Publisher and 121 | the TopicDiagnostic to be combined for added convenience. 122 | """ 123 | 124 | def __init__(self, pub, diag, freq, stamp): 125 | """ 126 | Construct a DiagnosedPublisher. 127 | 128 | @param pub The publisher on which statistics are being collected. 129 | @param diag The diagnostic_updater that the CompositeDiagnosticTask 130 | should add itself to. 131 | @param freq The parameters for the FrequencyStatus class that will be 132 | computing statistics. 133 | @param stamp The parameters for the TimeStampStatus class that will be 134 | computing statistics. 135 | """ 136 | TopicDiagnostic.__init__(self, pub.topic_name, diag, freq, stamp) 137 | self.publisher = pub 138 | 139 | def publish(self, message): 140 | """ 141 | Collect statistics and publishes the message. 142 | 143 | The timestamp to be used by the TimeStampStatus class will be 144 | extracted from message.header.stamp. 145 | """ 146 | self.tick(Time.from_msg(message.header.stamp)) 147 | self.publisher.publish(message) 148 | -------------------------------------------------------------------------------- /diagnostic_updater/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | @mainpage diagnostic_updater 4 | 5 | @htmlinclude manifest.html 6 | 7 | \b \ref diagnostic_updater contains assorted C++ classes to assist in diagnostic 8 | publication. These libraries are commonly used by device drivers as part of the diagnostics toolchain. The main parts of diagnostic_updater are: 9 | 10 | - \ref diagnostic_updater::DiagnosticStatusWrapper, a wrapper providing 11 | convenience functions for working with \ref 12 | diagnostics_msgs::DiagnosticStatus. 13 | 14 | - \ref Updater, a class for managing periodic publishing of the \ref 15 | diagnostic_updater::DiagnosticStatusWrapper output by a set of \ref 16 | diagnostic_updater::DiagnosticTask. 17 | 18 | - \ref diagnostic_updater::TopicDiagnostic and \ref 19 | diagnostic_updater::HeaderlessTopicDiagnostic for calculating and 20 | publishing statistics on timestamps and publication frequencies, and 21 | their corresponding \ref diagnostic_updater::DiagnosedPublisher and \ref 22 | diagnostic_updater::HeaderlessDiagnosedPublisher to update the 23 | statistics automatically when publications are made to a topic. 24 | 25 | Example uses of these classes can be found in \ref src/example.cpp. 26 | 27 | */ 28 | -------------------------------------------------------------------------------- /diagnostic_updater/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | diagnostic_updater 5 | 4.4.6 6 | diagnostic_updater contains tools for easily updating diagnostics. it is commonly used in device drivers to keep track of the status of output topics, device status, etc. 7 | Austin Hendrix 8 | Brice Rebsamen 9 | Christian Henkel 10 | Ralph Lange 11 | 12 | BSD-3-Clause 13 | 14 | http://www.ros.org/wiki/diagnostic_updater 15 | 16 | Jeremy Leibs 17 | Blaise Gassend 18 | Kevin Watts 19 | Brice Rebsamen 20 | 21 | ament_cmake 22 | ament_cmake_python 23 | ament_cmake_ros 24 | 25 | diagnostic_msgs 26 | rclcpp 27 | rclpy 28 | std_msgs 29 | 30 | ament_cmake_gtest 31 | ament_cmake_pytest 32 | ament_lint_auto 33 | ament_lint_common 34 | launch 35 | launch_testing 36 | launch_testing_ros 37 | python3-pytest 38 | rclcpp_lifecycle 39 | 40 | 41 | ament_cmake 42 | 43 | 44 | -------------------------------------------------------------------------------- /diagnostic_updater/test/dummy_process.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from diagnostic_updater import Updater 4 | import rclpy 5 | 6 | 7 | def main(): 8 | rclpy.init() 9 | 10 | node = rclpy.create_node('talker') 11 | updater = Updater(node) 12 | updater.add('do_nothing', lambda stat: stat) 13 | updater.verbose = True 14 | node.create_timer(0.1, lambda: updater.update()) 15 | 16 | try: 17 | rclpy.spin(node) 18 | except KeyboardInterrupt: 19 | pass 20 | finally: 21 | node.destroy_node() 22 | rclpy.try_shutdown() 23 | 24 | 25 | if __name__ == '__main__': 26 | main() 27 | -------------------------------------------------------------------------------- /diagnostic_updater/test/status_msg_test.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2023, Outrider Technologies, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | 37 | #include 38 | 39 | #include "rclcpp/rclcpp.hpp" 40 | 41 | #include "diagnostic_msgs/msg/diagnostic_array.hpp" 42 | #include "diagnostic_updater/diagnostic_updater.hpp" 43 | 44 | 45 | class MockUpdaterNode : public rclcpp::Node 46 | { 47 | public: 48 | explicit MockUpdaterNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) 49 | : Node("mock_updater_node", "test_namespace", options), 50 | updater_(this) 51 | { 52 | updater_.setHardwareID("test_hardware_id"); 53 | updater_.add("test_check", this, &MockUpdaterNode::update_diagnostics); 54 | updater_.force_update(); 55 | } 56 | 57 | void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) 58 | { 59 | stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "test message"); 60 | } 61 | 62 | rclcpp::Publisher::SharedPtr diagnostic_array_publisher_; 63 | diagnostic_updater::Updater updater_; 64 | }; 65 | 66 | class ListenerNode : public rclcpp::Node 67 | { 68 | public: 69 | ListenerNode() 70 | : Node("listener_node") 71 | { 72 | subscriber_ = 73 | this->create_subscription( 74 | "diagnostics", 10, 75 | std::bind(&ListenerNode::diagnostics_callback, this, std::placeholders::_1)); 76 | } 77 | 78 | diagnostic_msgs::msg::DiagnosticArray::SharedPtr get_diagnostic_array() const 79 | { 80 | return diagnostic_array_; 81 | } 82 | 83 | private: 84 | void diagnostics_callback(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg) 85 | { 86 | diagnostic_array_ = msg; 87 | } 88 | 89 | diagnostic_msgs::msg::DiagnosticArray::SharedPtr diagnostic_array_; 90 | rclcpp::Subscription::SharedPtr 91 | subscriber_; 92 | }; 93 | 94 | TEST(TestStatusMsg, test_name) { 95 | auto mock_node = std::make_shared(); 96 | auto listener_node = std::make_shared(); 97 | 98 | while (rclcpp::ok() && !listener_node->get_diagnostic_array()) { 99 | rclcpp::spin_some(mock_node); 100 | rclcpp::spin_some(listener_node); 101 | } 102 | 103 | const auto diagnostic_array = listener_node->get_diagnostic_array(); 104 | ASSERT_NE(nullptr, diagnostic_array); 105 | ASSERT_EQ(1u, diagnostic_array->status.size()); 106 | EXPECT_EQ("mock_updater_node: test_check", diagnostic_array->status[0].name); 107 | EXPECT_EQ("test_hardware_id", diagnostic_array->status[0].hardware_id); 108 | EXPECT_EQ("test message", diagnostic_array->status[0].message); 109 | EXPECT_EQ(diagnostic_msgs::msg::DiagnosticStatus::OK, diagnostic_array->status[0].level); 110 | } 111 | 112 | TEST(TestStatusMsg, test_fully_qualified_name) { 113 | rclcpp::NodeOptions options; 114 | options.append_parameter_override("diagnostic_updater.use_fqn", true); 115 | 116 | auto mock_node = std::make_shared(options); 117 | auto listener_node = std::make_shared(); 118 | 119 | while (rclcpp::ok() && !listener_node->get_diagnostic_array()) { 120 | rclcpp::spin_some(mock_node); 121 | rclcpp::spin_some(listener_node); 122 | } 123 | 124 | const auto diagnostic_array = listener_node->get_diagnostic_array(); 125 | ASSERT_NE(nullptr, diagnostic_array); 126 | ASSERT_EQ(1u, diagnostic_array->status.size()); 127 | EXPECT_EQ("/test_namespace/mock_updater_node: test_check", diagnostic_array->status[0].name); 128 | EXPECT_EQ("test_hardware_id", diagnostic_array->status[0].hardware_id); 129 | EXPECT_EQ("test message", diagnostic_array->status[0].message); 130 | EXPECT_EQ(diagnostic_msgs::msg::DiagnosticStatus::OK, diagnostic_array->status[0].level); 131 | } 132 | 133 | int main(int argc, char ** argv) 134 | { 135 | testing::InitGoogleTest(&argc, argv); 136 | rclcpp::init(argc, argv); 137 | 138 | const int ret = RUN_ALL_TESTS(); 139 | 140 | rclcpp::shutdown(); 141 | return ret; 142 | } 143 | -------------------------------------------------------------------------------- /diagnostic_updater/test/status_msg_test.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus 4 | from diagnostic_updater import Updater 5 | import rclpy 6 | 7 | 8 | class TestProcessOutput(unittest.TestCase): 9 | 10 | @ classmethod 11 | def setUpClass(cls): 12 | # Initialize the ROS context for the test node 13 | rclpy.init() 14 | 15 | @ classmethod 16 | def tearDownClass(cls): 17 | # Shutdown the ROS context 18 | rclpy.shutdown() 19 | 20 | def setUp(self): 21 | # Create a ROS node for tests 22 | self.node = rclpy.create_node('listener_node') 23 | self.subscriber = self.node.create_subscription( 24 | DiagnosticArray, 25 | '/diagnostics', 26 | lambda msg: self.received_status.append(msg.status[0]), 27 | 1) 28 | self.received_status = [] 29 | self.updater_node_name = 'updater_node' 30 | 31 | def tearDown(self): 32 | self.node.destroy_node() 33 | 34 | def update_diagnostics(self, stat): 35 | stat.summary(DiagnosticStatus.OK, 'test') 36 | return stat 37 | 38 | def test_name(self): 39 | updater_node = rclpy.create_node(self.updater_node_name) 40 | updater = Updater(updater_node) 41 | updater.setHardwareID('hardware_test') 42 | updater.add('test check', self.update_diagnostics) 43 | updater_node.create_timer(0.1, lambda: updater.update()) 44 | 45 | while len(self.received_status) < 1: 46 | rclpy.spin_once(updater_node) 47 | rclpy.spin_once(self.node) 48 | 49 | self.assertEqual(self.received_status[0].name, self.updater_node_name + ': test check') 50 | self.assertEqual(self.received_status[0].message, 'test') 51 | self.assertEqual(self.received_status[0].level, DiagnosticStatus.OK) 52 | self.assertEqual(self.received_status[0].hardware_id, 'hardware_test') 53 | updater_node.destroy_node() 54 | 55 | def test_fully_qualified_name(self): 56 | param = [rclpy.Parameter('diagnostic_updater.use_fqn', rclpy.Parameter.Type.BOOL, True)] 57 | updater_node = rclpy.create_node(node_name=self.updater_node_name, 58 | namespace='test_namespace', parameter_overrides=param, 59 | automatically_declare_parameters_from_overrides=True) 60 | updater = Updater(updater_node) 61 | updater.setHardwareID('hardware_test') 62 | updater.add('test_check', self.update_diagnostics) 63 | updater_node.create_timer(0.1, lambda: updater.update()) 64 | 65 | while len(self.received_status) < 1: 66 | rclpy.spin_once(updater_node) 67 | rclpy.spin_once(self.node) 68 | 69 | self.assertEqual(self.received_status[0].name, 70 | '/test_namespace/updater_node: test_check') 71 | self.assertEqual(self.received_status[0].message, 'test') 72 | self.assertEqual(self.received_status[0].level, DiagnosticStatus.OK) 73 | self.assertEqual(self.received_status[0].hardware_id, 'hardware_test') 74 | -------------------------------------------------------------------------------- /diagnostic_updater/test/test_diagnostic_status_wrapper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | """@author Brice Rebsamen .""" 5 | 6 | import unittest 7 | 8 | from diagnostic_msgs.msg import DiagnosticStatus 9 | 10 | from diagnostic_updater import DiagnosticStatusWrapper 11 | 12 | 13 | class TestDiagnosticStatusWrapper(unittest.TestCase): 14 | 15 | def test_init_empty(self): 16 | d = DiagnosticStatusWrapper() 17 | self.assertEqual(d.level, DiagnosticStatus.OK) 18 | self.assertEqual(d.message, '') 19 | self.assertEqual(d.values, []) 20 | 21 | def test_init_lvl_msg(self): 22 | d = DiagnosticStatusWrapper(level=DiagnosticStatus.WARN, 23 | message='test') 24 | self.assertEqual(d.level, DiagnosticStatus.WARN) 25 | self.assertEqual(d.message, 'test') 26 | self.assertEqual(d.values, []) 27 | 28 | def test_summary_lvl_msg(self): 29 | d = DiagnosticStatusWrapper() 30 | d.summary(DiagnosticStatus.WARN, 'test') 31 | self.assertEqual(d.level, DiagnosticStatus.WARN) 32 | self.assertEqual(d.message, 'test') 33 | 34 | def test_summary_dmsg(self): 35 | d = DiagnosticStatusWrapper(level=DiagnosticStatus.OK, message='ok') 36 | m = DiagnosticStatus(level=DiagnosticStatus.WARN, message='warn') 37 | d.summary(m) 38 | self.assertEqual(d.level, DiagnosticStatus.WARN) 39 | self.assertEqual(d.message, 'warn') 40 | 41 | def test_clear_summary(self): 42 | d = DiagnosticStatusWrapper(level=DiagnosticStatus.OK, message='ok') 43 | d.clearSummary() 44 | self.assertEqual(d.level, DiagnosticStatus.OK) 45 | self.assertEqual(d.message, '') 46 | 47 | def test_merge_summary_lvl_msg(self): 48 | d = DiagnosticStatusWrapper(level=DiagnosticStatus.OK, message='ok') 49 | d.mergeSummary(DiagnosticStatus.WARN, 'warn') 50 | self.assertEqual(d.level, DiagnosticStatus.WARN) 51 | self.assertEqual(d.message, 'warn') 52 | 53 | d.mergeSummary(DiagnosticStatus.ERROR, 'err') 54 | self.assertEqual(d.level, DiagnosticStatus.ERROR) 55 | self.assertEqual(d.message, 'warn; err') 56 | 57 | def test_merge_summary_dmsg(self): 58 | d = DiagnosticStatusWrapper(level=DiagnosticStatus.OK, message='ok') 59 | m = DiagnosticStatus(level=DiagnosticStatus.WARN, message='warn') 60 | d.mergeSummary(m) 61 | self.assertEqual(d.level, DiagnosticStatus.WARN) 62 | self.assertEqual(d.message, 'warn') 63 | 64 | m = DiagnosticStatus(level=DiagnosticStatus.ERROR, message='err') 65 | d.mergeSummary(m) 66 | self.assertEqual(d.level, DiagnosticStatus.ERROR) 67 | self.assertEqual(d.message, 'warn; err') 68 | 69 | def test_add(self): 70 | d = DiagnosticStatusWrapper() 71 | d.add('key', 'val') 72 | self.assertEqual(d.values[0].key, 'key') 73 | self.assertEqual(d.values[0].value, 'val') 74 | 75 | 76 | if __name__ == '__main__': 77 | unittest.main() 78 | -------------------------------------------------------------------------------- /diagnostics/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package diagnostics 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 4.4.6 (2025-05-26) 6 | ------------------ 7 | * C++17 and cmake 3.20 everywhere (`#510 `_) 8 | * Contributors: Christian Henkel 9 | 10 | 4.4.5 (2025-05-26) 11 | ------------------ 12 | 13 | 4.4.4 (2025-05-12) 14 | ------------------ 15 | 16 | 4.4.2 (2025-02-10) 17 | ------------------ 18 | 19 | 4.3.1 (2024-07-30) 20 | ------------------ 21 | 22 | 3.2.1 (2024-06-27) 23 | ------------------ 24 | 25 | 3.2.0 (2024-03-22) 26 | ------------------ 27 | 28 | 3.1.2 (2023-03-24) 29 | ------------------ 30 | 31 | 3.1.1 (2023-03-16) 32 | ------------------ 33 | * Maintainer update 34 | * Contributors: Austin, Ralph Lange 35 | 36 | 3.1.0 (2023-01-26) 37 | ------------------ 38 | * Adding READMEs to the repo (`#270 `_) 39 | * License fixes (`#263 `_) 40 | * Fix/cleanup ros1 (`#257 `_) 41 | * Contributors: Austin, Christian Henkel, Ralph Lange 42 | 43 | 1.9.3 (2018-05-02) 44 | ------------------ 45 | 46 | 1.9.2 (2017-07-15) 47 | ------------------ 48 | 49 | 1.9.1 (2017-07-15) 50 | ------------------ 51 | 52 | 1.9.0 (2017-04-25) 53 | ------------------ 54 | 55 | 1.8.10 (2016-06-14) 56 | ------------------- 57 | 58 | 1.8.9 (2016-03-02) 59 | ------------------ 60 | 61 | 1.8.8 (2015-08-06) 62 | ------------------ 63 | 64 | 1.8.7 (2015-01-09) 65 | ------------------ 66 | 67 | 1.8.6 (2014-12-10) 68 | ------------------ 69 | 70 | 1.8.5 (2014-07-29) 71 | ------------------ 72 | 73 | 1.8.4 (2014-07-24 20:51) 74 | ------------------------ 75 | 76 | 1.8.3 (2014-04-23) 77 | ------------------ 78 | 79 | 1.8.2 (2014-04-08) 80 | ------------------ 81 | 82 | 1.8.1 (2014-04-07) 83 | ------------------ 84 | * Add myself as maintainer 85 | * Contributors: Austin Hendrix 86 | 87 | 1.8.0 (2013-04-03) 88 | ------------------ 89 | * Updating metapackages to reflect REP-0127 90 | * Contributors: William Woodall 91 | 92 | 1.7.11 (2014-07-24 20:24) 93 | ------------------------- 94 | * Restore diagnostics meta package description 95 | For the wiki etc. 96 | * Updating metapackages to reflect REP-0127 97 | * Contributors: Felix Kolbe, William Woodall 98 | 99 | 1.7.10 (2013-02-22) 100 | ------------------- 101 | * Changed package.xml version number before releasing 102 | * Contributors: Brice Rebsamen 103 | 104 | 1.7.9 (2012-12-14) 105 | ------------------ 106 | 107 | 1.7.8 (2012-12-06) 108 | ------------------ 109 | 110 | 1.7.7 (2012-11-10) 111 | ------------------ 112 | 113 | 1.7.6 (2012-11-07 23:32) 114 | ------------------------ 115 | 116 | 1.7.5 (2012-11-07 21:53) 117 | ------------------------ 118 | 119 | 1.7.4 (2012-11-07 20:18) 120 | ------------------------ 121 | 122 | 1.7.3 (2012-11-04) 123 | ------------------ 124 | 125 | 1.7.2 (2012-10-30 22:31) 126 | ------------------------ 127 | 128 | 1.7.1 (2012-10-30 15:30) 129 | ------------------------ 130 | * fix a few things after the first release 131 | * add the meta-package 132 | * Contributors: Vincent Rabaud 133 | 134 | 1.7.0 (2012-10-29) 135 | ------------------ 136 | -------------------------------------------------------------------------------- /diagnostics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | project(diagnostics) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | endif() 8 | 9 | find_package(ament_cmake REQUIRED) 10 | ament_package() 11 | -------------------------------------------------------------------------------- /diagnostics/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | diagnostics 5 | 4.4.6 6 | diagnostics 7 | Austin Hendrix 8 | Brice Rebsamen 9 | Christian Henkel 10 | Ralph Lange 11 | 12 | BSD-3-Clause 13 | 14 | http://www.ros.org/wiki/diagnostics 15 | 16 | Kevin Watts 17 | Brice Rebsamen 18 | 19 | ament_cmake 20 | 21 | diagnostic_aggregator 22 | diagnostic_common_diagnostics 23 | diagnostic_updater 24 | self_test 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /self_test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | project(self_test) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_C_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | if(NOT DEFINED ENV{ROS_DISTRO}) 14 | message(FATAL_ERROR "ROS_DISTRO is not defined.") 15 | endif() 16 | 17 | message(STATUS "Build for $ENV{ROS_DISTRO}") 18 | 19 | if("$ENV{ROS_DISTRO}" STREQUAL "foxy") 20 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DNO_FUTURE") 21 | elseif("$ENV{ROS_DISTRO}" STREQUAL "galactic") 22 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DNO_FUTURE") 23 | 24 | # elseif("$ENV{ROS_DISTRO}" STREQUAL "humble") 25 | # -- define something here if needed 26 | # elseif("$ENV{ROS_DISTRO}" STREQUAL "rolling") 27 | # -- define something here if needed 28 | endif() 29 | 30 | find_package(ament_cmake REQUIRED) 31 | find_package(diagnostic_msgs REQUIRED) 32 | find_package(diagnostic_updater REQUIRED) 33 | find_package(rclcpp REQUIRED) 34 | 35 | add_executable(run_selftest src/run_selftest.cpp) 36 | target_include_directories(run_selftest 37 | PUBLIC 38 | $ 39 | $ 40 | ) 41 | target_link_libraries(run_selftest 42 | PUBLIC 43 | ${builtin_interfaces_TARGETS} 44 | ${diagnostic_msgs_TARGETS} 45 | diagnostic_updater::diagnostic_updater 46 | rclcpp::rclcpp 47 | rcutils::rcutils 48 | ) 49 | 50 | add_executable(selftest_example example/selftest_example.cpp) 51 | target_include_directories(selftest_example 52 | PUBLIC 53 | $ 54 | $ 55 | ) 56 | target_link_libraries(selftest_example 57 | PUBLIC 58 | ${builtin_interfaces_TARGETS} 59 | ${diagnostic_msgs_TARGETS} 60 | diagnostic_updater::diagnostic_updater 61 | rclcpp::rclcpp 62 | rcutils::rcutils 63 | ) 64 | 65 | if(BUILD_TESTING) 66 | find_package(ament_lint_auto REQUIRED) 67 | set(ament_cmake_copyright_FOUND TRUE) 68 | ament_lint_auto_find_test_dependencies() 69 | 70 | list(APPEND AMENT_LINT_AUTO_EXCLUDE 71 | ament_cmake_uncrustify # Inconsistent between jammy and noble 72 | ) 73 | 74 | add_subdirectory(test) 75 | endif() 76 | 77 | install( 78 | DIRECTORY include/ 79 | DESTINATION include 80 | ) 81 | 82 | install( 83 | TARGETS run_selftest selftest_example 84 | DESTINATION lib/${PROJECT_NAME} 85 | ) 86 | 87 | ament_export_include_directories(include) 88 | ament_package() 89 | -------------------------------------------------------------------------------- /self_test/README.md: -------------------------------------------------------------------------------- 1 | General information about this repository, including legal information, build instructions and known issues/limitations, are given in [README.md](../README.md) in the repository root. 2 | 3 | # The self_test package 4 | This package can be used to implement self tests for ROS packages. 5 | 6 | # Overview 7 | It publishes a service for the node to call to perform the self test. 8 | This then performs multiple user-defined checks on the node and reports the results. 9 | 10 | # Example 11 | The file [selftest_example.cpp](src/selftest_example.cpp) contains an example of how to use the self_test package. 12 | 13 | When we then call `$ ros2 run self_test run_selftest` we get the following output: 14 | ``` 15 | [INFO] [...] [...]: Self test FAILED for device with id: [12345] 16 | [INFO] [...] [...]: 1) Pretest 17 | [INFO] [...] [...]: Pretest completed successfully. 18 | [INFO] [...] [...]: 2) ID Lookup 19 | [INFO] [...] [...]: ID Lookup successful 20 | [INFO] [...] [...]: 3) Exception generating test 21 | [ERROR] [...] [...]: Uncaught exception: we did something that threw an exception 22 | [INFO] [...] [...]: 4) Value generating test 23 | [INFO] [...] [...]: We successfully changed the value. 24 | [INFO] [...] [...]: [some value] 42 25 | [INFO] [...] [...]: 5) Value testing test 26 | [INFO] [...] [...]: We observed the change in value 27 | ``` 28 | 29 | # C++ API 30 | The `TestRunner` class is the main class for self tests. 31 | It has a method `_add` which must be used to add the specific test as callback methods. 32 | The `TestRunner` then advertises the relevant `self_test` service and calls the aforemntioned callbacks when requested. 33 | 34 | # Nodes 35 | ## run_selftest 36 | This node is used to call the self test service. 37 | -------------------------------------------------------------------------------- /self_test/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | @mainpage self_test 4 | 5 | @htmlinclude manifest.html 6 | 7 | \b self_test contains the \ref self_test::TestRunner class that can be used 8 | to sequence a set of tests to be run in order to test a device. It 9 | advertises a self_test service. When the service is called, the \ref 10 | self_test::TestRunner calls the tests that have been defined in order, and 11 | combines the results into a \ref diagnostic_msgs::DiagnosticsArray. A 12 | detailed example can be found in \ref selftest_example.cpp. 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /self_test/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | self_test 5 | 4.4.6 6 | self_test 7 | Austin Hendrix 8 | Brice Rebsamen 9 | Christian Henkel 10 | Ralph Lange 11 | 12 | BSD-3-Clause 13 | 14 | http://www.ros.org/wiki/self_test 15 | 16 | Jeremy Leibs and Blaise Gassend 17 | Kevin Watts 18 | Brice Rebsamen 19 | 20 | ament_cmake 21 | 22 | ros_environment 23 | 24 | diagnostic_msgs 25 | diagnostic_updater 26 | rclcpp 27 | 28 | ament_lint_auto 29 | ament_lint_common 30 | ament_cmake_gtest 31 | 32 | 33 | ament_cmake 34 | 35 | 36 | -------------------------------------------------------------------------------- /self_test/src/run_selftest.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include "diagnostic_msgs/srv/self_test.hpp" 40 | #include "rclcpp/rclcpp.hpp" 41 | 42 | class ClientNode : public rclcpp::Node 43 | { 44 | public: 45 | explicit ClientNode(std::string _node_name) 46 | : Node("self_test_client"), node_name_to_test(_node_name) 47 | { 48 | client_ = create_client( 49 | node_name_to_test + 50 | "/self_test"); 51 | } 52 | 53 | rclcpp::Client::SharedFuture 54 | queue_async_request() 55 | { 56 | using namespace std::chrono_literals; 57 | using ServiceResponseFuture = 58 | rclcpp::Client::SharedFuture; 59 | 60 | while (!client_->wait_for_service(1s)) { 61 | if (!rclcpp::ok()) { 62 | RCLCPP_ERROR( 63 | this->get_logger(), 64 | "Interrupted while waiting for the service. Exiting."); 65 | return ServiceResponseFuture(); 66 | } 67 | RCLCPP_INFO( 68 | this->get_logger(), 69 | "service not available, waiting again..."); 70 | } 71 | auto request = std::make_shared(); 72 | 73 | auto response_received_callback = [this](ServiceResponseFuture future) { 74 | auto result_out = future.get(); 75 | 76 | RCLCPP_INFO( 77 | this->get_logger(), "Self test %s for device with id: [%s]", 78 | (result_out->passed ? "PASSED" : "FAILED"), 79 | result_out->id.c_str()); 80 | 81 | // for (size_t i = 0; i < result_out->status.size(); i++) { 82 | auto counter = 1lu; 83 | for (const auto & status : result_out->status) { 84 | RCLCPP_INFO( 85 | this->get_logger(), "%zu) %s", counter++, 86 | status.name.c_str()); 87 | if (status.level == 0) { 88 | RCLCPP_INFO(this->get_logger(), "\t%s", status.message.c_str()); 89 | } else if (status.level == 1) { 90 | RCLCPP_WARN(this->get_logger(), "\t%s", status.message.c_str()); 91 | } else { 92 | RCLCPP_ERROR(this->get_logger(), "\t%s", status.message.c_str()); 93 | } 94 | 95 | for (const auto & kv : status.values) { 96 | RCLCPP_INFO( 97 | this->get_logger(), "\t[%s] %s", kv.key.c_str(), 98 | kv.value.c_str()); 99 | } 100 | } 101 | }; 102 | #if defined(NO_FUTURE) 103 | return client_->async_send_request(request, response_received_callback); 104 | #else 105 | return client_->async_send_request(request, response_received_callback).future; 106 | #endif 107 | } 108 | 109 | private: 110 | rclcpp::Client::SharedPtr client_; 111 | std::string node_name_to_test; 112 | }; 113 | 114 | int main(int argc, char ** argv) 115 | { 116 | rclcpp::init(argc, argv); 117 | if (argc != 2) { 118 | RCLCPP_ERROR( 119 | rclcpp::get_logger("run_selftest"), 120 | "usage: run_selftest NODE_NAME"); 121 | return 1; 122 | } 123 | std::string node_name = argv[1]; 124 | 125 | auto node = std::make_shared(node_name); 126 | auto async_srv_request = node->queue_async_request(); 127 | if (!async_srv_request.valid()) { 128 | rclcpp::shutdown(); 129 | return -1; 130 | } 131 | 132 | rclcpp::spin_until_future_complete(node, async_srv_request); 133 | rclcpp::shutdown(); 134 | return 0; 135 | } 136 | -------------------------------------------------------------------------------- /self_test/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(ament_cmake_gtest REQUIRED) 2 | 3 | macro(add_self_test target_name file_path) 4 | ament_add_gtest(${target_name} ${file_path}) 5 | if(TARGET ${target_name}) 6 | target_link_libraries(${target_name} 7 | ${diagnostic_msgs_TARGETS} 8 | diagnostic_updater::diagnostic_updater 9 | rclcpp::rclcpp 10 | ) 11 | target_include_directories(${target_name} 12 | PUBLIC 13 | $ 14 | $ 15 | ) 16 | endif() 17 | endmacro() 18 | 19 | add_self_test(test_nominal_selftest nominal_selftest.cpp) 20 | add_self_test(test_no_id_selftest no_id_selftest.cpp) 21 | add_self_test(test_exception_selftest exception_selftest.cpp) 22 | add_self_test(test_error_selftest error_selftest.cpp) 23 | -------------------------------------------------------------------------------- /self_test/test/error_selftest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /*! 31 | *\author Kevin Watts 32 | *\brief Simple node with a self test that reports an error 33 | */ 34 | 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | #include "selftest_fixture.hpp" 41 | #include "selftest_node.hpp" 42 | 43 | class ErrorSelftestNode : public SelftestNode 44 | { 45 | public: 46 | ErrorSelftestNode() 47 | : SelftestNode("error_selftest_node") 48 | {} 49 | 50 | void setup_test_cases() override 51 | { 52 | self_test_.add("Pretest", this, &SelftestNode::pretest); 53 | 54 | self_test_.add("ID Lookup", this, &SelftestNode::test1); 55 | self_test_.add("Error generating test", this, &ErrorSelftestNode::test2); 56 | self_test_.add("Value generating test", this, &SelftestNode::test3); 57 | self_test_.add("Value testing test", this, &SelftestNode::test4); 58 | 59 | self_test_.add("Posttest", this, &SelftestNode::pretest); 60 | } 61 | 62 | void test2(diagnostic_updater::DiagnosticStatusWrapper & status) override 63 | { 64 | // Report an error, this should fail self test 65 | status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Reporting an error"); 66 | } 67 | }; 68 | 69 | // using directive necessary as gtest macro TEST_F gets confused with template classes 70 | using Fixture = SelftestFixture; 71 | TEST_F(Fixture, run_self_test) 72 | { 73 | auto client = node_->create_client( 74 | "error_selftest_node/self_test"); 75 | 76 | using namespace std::chrono_literals; 77 | if (!client->wait_for_service(5s)) { 78 | FAIL() << "could not connect to self test service"; 79 | } 80 | 81 | auto request = std::make_shared(); 82 | 83 | using ServiceResponseFuture = 84 | rclcpp::Client::SharedFuture; 85 | auto response_received_callback = [](ServiceResponseFuture future) { 86 | auto result_out = future.get(); 87 | 88 | EXPECT_FALSE(result_out->passed) << "ErrorSelfTestNode shall not pass!"; 89 | EXPECT_STREQ(std::to_string(12345).c_str(), result_out->id.c_str()); 90 | unsigned char max_level = 0; 91 | for (const auto & status : result_out->status) { 92 | max_level = std::max(status.level, max_level); 93 | auto some_val = std::find_if( 94 | status.values.begin(), status.values.end(), [](auto it) { 95 | return it.key == "some val"; 96 | }); 97 | if (some_val != status.values.end()) { 98 | EXPECT_EQ(std::to_string(42), some_val->value); 99 | } 100 | } 101 | EXPECT_TRUE(max_level > 0) << 102 | "Self test failed, but no sub tests reported a failure or warning"; 103 | }; 104 | auto future = client->async_send_request(request, response_received_callback); 105 | if (!future.valid()) { 106 | FAIL() << "could not correctly send self test service request"; 107 | } 108 | rclcpp::spin_until_future_complete(node_, future); 109 | } 110 | -------------------------------------------------------------------------------- /self_test/test/exception_selftest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /*! 31 | *\brief Simple self test that throws an exception 32 | *\author Kevin Watts 33 | */ 34 | 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | #include "selftest_fixture.hpp" 42 | #include "selftest_node.hpp" 43 | 44 | class ExceptionSelftestNode : public SelftestNode 45 | { 46 | public: 47 | ExceptionSelftestNode() 48 | : SelftestNode("exception_selftest_node") 49 | {} 50 | 51 | void setup_test_cases() override 52 | { 53 | self_test_.add("Pretest", this, &SelftestNode::pretest); 54 | 55 | self_test_.add("ID Lookup", this, &SelftestNode::test1); 56 | self_test_.add("Exception generating test", this, &ExceptionSelftestNode::test2); 57 | self_test_.add("Value generating test", this, &SelftestNode::test3); 58 | self_test_.add("Value testing test", this, &SelftestNode::test4); 59 | 60 | self_test_.add("Posttest", this, &SelftestNode::pretest); 61 | } 62 | 63 | // Throwing an exception 64 | void test2(diagnostic_updater::DiagnosticStatusWrapper & status) override 65 | { 66 | status.level = 0; 67 | 68 | throw std::runtime_error("we did something that threw an exception"); 69 | 70 | // Here's where we would report success if we'd made it past 71 | status.summary( 72 | diagnostic_msgs::msg::DiagnosticStatus::OK, 73 | "We made it past the exception throwing statement."); 74 | } 75 | }; 76 | 77 | // using directive necessary as gtest macro TEST_F gets confused with template classes 78 | using Fixture = SelftestFixture; 79 | TEST_F(Fixture, run_self_test) 80 | { 81 | auto client = node_->create_client( 82 | "exception_selftest_node/self_test"); 83 | 84 | using namespace std::chrono_literals; 85 | if (!client->wait_for_service(5s)) { 86 | FAIL() << "could not connect to self test service"; 87 | } 88 | 89 | auto request = std::make_shared(); 90 | 91 | using ServiceResponseFuture = 92 | rclcpp::Client::SharedFuture; 93 | auto response_received_callback = [](ServiceResponseFuture future) { 94 | auto result_out = future.get(); 95 | 96 | EXPECT_FALSE(result_out->passed) << "ExceptionSelfTestNode shall not pass!"; 97 | EXPECT_STREQ(std::to_string(12345).c_str(), result_out->id.c_str()); 98 | unsigned char max_level = 0; 99 | bool found_exception = false; 100 | for (const auto & status : result_out->status) { 101 | max_level = std::max(status.level, max_level); 102 | auto some_val = std::find_if( 103 | status.values.begin(), status.values.end(), [](auto it) { 104 | return it.key == "some val"; 105 | }); 106 | if (some_val != status.values.end()) { 107 | EXPECT_EQ(std::to_string(42), some_val->value); 108 | } 109 | auto ex = status.message.find("exception"); 110 | if (ex != std::string::npos) { 111 | found_exception = true; 112 | } 113 | } 114 | EXPECT_TRUE(max_level > 0) << 115 | "Self test failed, but no sub tests reported a failure or warning"; 116 | EXPECT_TRUE(found_exception) << 117 | "Self test threw and exception, but we didn't catch it and report it"; 118 | }; 119 | auto future = client->async_send_request(request, response_received_callback); 120 | if (!future.valid()) { 121 | FAIL() << "could not correctly send self test service request"; 122 | } 123 | rclcpp::spin_until_future_complete(node_, future); 124 | } 125 | -------------------------------------------------------------------------------- /self_test/test/no_id_selftest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2010, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /*! 31 | *\brief Simple self test that does not set a node ID on return 32 | *\author Kevin Watts 33 | */ 34 | 35 | #include 36 | 37 | #include 38 | 39 | #include "selftest_fixture.hpp" 40 | #include "selftest_node.hpp" 41 | 42 | class NoIDSelftestNode : public SelftestNode 43 | { 44 | public: 45 | NoIDSelftestNode() 46 | : SelftestNode("no_id_selftest_node") 47 | {} 48 | 49 | void setup_test_cases() override 50 | { 51 | self_test_.add("Pretest", this, &SelftestNode::pretest); 52 | 53 | self_test_.add("ID Lookup", this, &NoIDSelftestNode::test1); 54 | self_test_.add("Value generating test", this, &NoIDSelftestNode::test3); 55 | self_test_.add("Value testing test", this, &NoIDSelftestNode::test4); 56 | 57 | self_test_.add("Posttest", this, &NoIDSelftestNode::pretest); 58 | } 59 | 60 | void test1(diagnostic_updater::DiagnosticStatusWrapper & status) override 61 | { 62 | status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "ID not set"); 63 | } 64 | }; 65 | 66 | // using directive necessary as gtest macro TEST_F gets confused with template classes 67 | using Fixture = SelftestFixture; 68 | TEST_F(Fixture, run_self_test) 69 | { 70 | auto client = 71 | node_->create_client("no_id_selftest_node/self_test"); 72 | 73 | using namespace std::chrono_literals; 74 | if (!client->wait_for_service(5s)) { 75 | FAIL() << "could not connect to self test service"; 76 | } 77 | 78 | auto request = std::make_shared(); 79 | 80 | using ServiceResponseFuture = 81 | rclcpp::Client::SharedFuture; 82 | auto response_received_callback = [](ServiceResponseFuture future) { 83 | auto result_out = future.get(); 84 | 85 | EXPECT_TRUE(result_out->passed) << "NoIDSelfTestNode is expected to pass"; 86 | EXPECT_STREQ("", result_out->id.c_str()) << "NoIDSelfTestNode should not have an ID"; 87 | for (const auto & status : result_out->status) { 88 | EXPECT_EQ(0, status.level); 89 | auto some_val = std::find_if( 90 | status.values.begin(), status.values.end(), [](auto it) { 91 | return it.key == "some val"; 92 | }); 93 | if (some_val != status.values.end()) { 94 | EXPECT_EQ(std::to_string(42), some_val->value); 95 | } 96 | } 97 | }; 98 | auto future = client->async_send_request(request, response_received_callback); 99 | if (!future.valid()) { 100 | FAIL() << "could not correctly send self test service request"; 101 | } 102 | rclcpp::spin_until_future_complete(node_, future); 103 | } 104 | -------------------------------------------------------------------------------- /self_test/test/nominal_selftest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2010, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | 32 | #include 33 | 34 | #include "selftest_fixture.hpp" 35 | #include "selftest_node.hpp" 36 | 37 | class NominalSelftestNode : public SelftestNode 38 | { 39 | public: 40 | NominalSelftestNode() 41 | : SelftestNode("nominal_selftest_node") 42 | {} 43 | }; 44 | 45 | // using directive necessary as gtest macro TEST_F gets confused with template classes 46 | using Fixture = SelftestFixture; 47 | TEST_F(Fixture, run_self_test) 48 | { 49 | auto client = node_->create_client( 50 | "nominal_selftest_node/self_test"); 51 | 52 | using namespace std::chrono_literals; 53 | if (!client->wait_for_service(5s)) { 54 | FAIL() << "could not connect to self test service"; 55 | } 56 | 57 | auto request = std::make_shared(); 58 | 59 | using ServiceResponseFuture = 60 | rclcpp::Client::SharedFuture; 61 | auto response_received_callback = [](ServiceResponseFuture future) { 62 | auto result_out = future.get(); 63 | 64 | EXPECT_TRUE(result_out->passed) << "NominalSelftestNode is expected to pass"; 65 | EXPECT_STREQ(std::to_string(12345).c_str(), result_out->id.c_str()); 66 | for (const auto & status : result_out->status) { 67 | EXPECT_EQ(0, status.level); 68 | auto some_val = std::find_if( 69 | status.values.begin(), status.values.end(), [](auto it) { 70 | return it.key == "some val"; 71 | }); 72 | if (some_val != status.values.end()) { 73 | EXPECT_EQ(std::to_string(42), some_val->value); 74 | } 75 | } 76 | }; 77 | auto future = client->async_send_request(request, response_received_callback); 78 | if (!future.valid()) { 79 | FAIL() << "could not correctly send self test service request"; 80 | } 81 | rclcpp::spin_until_future_complete(node_, future); 82 | } 83 | -------------------------------------------------------------------------------- /self_test/test/selftest_fixture.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2010, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef SELFTEST_FIXTURE_HPP_ 31 | #define SELFTEST_FIXTURE_HPP_ 32 | 33 | #include 34 | 35 | #include 36 | 37 | #include "rclcpp/rclcpp.hpp" 38 | 39 | template 40 | class SelftestFixture : public ::testing::Test 41 | { 42 | protected: 43 | static void SetUpTestCase() 44 | { 45 | rclcpp::init(0, nullptr); 46 | } 47 | 48 | static void TearDownTestCase() 49 | { 50 | rclcpp::shutdown(); 51 | } 52 | 53 | void SetUp() 54 | { 55 | node_ = std::make_shared(); 56 | } 57 | 58 | void TearDown() 59 | { 60 | node_.reset(); 61 | } 62 | 63 | std::shared_ptr node_; 64 | }; 65 | 66 | #endif // SELFTEST_FIXTURE_HPP_ 67 | -------------------------------------------------------------------------------- /self_test/test/selftest_node.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2010, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef SELFTEST_NODE_HPP_ 31 | #define SELFTEST_NODE_HPP_ 32 | 33 | #include 34 | #include 35 | 36 | #include "self_test/test_runner.hpp" 37 | 38 | /* 39 | *\author Kevin Watts 40 | *\brief Returns nominal self-test values 41 | */ 42 | class SelftestNode : public rclcpp::Node 43 | { 44 | protected: 45 | self_test::TestRunner self_test_; 46 | double some_val_; 47 | 48 | public: 49 | explicit SelftestNode(const std::string & node_name) 50 | : rclcpp::Node(node_name), 51 | self_test_( 52 | get_node_base_interface(), get_node_services_interface(), get_node_logging_interface()) 53 | { 54 | setup_test_cases(); 55 | } 56 | 57 | virtual ~SelftestNode() = default; 58 | 59 | virtual void setup_test_cases() 60 | { 61 | self_test_.add("Pretest", this, &SelftestNode::pretest); 62 | 63 | self_test_.add("ID Lookup", this, &SelftestNode::test1); 64 | self_test_.add("Exception generating test", this, &SelftestNode::test2); 65 | self_test_.add("Value generating test", this, &SelftestNode::test3); 66 | self_test_.add("Value testing test", this, &SelftestNode::test4); 67 | 68 | self_test_.add("Posttest", this, &SelftestNode::pretest); 69 | } 70 | 71 | virtual void pretest(diagnostic_updater::DiagnosticStatusWrapper & status) 72 | { 73 | RCLCPP_INFO(this->get_logger(), "Doing preparation stuff before we run our test."); 74 | status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Pretest completed successfully."); 75 | some_val_ = 1.0; 76 | } 77 | 78 | virtual void test1(diagnostic_updater::DiagnosticStatusWrapper & status) 79 | { 80 | // Look up ID here 81 | char ID[] = "12345"; 82 | bool lookup_successful = true; 83 | 84 | if (lookup_successful) { 85 | status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "ID Lookup successful"); 86 | self_test_.setID(ID); 87 | } else { 88 | status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "ID Lookup failed"); 89 | } 90 | } 91 | 92 | // Tests do not necessarily need to catch their exceptions. 93 | virtual void test2(diagnostic_updater::DiagnosticStatusWrapper & status) 94 | { 95 | status.level = 0; 96 | 97 | // Here's where we would report success if we'd made it past 98 | status.summary( 99 | diagnostic_msgs::msg::DiagnosticStatus::OK, 100 | "We made it past the exception throwing statement."); 101 | } 102 | 103 | virtual void test3(diagnostic_updater::DiagnosticStatusWrapper & status) 104 | { 105 | some_val_ += 41.0; 106 | 107 | status.add("some value", some_val_); 108 | status.summary( 109 | diagnostic_msgs::msg::DiagnosticStatus::OK, "We successfully changed the value."); 110 | } 111 | 112 | virtual void test4(diagnostic_updater::DiagnosticStatusWrapper & status) 113 | { 114 | if (some_val_ == 42.0) { 115 | status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "We observed the change in value"); 116 | } else { 117 | status.summaryf( 118 | diagnostic_msgs::msg::DiagnosticStatus::ERROR, 119 | "We failed to observe the change in value, it is currently %f.", some_val_); 120 | } 121 | } 122 | 123 | virtual void posttest(diagnostic_updater::DiagnosticStatusWrapper & status) 124 | { 125 | RCLCPP_INFO(this->get_logger(), "Doing cleanup stuff after we run our test."); 126 | status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Posttest completed successfully."); 127 | } 128 | }; 129 | #endif // SELFTEST_NODE_HPP_ 130 | --------------------------------------------------------------------------------