From c6a77b970c90b08a838c23a45ddfdc4e735724d5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ingo=20L=C3=BCtkebohle=20=28CR/ASD1=29?= Date: Wed, 2 Apr 2025 14:24:00 +0200 Subject: [PATCH 1/6] Original topic monitor version for galactic --- rob_topic_monitor/.gitignore | 2 + rob_topic_monitor/CMakeLists.txt | 106 ++++++ rob_topic_monitor/README.md | 82 +++++ .../config/diagnostics.perspective | 221 +++++++++++++ rob_topic_monitor/config/foxglove_bridge.yaml | 34 ++ rob_topic_monitor/config/topic_check.yaml | 45 +++ .../include/update_functions.hpp | 307 ++++++++++++++++++ .../launch/topic_monitor_launch.py | 76 +++++ rob_topic_monitor/package.xml | 34 ++ .../src/header_topic_monitor_component.cpp | 234 +++++++++++++ .../src/topic_monitor_component.cpp | 283 ++++++++++++++++ rob_topic_monitor/test/test_topic_monitor.py | 241 ++++++++++++++ 12 files changed, 1665 insertions(+) create mode 100644 rob_topic_monitor/.gitignore create mode 100644 rob_topic_monitor/CMakeLists.txt create mode 100644 rob_topic_monitor/README.md create mode 100644 rob_topic_monitor/config/diagnostics.perspective create mode 100644 rob_topic_monitor/config/foxglove_bridge.yaml create mode 100644 rob_topic_monitor/config/topic_check.yaml create mode 100644 rob_topic_monitor/include/update_functions.hpp create mode 100644 rob_topic_monitor/launch/topic_monitor_launch.py create mode 100644 rob_topic_monitor/package.xml create mode 100644 rob_topic_monitor/src/header_topic_monitor_component.cpp create mode 100644 rob_topic_monitor/src/topic_monitor_component.cpp create mode 100755 rob_topic_monitor/test/test_topic_monitor.py diff --git a/rob_topic_monitor/.gitignore b/rob_topic_monitor/.gitignore new file mode 100644 index 000000000..cc4087c20 --- /dev/null +++ b/rob_topic_monitor/.gitignore @@ -0,0 +1,2 @@ +.pytest_cache +**/__pycache__ \ No newline at end of file diff --git a/rob_topic_monitor/CMakeLists.txt b/rob_topic_monitor/CMakeLists.txt new file mode 100644 index 000000000..32da41042 --- /dev/null +++ b/rob_topic_monitor/CMakeLists.txt @@ -0,0 +1,106 @@ +cmake_minimum_required(VERSION 3.8) +project(rob_topic_monitor) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +find_package(ament_cmake_python REQUIRED) + +find_package(rclpy REQUIRED) +find_package(diagnostic_msgs REQUIRED) +find_package(diagnostic_updater REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(lifecycle_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(diagnostic_updater REQUIRED) +find_package(std_msgs REQUIRED) + +add_library(topic_monitor_component SHARED src/topic_monitor_component.cpp) +rclcpp_components_register_node(topic_monitor_component + PLUGIN "rob_topic_monitor::TopicMonitor" + EXECUTABLE topic_monitor +) +target_include_directories(topic_monitor_component PRIVATE include) +ament_target_dependencies(topic_monitor_component + "lifecycle_msgs" + "rclcpp" + "rclcpp_lifecycle" + "rclcpp_components" + "diagnostic_msgs" + "diagnostic_updater" +) +ament_export_targets(export_topic_monitor_component) +install(TARGETS topic_monitor_component + EXPORT export_topic_monitor_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +add_library(header_topic_monitor_component SHARED src/header_topic_monitor_component.cpp) +rclcpp_components_register_node(header_topic_monitor_component + PLUGIN "rob_topic_monitor::HeaderTopicMonitor" + EXECUTABLE header_topic_monitor +) +target_include_directories(header_topic_monitor_component PRIVATE include) +ament_target_dependencies(header_topic_monitor_component + "lifecycle_msgs" + "std_msgs" + "rclcpp" + "rclcpp_lifecycle" + "rclcpp_components" + "diagnostic_msgs" + "diagnostic_updater" +) +ament_export_targets(export_header_topic_monitor_component) +install(TARGETS header_topic_monitor_component + EXPORT export_header_topic_monitor_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + + +# Install launch files. +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ +) +# Install config files. +install( + DIRECTORY config + DESTINATION share/${PROJECT_NAME}/ +) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_pytest REQUIRED) + file(GLOB _pytest_tests TESTS test/*.py) + foreach(_test_path ${_pytest_tests}) + get_filename_component(_test_name ${_test_path} NAME_WE) + ament_add_pytest_test(${_test_name} ${_test_path} + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + TIMEOUT 60 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + endforeach() +endif() + +ament_package() diff --git a/rob_topic_monitor/README.md b/rob_topic_monitor/README.md new file mode 100644 index 000000000..2b5aaa3ac --- /dev/null +++ b/rob_topic_monitor/README.md @@ -0,0 +1,82 @@ +# ROB Topic Monitor + +This is a generic topic monitor to check that topic publication rate is as expected, reporting status using diagnostics. + +## Acknowledgments + +Originally developed for TOP-100 "BAUTIRO". + +## Technical Debt + +This code was originally developed for ROS 2 "Galactic" and contains backports of some functionality that are no longer necessary for later ROS 2 versions. + +Moreover, the header_topic_monitor is largely copy-pasted. + +## Usage + +The nodes and components use the ROS 2 life-cycle based, which means they won't just activate when run. See `launch/topic_monitor_launch.py` for an example of how to start the nodes. + +### Nodes + +There are two nodes which behave almost the same + * topic_monitor_node -- this supports _any_ kind of topic and checks for frequency based on receive time + * header_topic_monitor_node -- this supports only topics with a Header, and it checks that message age is within a given range + +It is totally possible to run both of these at the same time _for the same topics_, to check for both frequency +and max message age at the same time. + +Some of the parameters are common for both nodes: + +#### Limiting which topics to monitor + +By default, the following internal topic names will be ignored, as they are usually irrelevant: + +* ^/rosout$ +* .*/parameter_events +* ^/diagnostics$ +* .*/transition_event + +`monitor_configured_only` + If false (the default), the topic monitor will monitor everything but the internal topics mentioned above. + To instead only monitor those topics which have been explicitly configured, set this to "true" + +#### Influencing the output + +`diag_prefix` + This string will be prefixed to the name of the diagnostic for use within the aggregator. + +### `topic_monitor_node` + +Without any parameters, this node (component also available) will subscribe to all normal topics in a system and minimally monitor that there is activity on them (defined as "at least one message per second"). For more specific configuration, the following parameters are available. + +#### Frequency Monitoring + +Usually, we want to make sure that the frequency of a topic is within certain bounds. + +For example, to monitor that the frequency of the "talker" topic is about 25Hz and the frequency of the "cmd_vel" topic is about 10Hz, use the following three parameters and pass them lists in the same order. This means, the second element in "min_freqs" applies to the topic given by the second element of the "topics" parameter. + + topics: [ "talker", "cmd_vel" ] + min_freqs: [ 23.0, 8.0 ] + max_freqs: [ 26.5, 11.0 ] + +Please note that the frequencies will be computed purely based on _arrival_ timing, the header is currently ignored. + +### `header_topic_monitor_node` + +Without any parameters, this node (component also available) will subscribe to all normal topics in a system and minimally monitor that there is activity on them (defined as "at least one message per second"). For more specific configuration, the following parameters are available. + +#### Message Age Monitoring + +Usually, we want to make sure that message is not much older than expected (allowing for some jitter due to network transmission), +but also not newer than expected (which would indicate issues with timestamp or system time between machines). + +Since this relies on a sender-side timestamp, this monitor only works with message that include a Header. + +NOTE: You can configure it for any topic and it _will_ interpret the data in there as a timestamp, but unless it +really is a header, the results will be rather random ;-) + +Configuration is similar to the frequency monitor, but giving a range for delays instead: + + topics: [ "talker", "cmd_vel" ] + min_delays: [ 80, 8.0 ] + max_delays: [ 120, 11.0 ] diff --git a/rob_topic_monitor/config/diagnostics.perspective b/rob_topic_monitor/config/diagnostics.perspective new file mode 100644 index 000000000..45bc8679a --- /dev/null +++ b/rob_topic_monitor/config/diagnostics.perspective @@ -0,0 +1,221 @@ +{ + "keys": {}, + "groups": { + "mainwindow": { + "keys": { + "geometry": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb0003000000000f8b0000007000001f9f0000070000000f8b000000ba00001f9f000007000000000100000000180000000f8b000000ba00001f9f00000700')", + "type": "repr(QByteArray.hex)", + "pretty-print": " p " + }, + "state": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd00000001000000030000101500000603fc0100000003fb00000022007200710074005f0074006f0070005f005f0054004f0050005f005f0031005f005f0100000000000002c80000013700fffffffc000002d4000006770000028500fffffffc0200000002fb0000004c007200710074005f0063006f006e0073006f006c0065005f005f0043006f006e0073006f006c0065005f005f0031005f005f0043006f006e0073006f006c00650057006900640067006500740100000022000002e30000029f00fffffffb00000078007200710074005f00720075006e00740069006d0065005f006d006f006e00690074006f0072005f005f00520075006e00740069006d0065004d006f006e00690074006f0072005f005f0031005f005f00520075006e00740069006d0065004d006f006e00690074006f0072005700690064006700650074010000031100000314000000ab00fffffffb0000004c007200710074005f0074006f007000690063005f005f0054006f0070006900630050006c007500670069006e005f005f0031005f005f0054006f0070006900630057006900640067006500740100000957000006be0000011c00ffffff000010150000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", + "type": "repr(QByteArray.hex)", + "pretty-print": " \"rqt_top__TOP__1__ w \" W " + } + }, + "groups": { + "toolbar_areas": { + "keys": { + "MinimizedDockWidgetsToolbar": { + "repr": "8", + "type": "repr" + } + }, + "groups": {} + } + } + }, + "pluginmanager": { + "keys": { + "running-plugins": { + "repr": "{'rqt_console/Console': [1], 'rqt_runtime_monitor/RuntimeMonitor': [1], 'rqt_top/TOP': [1], 'rqt_topic/TopicPlugin': [1]}", + "type": "repr" + } + }, + "groups": { + "plugin__rqt_console__Console__1": { + "keys": {}, + "groups": { + "dock_widget__ConsoleWidget": { + "keys": { + "dock_widget_title": { + "repr": "'Console'", + "type": "repr" + }, + "dockable": { + "repr": "True", + "type": "repr" + }, + "parent": { + "repr": "None", + "type": "repr" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "exclude_filters": { + "repr": "'severity'", + "type": "repr" + }, + "filter_splitter": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff0000000100000002000000d4000000d40100000009010000000200')", + "type": "repr(QByteArray.hex)", + "pretty-print": " " + }, + "highlight_filters": { + "repr": "'message'", + "type": "repr" + }, + "message_limit": { + "repr": "20000", + "type": "repr" + }, + "paused": { + "repr": "False", + "type": "repr" + }, + "settings_exist": { + "repr": "True", + "type": "repr" + }, + "show_highlighted_only": { + "repr": "False", + "type": "repr" + }, + "table_splitter": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000001000000020000008a000001b10100000009010000000200')", + "type": "repr(QByteArray.hex)", + "pretty-print": " " + } + }, + "groups": { + "exclude_filter_0": { + "keys": { + "enabled": { + "repr": "True", + "type": "repr" + }, + "itemlist": { + "repr": "''", + "type": "repr" + } + }, + "groups": {} + }, + "highlight_filter_0": { + "keys": { + "enabled": { + "repr": "True", + "type": "repr" + }, + "regex": { + "repr": "False", + "type": "repr" + }, + "text": { + "repr": "''", + "type": "repr" + } + }, + "groups": {} + } + } + } + } + }, + "plugin__rqt_runtime_monitor__RuntimeMonitor__1": { + "keys": {}, + "groups": { + "dock_widget__RuntimeMonitorWidget": { + "keys": { + "dock_widget_title": { + "repr": "'Runtime Monitor'", + "type": "repr" + }, + "dockable": { + "repr": "True", + "type": "repr" + }, + "parent": { + "repr": "None", + "type": "repr" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_top__TOP__1": { + "keys": {}, + "groups": { + "dock_widget__": { + "keys": { + "dock_widget_title": { + "repr": "'Process Monitor'", + "type": "repr" + }, + "dockable": { + "repr": "True", + "type": "repr" + }, + "parent": { + "repr": "None", + "type": "repr" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "filter_text": { + "repr": "''", + "type": "repr" + }, + "is_regex": { + "repr": "0", + "type": "repr" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_topic__TopicPlugin__1": { + "keys": {}, + "groups": { + "dock_widget__TopicWidget": { + "keys": { + "dock_widget_title": { + "repr": "'Topic Monitor'", + "type": "repr" + }, + "dockable": { + "repr": "True", + "type": "repr" + }, + "parent": { + "repr": "None", + "type": "repr" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "tree_widget_header_state": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff0000000000000001000000000000000501000000000000000000000006200000000100000005000000c8000006980000000601010001000000000000000006000000c8ffffffff000000810000000300000006000001340000000100000003000001d10000000100000003000000c200000001000000030000005400000001000000030000027d0000000100000003000000000000000100000003000003e800000000c8')", + "type": "repr(QByteArray.hex)", + "pretty-print": " " + } + }, + "groups": {} + } + } + } + } + } + } +} \ No newline at end of file diff --git a/rob_topic_monitor/config/foxglove_bridge.yaml b/rob_topic_monitor/config/foxglove_bridge.yaml new file mode 100644 index 000000000..988d8c2cf --- /dev/null +++ b/rob_topic_monitor/config/foxglove_bridge.yaml @@ -0,0 +1,34 @@ +foxglove_bridge_ccu: + ros__parameters: + address: 0.0.0.0 + asset_uri_allowlist: + - ^package://(?:\w+/)*\w+\..*$ + capabilities: + - clientPublish + - connectionGraph + - services + - assets + certfile: '' + client_topic_whitelist: + - .* + include_hidden: false + keyfile: '' + max_qos_depth: 100 + min_qos_depth: 1 + param_whitelist: + - .* + port: 8765 + qos_overrides: + /parameter_events: + publisher: + depth: 1000 + durability: volatile + history: keep_last + reliability: reliable + send_buffer_limit: 50000000 + service_whitelist: + - .* + tls: false + topic_whitelist: + - .* + use_compression: true diff --git a/rob_topic_monitor/config/topic_check.yaml b/rob_topic_monitor/config/topic_check.yaml new file mode 100644 index 000000000..bd1ae1bb9 --- /dev/null +++ b/rob_topic_monitor/config/topic_check.yaml @@ -0,0 +1,45 @@ +# /diagnostics/topic_frequency_monitor: +# ros__parameters: +# monitor_configured_only: True +# topics: +# - /odom +# - /base_link/odom +# - /rpm/sensors/front/imu/imu/data +# - /rpm/sensors/front/lidar3d/points +# - /rpm/sensors/rear/lidar3d/points +# min_freqs: +# - 49.0 +# - 7.5 +# - 99.0 +# - 9.5 +# - 9.5 +# max_freqs: +# - 51.0 +# - 10.0 +# - 100.0 +# - 10.5 +# - 10.5 + +# /diagnostics/message_age_monitor: +# ros__parameters: +# monitor_configured_only: True +# topics: +# - /odom +# - /base_link/odom +# - /rpm/sensors/front/imu/imu/data +# - /rpm/sensors/front/lidar3d/points +# - /rpm/sensors/rear/lidar3d/points +# min_delays: +# - -0.09 # can occur due to simulated time +# - -0.09 +# - -0.09 +# - 0.0 +# - 0.0 +# max_delays: +# - 0.1 +# - 0.1 +# - 0.1 +# - 0.200 +# - 0.200 + + diff --git a/rob_topic_monitor/include/update_functions.hpp b/rob_topic_monitor/include/update_functions.hpp new file mode 100644 index 000000000..c7ce00466 --- /dev/null +++ b/rob_topic_monitor/include/update_functions.hpp @@ -0,0 +1,307 @@ +// Copyright (c) 2009, Willow Garage, Inc. + +/* Software License Agreement (BSD License) + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + + +#ifndef UPDATE_FUNCTIONS_HPP_ +#define UPDATE_FUNCTIONS_HPP_ + +#include +#include +#include +#include + +#include "diagnostic_updater/diagnostic_updater.hpp" + +// WORKAROUND until FrequencyStatus with sim_time support is available (humble+) +namespace topic_monitor +{ + +using diagnostic_updater::DiagnosticTask; + +struct FrequencyStatusParam +{ + FrequencyStatusParam( + double * min_freq, double * max_freq, + double tolerance = 0.1, int window_size = 5) + : min_freq_(min_freq), max_freq_(max_freq), tolerance_(tolerance), + window_size_(window_size) {} + + double * min_freq_; + + double * max_freq_; + + double tolerance_; + + int window_size_; +}; + +class FrequencyStatus : public DiagnosticTask +{ +private: + const FrequencyStatusParam params_; + + int count_; + std::vector times_; + std::vector seq_nums_; + int hist_indx_; + std::mutex lock_; + rclcpp::Logger debug_logger_; + const rclcpp::Clock::SharedPtr clock_ptr_; + +public: + FrequencyStatus( + const FrequencyStatusParam & params, + std::string name, + const rclcpp::Clock::SharedPtr & clock = std::make_shared()) + : DiagnosticTask(name), params_(params), times_(params_.window_size_), + seq_nums_(params_.window_size_), + debug_logger_(rclcpp::get_logger("FrequencyStatus_debug_logger")), + clock_ptr_(clock) + { + clear(); + } + + explicit FrequencyStatus( + const FrequencyStatusParam & params, + const rclcpp::Clock::SharedPtr & clock = std::make_shared()) + : FrequencyStatus(params, "Frequency Status", clock) + {} + + void clear() + { + std::unique_lock lock(lock_); + rclcpp::Time curtime = clock_ptr_->now(); + count_ = 0; + + for (int i = 0; i < params_.window_size_; i++) { + times_[i] = curtime; + seq_nums_[i] = count_; + } + + hist_indx_ = 0; + } + + void tick() + { + std::unique_lock lock(lock_); + RCLCPP_DEBUG(debug_logger_, "TICK %i", count_); + count_++; + } + + virtual void run(diagnostic_updater::DiagnosticStatusWrapper & stat) + { + std::unique_lock lock(lock_); + rclcpp::Time curtime = clock_ptr_->now(); + + int curseq = count_; + int events = curseq - seq_nums_[hist_indx_]; + double window = curtime.seconds() - times_[hist_indx_].seconds(); + double freq = events / window; + seq_nums_[hist_indx_] = curseq; + times_[hist_indx_] = curtime; + hist_indx_ = (hist_indx_ + 1) % params_.window_size_; + + if (events == 0) { + stat.summary(2, "No events recorded."); + } else if (freq < *params_.min_freq_ * (1 - params_.tolerance_)) { + stat.summary(1, "Frequency too low."); + } else if (freq > *params_.max_freq_ * (1 + params_.tolerance_)) { + stat.summary(1, "Frequency too high."); + } else { + stat.summary(0, "Desired frequency met"); + } + + stat.addf("Events in window", "%d", events); + stat.addf("Events since startup", "%d", count_); + stat.addf("Duration of window (s)", "%f", window); + stat.addf("Actual frequency (Hz)", "%f", freq); + if (*params_.min_freq_ == *params_.max_freq_) { + stat.addf("Target frequency (Hz)", "%f", *params_.min_freq_); + } + if (*params_.min_freq_ > 0) { + stat.addf( + "Minimum acceptable frequency (Hz)", "%f", + *params_.min_freq_ * (1 - params_.tolerance_)); + } + if (std::isfinite(*params_.max_freq_)) { + stat.addf( + "Maximum acceptable frequency (Hz)", "%f", + *params_.max_freq_ * (1 + params_.tolerance_)); + } + } +}; + +struct TimeStampStatusParam +{ + TimeStampStatusParam( + const double min_acceptable = -1, + const double max_acceptable = 5) + : max_acceptable_(max_acceptable), min_acceptable_(min_acceptable) {} + + double max_acceptable_; + + double min_acceptable_; +}; + +static TimeStampStatusParam DefaultTimeStampStatusParam = + TimeStampStatusParam(); + +class TimeStampStatus : public DiagnosticTask +{ +private: + void init() + { + early_count_ = 0; + late_count_ = 0; + zero_count_ = 0; + zero_seen_ = false; + max_delta_ = 0; + min_delta_ = 0; + deltas_valid_ = false; + } + +public: + TimeStampStatus( + const TimeStampStatusParam & params, + std::string name, + const rclcpp::Clock::SharedPtr & clock = std::make_shared()) + : DiagnosticTask(name), params_(params), clock_ptr_(clock) + { + init(); + } + + explicit TimeStampStatus( + const TimeStampStatusParam & params, + const rclcpp::Clock::SharedPtr & clock = std::make_shared()) + : DiagnosticTask("Timestamp Status"), params_(params), clock_ptr_(clock) + { + init(); + } + + explicit TimeStampStatus( + const rclcpp::Clock::SharedPtr & clock = std::make_shared()) + : DiagnosticTask("Timestamp Status"), clock_ptr_(clock) {init();} + + void tick(double stamp) + { + std::unique_lock lock(lock_); + + if (stamp == 0) { + zero_seen_ = true; + } else { + const double delta = clock_ptr_->now().seconds() - stamp; + + if (!deltas_valid_ || delta > max_delta_) { + max_delta_ = delta; + } + + if (!deltas_valid_ || delta < min_delta_) { + min_delta_ = delta; + } + + deltas_valid_ = true; + } + } + + void tick(const rclcpp::Time t) {tick(t.seconds());} + + virtual void run(diagnostic_updater::DiagnosticStatusWrapper & stat) + { + std::unique_lock lock(lock_); + + stat.summary(0, "Timestamps are reasonable."); + if (!deltas_valid_) { + stat.summary(1, "No data since last update."); + } else { + if (min_delta_ < params_.min_acceptable_) { + stat.summary(2, "Timestamps too far in future seen."); + early_count_++; + } + + if (max_delta_ > params_.max_acceptable_) { + stat.summary(2, "Timestamps too far in past seen."); + late_count_++; + } + + if (zero_seen_) { + stat.summary(2, "Zero timestamp seen."); + zero_count_++; + } + } + + stat.addf("Earliest timestamp delay:", "%f", min_delta_); + stat.addf("Latest timestamp delay:", "%f", max_delta_); + stat.addf( + "Earliest acceptable timestamp delay:", "%f", + params_.min_acceptable_); + stat.addf( + "Latest acceptable timestamp delay:", "%f", + params_.max_acceptable_); + stat.add("Late diagnostic update count:", late_count_); + stat.add("Early diagnostic update count:", early_count_); + stat.add("Zero seen diagnostic update count:", zero_count_); + + deltas_valid_ = false; + min_delta_ = 0; + max_delta_ = 0; + zero_seen_ = false; + } + +private: + TimeStampStatusParam params_; + int early_count_; + int late_count_; + int zero_count_; + bool zero_seen_; + double max_delta_; + double min_delta_; + bool deltas_valid_; + const rclcpp::Clock::SharedPtr clock_ptr_; + std::mutex lock_; +}; + +class Heartbeat : public DiagnosticTask +{ +public: + Heartbeat() + : DiagnosticTask("Heartbeat") {} + + virtual void run(diagnostic_updater::DiagnosticStatusWrapper & stat) + { + stat.summary(0, "Alive"); + } +}; +} // namespace topic_monitor + +#endif // UPDATE_FUNCTIONS_HPP_ diff --git a/rob_topic_monitor/launch/topic_monitor_launch.py b/rob_topic_monitor/launch/topic_monitor_launch.py new file mode 100644 index 000000000..0b0477716 --- /dev/null +++ b/rob_topic_monitor/launch/topic_monitor_launch.py @@ -0,0 +1,76 @@ +# Copyright 2024 Robert Bosch GmbH and its subsidiaries +# +# All rights reserved, also regarding any disposal, exploitation, reproduction, +# editing, distribution, as well as in the event of applications for industrial +# property rights. + +from launch import LaunchDescription +from launch.actions import GroupAction +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration +from launch_ros.actions import Node, PushRosNamespace +from launch_ros.substitutions import FindPackageShare + +DIAG_NAMESPACE = "/diagnostics" +FREQ_MONITOR_NAME = "topic_frequency_monitor" +AGE_MONITOR_NAME = "message_age_monitor" + + +def generate_launch_description(): + + return LaunchDescription([ + DeclareLaunchArgument( + "use_sim_time", default_value="False", description="Whether to use sim_time" + ), + SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"), + DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ), + GroupAction( + actions=[ + PushRosNamespace(namespace=DIAG_NAMESPACE), + Node( + package="rob_topic_monitor", + executable="topic_monitor", + name=FREQ_MONITOR_NAME, + parameters=[ + {'use_sim_time': LaunchConfiguration('use_sim_time'), + 'monitor_configured_only': True}, + PathJoinSubstitution([ + FindPackageShare("rob_topic_monitor"), + "config", + "topic_check.yaml"]) + ] + ), + Node( + package="rob_topic_monitor", + executable="header_topic_monitor", + name=AGE_MONITOR_NAME, + parameters=[ + {'use_sim_time': LaunchConfiguration('use_sim_time'), + 'monitor_configured_only': False}, + PathJoinSubstitution([ + FindPackageShare("rob_topic_monitor"), + "config", + "topic_check.yaml"]) + ] + ), + Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + namespace="", + name="lifecycle_manager_module_diag", + output="both", + parameters=[ + {'use_sim_time': LaunchConfiguration('use_sim_time'), + "autostart": True, + "node_names": [ + [DIAG_NAMESPACE, "/", FREQ_MONITOR_NAME], + [DIAG_NAMESPACE, "/", AGE_MONITOR_NAME] + ], + 'bond_timeout': 0.0}, + ], + ), + ] + ) + ]) diff --git a/rob_topic_monitor/package.xml b/rob_topic_monitor/package.xml new file mode 100644 index 000000000..7d0385a32 --- /dev/null +++ b/rob_topic_monitor/package.xml @@ -0,0 +1,34 @@ + + + + rob_topic_monitor + 1.0.0 + Generic Topic Monitor + Ingo Lütkebohle + BIOSv4 + + ament_cmake + + diagnostic_msgs + rclpy + rclcpp + rclcpp_lifecycle + rclcpp_components + statistics_msgs + diagnostic_updater + std_msgs + + python3-ntplib + + ament_lint_auto + ament_lint_common + ament_cmake_pytest + launch_testing + examples_rclcpp_minimal_publisher + examples_rclcpp_minimal_action_server + example_interfaces + + + ament_cmake + + diff --git a/rob_topic_monitor/src/header_topic_monitor_component.cpp b/rob_topic_monitor/src/header_topic_monitor_component.cpp new file mode 100644 index 000000000..93bfbc79b --- /dev/null +++ b/rob_topic_monitor/src/header_topic_monitor_component.cpp @@ -0,0 +1,234 @@ +// Copyright 2024 Robert Bosch GmbH and its subsidiaries +// All rights reserved, also regarding any disposal, exploitation, +// reproduction, editing, distribution, as well as in the event of applications +// for industrial property rights. + +#include +#include +#include +#include +#include +#include +#include +#include +#include // NOLINT: upstream +#include // NOLINT: upstream +#include // NOLINT: upstream +#include // NOLINT: upstream +#include // NOLINT: upstream +#include "rclcpp/serialization.hpp" +#include "update_functions.hpp" + +using namespace std::chrono_literals; + +namespace rob_topic_monitor +{ +constexpr const char * TOPICS_PARAM_NAME = "topics"; +constexpr const char * MIN_DELAYS_PARAM_NAME = "min_delays"; +constexpr const char * MAX_DELAYS_PARAM_NAME = "max_delays"; +constexpr const char * MONITOR_CONFIGURED_ONLY_PARAM_NAME = "monitor_configured_only"; +constexpr const char * DIAG_PREFIX_PARAM_NAME = "diag_prefix"; + + +using namespace std::chrono_literals; +class HeaderTopicMonitor : public rclcpp_lifecycle::LifecycleNode +{ +public: + explicit HeaderTopicMonitor(const std::string & node_name, rclcpp::NodeOptions options) + : rclcpp_lifecycle::LifecycleNode(node_name, options.allow_undeclared_parameters(true)) + { + auto desc = rcl_interfaces::msg::ParameterDescriptor{}; + desc.description = "Topics to specify expectations for"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; + declare_parameter(TOPICS_PARAM_NAME, std::vector(), desc); + auto desc_min = rcl_interfaces::msg::ParameterDescriptor{}; + desc_min.description = "Minimum delay for topic from 'topics'"; + desc_min.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY; + declare_parameter(MIN_DELAYS_PARAM_NAME, std::vector(), desc_min); + auto desc_max = rcl_interfaces::msg::ParameterDescriptor{}; + desc_max.description = "Maximum delay for topic from 'topics'"; + desc_max.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY; + declare_parameter(MAX_DELAYS_PARAM_NAME, std::vector(), desc_max); + // by default, we include all topics + declare_parameter(MONITOR_CONFIGURED_ONLY_PARAM_NAME, false); + // prefix for diagnostics (for use with modules) + declare_parameter(DIAG_PREFIX_PARAM_NAME, ""); + } + explicit HeaderTopicMonitor(const rclcpp::NodeOptions & options) + : HeaderTopicMonitor("header_topic_monitor", options) + { + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_configure(const rclcpp_lifecycle::State &) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_activate(const rclcpp_lifecycle::State & state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State & state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_cleanup(const rclcpp_lifecycle::State &) override + { + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_shutdown(const rclcpp_lifecycle::State &) override + { + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + void topic_cb( + const std::string & topic_name, + const std::shared_ptr & msg) + { + std_msgs::msg::Header header; + const auto diag = topic_diag_map_.find(topic_name); + _header_serializer.deserialize_message(msg.get(), &header); + + if (diag != topic_diag_map_.end()) { + diag->second->tick(header.stamp); + } else { + RCLCPP_WARN(get_logger(), "No diagnostic found for topic %s", topic_name.c_str()); + } + } + +protected: + void update_topic_subscriptions(); + bool skip_topic(const std::string & name); + std::string get_prefixed_name(const std::string & topic_name) const; + +private: + std::vector hidden_topics_{std::regex("^/rosout$"), std::regex(".*/parameter_events"), + std::regex("^/diagnostics$"), std::regex(".*/transition_event"), std::regex("^/clock")}; + std::shared_ptr timer_; + std::shared_ptr updater_; + std::unordered_map> subscribed_topics_; + std::set known_topics_; + std::unordered_map> topic_diag_map_; + std::vector topics_; + std::vector min_delays; + std::vector max_delays; + bool monitor_configured_only_{true}; + std::string diag_prefix_; + rclcpp::Serialization _header_serializer; +}; + +std::string HeaderTopicMonitor::get_prefixed_name(const std::string & topic_name) const +{ + if (topic_name.find(diag_prefix_) == 0) { + return topic_name; + } + return diag_prefix_ + std::string("/") + topic_name; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +HeaderTopicMonitor::on_configure(const rclcpp_lifecycle::State &) +{ + RCLCPP_DEBUG(get_logger(), "Configuring"); + // configure the diagnostics + topics_ = get_parameter(TOPICS_PARAM_NAME).as_string_array(); + min_delays = get_parameter(MIN_DELAYS_PARAM_NAME).as_double_array(); + max_delays = get_parameter(MAX_DELAYS_PARAM_NAME).as_double_array(); + if (topics_.size() != min_delays.size() || topics_.size() != max_delays.size()) { + throw std::invalid_argument("Topics and min/max_delays must have same number of arguments"); + } + monitor_configured_only_ = get_parameter(MONITOR_CONFIGURED_ONLY_PARAM_NAME).as_bool(); + diag_prefix_ = get_parameter(DIAG_PREFIX_PARAM_NAME).as_string(); + + RCLCPP_DEBUG( + get_logger(), "Done configuring for %ld topics, config only: %d", topic_diag_map_.size(), + monitor_configured_only_); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +HeaderTopicMonitor::on_activate(const rclcpp_lifecycle::State &) +{ + RCLCPP_DEBUG(get_logger(), "Activating"); + updater_ = std::make_shared(this); + char HOSTNAME[1000]; + gethostname(HOSTNAME, 1000); + updater_->setHardwareID(std::string(HOSTNAME)); + for (size_t i = 0; i < topics_.size(); ++i) { + topic_monitor::TimeStampStatusParam param(min_delays[i], max_delays[i]); + auto diag = std::make_shared( + param, get_prefixed_name(topics_[i]), get_clock()); + topic_diag_map_[topics_[i]] = diag; + updater_->add(*diag); + } + // check existing topics + try { + update_topic_subscriptions(); + } catch (const std::exception & ex) { + RCLCPP_ERROR(this->get_logger(), "Failure to update subscriptions: %s", ex.what()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + timer_ = create_wall_timer(1s, [this]() {this->update_topic_subscriptions();}); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +HeaderTopicMonitor::on_deactivate(const rclcpp_lifecycle::State &) +{ + RCLCPP_DEBUG(get_logger(), "Deactivating"); + timer_.reset(); + subscribed_topics_.clear(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +bool HeaderTopicMonitor::skip_topic(const std::string & topic_name) +{ + // skip if we already subscribed + if (subscribed_topics_.find(topic_name) != subscribed_topics_.end()) { + RCLCPP_DEBUG(get_logger(), "Already subscribed to %s, skipping", topic_name.c_str()); + return true; + } + // if it's configured, we monitor it + if (std::find(topics_.begin(), topics_.end(), topic_name) != topics_.end()) { + RCLCPP_DEBUG(get_logger(), "Topic %s is configured, no skip", topic_name.c_str()); + return false; + } + // skip if we have seen this before and ignored it + if (known_topics_.find(topic_name) != known_topics_.end()) { + return true; + } + // check for matches against internal topic names + for (const auto & re : hidden_topics_) { + if (std::regex_match(topic_name, re)) { + known_topics_.insert(topic_name); + RCLCPP_DEBUG(get_logger(), "Topic %s matches ignore list, skip it", topic_name.c_str()); + return true; + } + } + return monitor_configured_only_; +} + +void HeaderTopicMonitor::update_topic_subscriptions() +{ + RCLCPP_DEBUG( + get_logger(), "Examining topic list for changes, currently monitoring %ld topics", + subscribed_topics_.size()); + const auto topics = this->get_topic_names_and_types(); + for (const auto & e : topics) { + const std::string topic_name = e.first; + if (skip_topic(topic_name)) { + continue; + } + // subscribe with statistics enabled + RCLCPP_DEBUG(get_logger(), "Starting to monitor topic %s", topic_name.c_str()); + std::shared_ptr sub = this->create_generic_subscription( + e.first, *e.second.begin(), rclcpp::QoS(10), + [this, topic_name](std::shared_ptr msg) { + this->topic_cb(topic_name, msg); + }); + subscribed_topics_[topic_name] = sub; + } + RCLCPP_DEBUG( + get_logger(), "Done updating topic list. Found %ld, hidden %ld, now monitoring %ld", + topics.size(), + known_topics_.size(), subscribed_topics_.size()); +} +} // namespace rob_topic_monitor + +#include // NOLINT: upstream +RCLCPP_COMPONENTS_REGISTER_NODE(rob_topic_monitor::HeaderTopicMonitor) diff --git a/rob_topic_monitor/src/topic_monitor_component.cpp b/rob_topic_monitor/src/topic_monitor_component.cpp new file mode 100644 index 000000000..652a72a65 --- /dev/null +++ b/rob_topic_monitor/src/topic_monitor_component.cpp @@ -0,0 +1,283 @@ +// Copyright 2024 Robert Bosch GmbH and its subsidiaries +// All rights reserved, also regarding any disposal, exploitation, +// reproduction, editing, distribution, as well as in the event of applications +// for industrial property rights. + +#include +#include +#include +#include +#include +#include +#include +#include +#include // NOLINT: upstream +#include // NOLINT: upstream +#include // NOLINT: upstream +#include // NOLINT: upstream +#include // NOLINT: upstream +#include "update_functions.hpp" + +using namespace std::chrono_literals; + +namespace +{ +// very simply DiagnosticTask that just checks whether there has been _any_ +// kind of activity during the last period +class ActivityDiagnosticTask : public diagnostic_updater::DiagnosticTask +{ +public: + ActivityDiagnosticTask(const std::string & name, rclcpp::Clock::SharedPtr clock) + : diagnostic_updater::DiagnosticTask(name), clock_(clock), last_tick_(0) + { + } + virtual void tick() + { + last_tick_ = clock_->now(); + } + void run(diagnostic_updater::DiagnosticStatusWrapper & stat) override + { + stat.name = getName(); + if (last_tick_.seconds() == 0) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "No data received, yet"; + stat.addf("period", "%f", std::numeric_limits::max()); + } else { + const auto currentTime = clock_->now(); + // more than 1s without tick is considered warning + const auto elapsed = currentTime - last_tick_; + stat.addf("period", "%f", elapsed.seconds()); + if (elapsed <= rclcpp::Duration(1.1s)) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + } else { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "Last data received more than 1s ago"; + } + } + } + +private: + rclcpp::Clock::SharedPtr clock_; + rclcpp::Time last_tick_; +}; +} // namespace + +namespace rob_topic_monitor +{ +constexpr const char * TOPICS_PARAM_NAME = "topics"; +constexpr const char * MIN_FREQS_PARAM_NAME = "min_freqs"; +constexpr const char * MAX_FREQS_PARAM_NAME = "max_freqs"; +constexpr const char * MONITOR_CONFIGURED_ONLY_PARAM_NAME = "monitor_configured_only"; +constexpr const char * DIAG_PREFIX_PARAM_NAME = "diag_prefix"; + +using namespace std::chrono_literals; +class TopicMonitor : public rclcpp_lifecycle::LifecycleNode +{ +public: + explicit TopicMonitor(const std::string & node_name, rclcpp::NodeOptions options) + : rclcpp_lifecycle::LifecycleNode(node_name, options.allow_undeclared_parameters(true)) + { + auto desc = rcl_interfaces::msg::ParameterDescriptor{}; + desc.description = "Topics to specify expectations for"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; + declare_parameter(TOPICS_PARAM_NAME, std::vector(), desc); + auto desc_min = rcl_interfaces::msg::ParameterDescriptor{}; + desc_min.description = "Minimum frequency for topic from 'topics'"; + desc_min.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY; + declare_parameter(MIN_FREQS_PARAM_NAME, std::vector(), desc_min); + auto desc_max = rcl_interfaces::msg::ParameterDescriptor{}; + desc_max.description = "Maximum frequency for topic from 'topics'"; + desc_max.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY; + declare_parameter(MAX_FREQS_PARAM_NAME, std::vector(), desc_max); + // by default, we include all topics + declare_parameter(MONITOR_CONFIGURED_ONLY_PARAM_NAME, false); + // prefix for diagnostics (for use with modules) + declare_parameter(DIAG_PREFIX_PARAM_NAME, ""); + } + explicit TopicMonitor(const rclcpp::NodeOptions & options) + : TopicMonitor("topic_monitor", options) + { + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_configure(const rclcpp_lifecycle::State &) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_activate(const rclcpp_lifecycle::State & state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State & state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_cleanup(const rclcpp_lifecycle::State &) override + { + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_shutdown(const rclcpp_lifecycle::State &) override + { + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + void topic_cb(const std::string & topic_name, const std::shared_ptr &) + { + const auto diag = topic_diag_map_.find(topic_name); + if (diag != topic_diag_map_.end()) { + diag->second->tick(); + } else { + const auto fallback_diag = fallback_topic_diag_map_.find(topic_name); + if (fallback_diag != fallback_topic_diag_map_.end()) { + fallback_diag->second->tick(); + } else { + RCLCPP_WARN(get_logger(), "No diagnostic found for topic %s", topic_name.c_str()); + } + } + } + +protected: + void update_topic_subscriptions(); + bool skip_topic(const std::string & name); + std::string get_prefixed_name(const std::string & topic_name) const; + +private: + std::vector hidden_topics_{std::regex("^/rosout$"), std::regex(".*/parameter_events"), + std::regex("^/diagnostics$"), std::regex(".*/transition_event"), std::regex("^/clock")}; + std::shared_ptr timer_; + std::shared_ptr updater_; + std::unordered_map> subscribed_topics_; + std::set known_topics_; + std::unordered_map> topic_diag_map_; + std::unordered_map> fallback_topic_diag_map_; + std::vector topics_; + std::vector min_freqs; + std::vector max_freqs; + bool monitor_configured_only_{true}; + std::string diag_prefix_; +}; + +std::string TopicMonitor::get_prefixed_name(const std::string & topic_name) const +{ + if (topic_name.find(diag_prefix_) == 0) { + return topic_name; + } + return diag_prefix_ + std::string("/") + topic_name; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TopicMonitor::on_configure(const rclcpp_lifecycle::State &) +{ + RCLCPP_DEBUG(get_logger(), "Configuring"); + // configure the diagnostics + topics_ = get_parameter(TOPICS_PARAM_NAME).as_string_array(); + min_freqs = get_parameter(MIN_FREQS_PARAM_NAME).as_double_array(); + max_freqs = get_parameter(MAX_FREQS_PARAM_NAME).as_double_array(); + if (topics_.size() != min_freqs.size() || topics_.size() != max_freqs.size()) { + throw std::invalid_argument("Topics and min/max_freqs must have same number of arguments"); + } + monitor_configured_only_ = get_parameter(MONITOR_CONFIGURED_ONLY_PARAM_NAME).as_bool(); + diag_prefix_ = get_parameter(DIAG_PREFIX_PARAM_NAME).as_string(); + + RCLCPP_DEBUG( + get_logger(), "Done configuring for %ld topics, config only: %d", topic_diag_map_.size(), + monitor_configured_only_); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TopicMonitor::on_activate(const rclcpp_lifecycle::State &) +{ + RCLCPP_DEBUG(get_logger(), "Activating"); + updater_ = std::make_shared(this); + char HOSTNAME[1000]; + gethostname(HOSTNAME, 1000); + updater_->setHardwareID(std::string(HOSTNAME)); + for (size_t i = 0; i < topics_.size(); ++i) { + topic_monitor::FrequencyStatusParam param(&(min_freqs[i]), &(max_freqs[i])); + auto diag = std::make_shared( + param, get_prefixed_name(topics_[i]), get_clock()); + topic_diag_map_[topics_[i]] = diag; + updater_->add(*diag); + } + // check existing topics + try { + update_topic_subscriptions(); + } catch (const std::exception & ex) { + RCLCPP_ERROR(this->get_logger(), "Failure to update subscriptions: %s", ex.what()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + timer_ = create_wall_timer(1s, [this]() {this->update_topic_subscriptions();}); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TopicMonitor::on_deactivate(const rclcpp_lifecycle::State &) +{ + RCLCPP_DEBUG(get_logger(), "Deactivating"); + timer_.reset(); + subscribed_topics_.clear(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +bool TopicMonitor::skip_topic(const std::string & topic_name) +{ + // skip if we already subscribed + if (subscribed_topics_.find(topic_name) != subscribed_topics_.end()) { + RCLCPP_DEBUG(get_logger(), "Already subscribed to %s, skipping", topic_name.c_str()); + return true; + } + // if it's configured, we monitor it + if (std::find(topics_.begin(), topics_.end(), topic_name) != topics_.end()) { + RCLCPP_DEBUG(get_logger(), "Topic %s is configured, no skip", topic_name.c_str()); + return false; + } + // skip if we have seen this before and ignored it + if (known_topics_.find(topic_name) != known_topics_.end()) { + return true; + } + // check for matches against internal topic names + for (const auto & re : hidden_topics_) { + if (std::regex_match(topic_name, re)) { + known_topics_.insert(topic_name); + RCLCPP_DEBUG(get_logger(), "Topic %s matches ignore list, skip it", topic_name.c_str()); + return true; + } + } + return monitor_configured_only_; +} + +void TopicMonitor::update_topic_subscriptions() +{ + RCLCPP_DEBUG( + get_logger(), "Examining topic list for changes, currently monitoring %ld topics", + subscribed_topics_.size()); + const auto topics = this->get_topic_names_and_types(); + for (const auto & e : topics) { + const std::string topic_name = e.first; + if (skip_topic(topic_name)) { + continue; + } + // subscribe with statistics enabled + RCLCPP_DEBUG(get_logger(), "Starting to monitor topic %s", topic_name.c_str()); + std::shared_ptr sub = this->create_generic_subscription( + e.first, *e.second.begin(), rclcpp::QoS(10), + [this, topic_name](std::shared_ptr msg) { + this->topic_cb(topic_name, msg); + }); + subscribed_topics_[topic_name] = sub; + // add a default diagnostic if none is configured + if (topic_diag_map_.find(topic_name) == topic_diag_map_.end()) { + auto fallback_diag = std::make_shared( + get_prefixed_name( + topic_name), get_clock()); + fallback_topic_diag_map_[topic_name] = fallback_diag; + updater_->add(*fallback_diag); + } + } + RCLCPP_DEBUG( + get_logger(), "Done updating topic list. Found %ld, hidden %ld, now monitoring %ld", + topics.size(), + known_topics_.size(), subscribed_topics_.size()); +} +} // namespace rob_topic_monitor + +#include // NOLINT: upstream +RCLCPP_COMPONENTS_REGISTER_NODE(rob_topic_monitor::TopicMonitor) diff --git a/rob_topic_monitor/test/test_topic_monitor.py b/rob_topic_monitor/test/test_topic_monitor.py new file mode 100755 index 000000000..24ccbea0c --- /dev/null +++ b/rob_topic_monitor/test/test_topic_monitor.py @@ -0,0 +1,241 @@ +#! /usr/bin/env python3 + +# Copyright 2024 Robert Bosch GmbH and its subsidiaries +# +# All rights reserved, also regarding any disposal, exploitation, reproduction, +# editing, distribution, as well as in the event of applications for industrial +# property rights. + +# # +# # Copyright 2019 Open Source Robotics Foundation, Inc. +# # +# # Licensed under the Apache License, Version 2.0 (the "License"); +# # you may not use this file except in compliance with the License. +# # You may obtain a copy of the License at +# # +# # http://www.apache.org/licenses/LICENSE-2.0 +# # +# # Unless required by applicable law or agreed to in writing, software +# # distributed under the License is distributed on an "AS IS" BASIS, +# # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# # See the License for the specific language governing permissions and +# # limitations under the License. + +import time +import unittest +import pytest + +import launch +from launch.actions import EmitEvent, RegisterEventHandler, LogInfo, SetEnvironmentVariable +import launch.event_handlers.on_process_start + +from launch_ros.actions import LifecycleNode, Node +from launch_ros.events.lifecycle import ChangeState +import launch_ros.events.lifecycle + +import launch_testing +import launch_testing.actions +import launch_testing.asserts + +from lifecycle_msgs.msg import Transition +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus + +import rclpy +from rclpy.time import Time, Duration + + +def create_change_state(target, target_state): + return EmitEvent( + event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(target), + transition_id=target_state, + ) + ) + + +def create_register_configure(target_action): + return RegisterEventHandler( + launch.event_handlers.on_process_start.OnProcessStart( + target_action=target_action, + on_start=[ + LogInfo(msg=f"Emitting configure event for {target_action}"), + create_change_state( + target_action, Transition.TRANSITION_CONFIGURE + ) + ], + ) + ) + + +def create_register_activate(target_action): + return RegisterEventHandler( + launch_ros.event_handlers.OnStateTransition( + target_lifecycle_node=target_action, + start_state="configuring", + goal_state="inactive", + entities=[ + LogInfo(msg=f"Emitting activate event for {target_action}"), + create_change_state( + target_action, Transition.TRANSITION_ACTIVATE + ) + ], + ) + ) + + +ALL_MONITOR_NAME = "all_monitor" +FREQ_MONITOR_NAME = "frequency_monitor" + + +@pytest.mark.launch_test +def generate_test_description(): + monitor_node = LifecycleNode( + package="rob_topic_monitor", + executable="topic_monitor", + name=ALL_MONITOR_NAME, + output="both", + namespace="", + arguments=['--ros-args', '--log-level', 'all_monitor:=INFO'] + ) + frequency_monitor_node = LifecycleNode( + package="rob_topic_monitor", + executable="topic_monitor", + name=FREQ_MONITOR_NAME, + output="both", + namespace="", + parameters=[{"topics": ["/topic"], + 'min_freqs': [1.8], + 'max_freqs': [2.1], + 'diag_prefix': 'freq', + 'monitor_configured_only': True, + 'diagnostic_updater.use_fqn': True}], + arguments=['--ros-args', '--log-level', 'frequency_monitor:=INFO'] + ) + talker_node = Node( + package="examples_rclcpp_minimal_publisher", + executable="publisher_lambda", + output="log", + name="talker", + arguments=['--ros-args', '--log-level', 'talker:=warn'] + ) + talker2_node = Node( + package="examples_rclcpp_minimal_publisher", + executable="publisher_lambda", + name="talker2", + output="log", + remappings=[("topic", "ignore_topic")], + arguments=['--ros-args', '--log-level', 'talker2:=warn'] + ) + return launch.LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + talker_node, + talker2_node, + monitor_node, + frequency_monitor_node, + # Right after the monitor starts, make it take the 'configure' transition. + create_register_configure(monitor_node), + create_register_configure(frequency_monitor_node), + # When the monitor reaches the 'inactive' state, 'activate'. + create_register_activate(monitor_node), + create_register_activate(frequency_monitor_node), + # When the monitor node reaches the 'active' state, we're ready for testing + RegisterEventHandler( + launch_ros.event_handlers.OnStateTransition( + target_lifecycle_node=monitor_node, + start_state="activating", + goal_state="active", + entities=[ + LogInfo(msg="Monitor reached active state"), + launch_testing.actions.ReadyToTest(), + ], + ) + ), + ] + ) + + +@pytest.mark.skip(reason="Brittle on CI as long as we don't have proper resource constraints") +class TestMonitor(unittest.TestCase): + TIMEOUT = 30 + + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node("test_topic_monitor_node") + self.log = self.node.get_logger() + self.sub = self.node.create_subscription( + DiagnosticArray, "/diagnostics", self.stat_cb, 10 + ) + self.pub_count = self.node.count_publishers("/diagnostics") + self.log.info(f"Number of publishers for /diagnostics: {self.pub_count}") + self.messages = [] + self.freq_messages = [] + + start_time = time.time() + while len(self.messages) < 3 or len(self.freq_messages) < 3: + rclpy.spin_once(self.node, timeout_sec=1.0) + self.log.debug(f"Got {len(self.messages)} and {len(self.freq_messages)}") + if (time.time() - start_time) > self.TIMEOUT: + self.fail("Timed out waiting for message in /diagnostics topic") + self.log.debug(f"Got {len(self.messages)} and {len(self.freq_messages)}") + + def tearDown(self): + self.node.destroy_node() + del self.node + + def stat_cb(self, msg): + """Store message for future processing.""" + if len(msg.status) == 0: + return + if FREQ_MONITOR_NAME in msg.status[0].name: + self.freq_messages.append(msg) + else: + self.messages.append(msg) + + def test_diag_msg(self): + """Check that diagnostics messages contain the right content.""" + last_msg = self.messages.pop() + # header + current_time = self.node.get_clock().now() + header_time = Time.from_msg(last_msg.header.stamp) + self.assertLess(current_time - header_time, Duration(seconds=0.1)) + last_status = last_msg.status[0] + # status should be OK + self.assertEqual(last_status.level, DiagnosticStatus.OK) + # period estimate should be 4 Hz + keys = [value.key for value in last_status.values] + self.assertTrue("period" in keys) + # disabled until we can get deterministic measurement on CI + # self.assertAlmostEqual( + # float(last_status.values[keys.index("period")].value), 0.25, delta=0.02) + # check that the two topics of this test are present + # (a bit more complex, since on CI other topics may be running in parallel) + names = [status.name for status in last_msg.status] + self.assertIn(f"{ALL_MONITOR_NAME}: /topic", names, f"{names}") + self.assertIn(f"{ALL_MONITOR_NAME}: /ignore_topic", names, f"{names}") + + def test_frequency_diag_msg(self): + """Check that the frequency diagnostic works.""" + last_msg = self.freq_messages.pop() + self.log.debug(f"{last_msg}") + self.assertTrue(len(last_msg.status) > 0) + status = last_msg.status[0] + # check some fields for present/content + self.assertTrue(FREQ_MONITOR_NAME in status.name) + keys = [kv.key for kv in status.values] + self.assertIn("Actual frequency (Hz)", keys) + # disabled until we can get deterministic measurement on CI + # freq = [kv.value for kv in status.values if kv.key == "Actual frequency (Hz)"][0] + # self.assertAlmostEqual(float(freq), 2.0, delta=0.02) + + def test_ignore_unconfigured(self): + """Check that we ignore the topic we don't monitor.""" + last_msg = self.freq_messages.pop() + self.assertEqual(len(last_msg.status), 1) From f93154615b1ee1b1cff1f5c02bfb4c0b498ac1f1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ingo=20L=C3=BCtkebohle=20=28CR/ASD1=29?= Date: Wed, 2 Apr 2025 14:28:24 +0200 Subject: [PATCH 2/6] Rename to diagnostic_topic_monitor --- {rob_topic_monitor => diagnostic_topic_monitor}/.gitignore | 0 .../CMakeLists.txt | 2 +- {rob_topic_monitor => diagnostic_topic_monitor}/README.md | 0 .../config/diagnostics.perspective | 0 .../config/foxglove_bridge.yaml | 0 .../config/topic_check.yaml | 0 .../include/update_functions.hpp | 0 .../launch/topic_monitor_launch.py | 0 {rob_topic_monitor => diagnostic_topic_monitor}/package.xml | 2 +- .../src/header_topic_monitor_component.cpp | 6 +++--- .../src/topic_monitor_component.cpp | 6 +++--- .../test/test_topic_monitor.py | 0 12 files changed, 8 insertions(+), 8 deletions(-) rename {rob_topic_monitor => diagnostic_topic_monitor}/.gitignore (100%) rename {rob_topic_monitor => diagnostic_topic_monitor}/CMakeLists.txt (98%) rename {rob_topic_monitor => diagnostic_topic_monitor}/README.md (100%) rename {rob_topic_monitor => diagnostic_topic_monitor}/config/diagnostics.perspective (100%) rename {rob_topic_monitor => diagnostic_topic_monitor}/config/foxglove_bridge.yaml (100%) rename {rob_topic_monitor => diagnostic_topic_monitor}/config/topic_check.yaml (100%) rename {rob_topic_monitor => diagnostic_topic_monitor}/include/update_functions.hpp (100%) rename {rob_topic_monitor => diagnostic_topic_monitor}/launch/topic_monitor_launch.py (100%) rename {rob_topic_monitor => diagnostic_topic_monitor}/package.xml (96%) rename {rob_topic_monitor => diagnostic_topic_monitor}/src/header_topic_monitor_component.cpp (98%) rename {rob_topic_monitor => diagnostic_topic_monitor}/src/topic_monitor_component.cpp (98%) rename {rob_topic_monitor => diagnostic_topic_monitor}/test/test_topic_monitor.py (100%) diff --git a/rob_topic_monitor/.gitignore b/diagnostic_topic_monitor/.gitignore similarity index 100% rename from rob_topic_monitor/.gitignore rename to diagnostic_topic_monitor/.gitignore diff --git a/rob_topic_monitor/CMakeLists.txt b/diagnostic_topic_monitor/CMakeLists.txt similarity index 98% rename from rob_topic_monitor/CMakeLists.txt rename to diagnostic_topic_monitor/CMakeLists.txt index 32da41042..dbee9335d 100644 --- a/rob_topic_monitor/CMakeLists.txt +++ b/diagnostic_topic_monitor/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(rob_topic_monitor) +project(diagnostic_topic_monitor) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/rob_topic_monitor/README.md b/diagnostic_topic_monitor/README.md similarity index 100% rename from rob_topic_monitor/README.md rename to diagnostic_topic_monitor/README.md diff --git a/rob_topic_monitor/config/diagnostics.perspective b/diagnostic_topic_monitor/config/diagnostics.perspective similarity index 100% rename from rob_topic_monitor/config/diagnostics.perspective rename to diagnostic_topic_monitor/config/diagnostics.perspective diff --git a/rob_topic_monitor/config/foxglove_bridge.yaml b/diagnostic_topic_monitor/config/foxglove_bridge.yaml similarity index 100% rename from rob_topic_monitor/config/foxglove_bridge.yaml rename to diagnostic_topic_monitor/config/foxglove_bridge.yaml diff --git a/rob_topic_monitor/config/topic_check.yaml b/diagnostic_topic_monitor/config/topic_check.yaml similarity index 100% rename from rob_topic_monitor/config/topic_check.yaml rename to diagnostic_topic_monitor/config/topic_check.yaml diff --git a/rob_topic_monitor/include/update_functions.hpp b/diagnostic_topic_monitor/include/update_functions.hpp similarity index 100% rename from rob_topic_monitor/include/update_functions.hpp rename to diagnostic_topic_monitor/include/update_functions.hpp diff --git a/rob_topic_monitor/launch/topic_monitor_launch.py b/diagnostic_topic_monitor/launch/topic_monitor_launch.py similarity index 100% rename from rob_topic_monitor/launch/topic_monitor_launch.py rename to diagnostic_topic_monitor/launch/topic_monitor_launch.py diff --git a/rob_topic_monitor/package.xml b/diagnostic_topic_monitor/package.xml similarity index 96% rename from rob_topic_monitor/package.xml rename to diagnostic_topic_monitor/package.xml index 7d0385a32..a68b62df1 100644 --- a/rob_topic_monitor/package.xml +++ b/diagnostic_topic_monitor/package.xml @@ -1,7 +1,7 @@ - rob_topic_monitor + diagnostic_topic_monitor 1.0.0 Generic Topic Monitor Ingo Lütkebohle diff --git a/rob_topic_monitor/src/header_topic_monitor_component.cpp b/diagnostic_topic_monitor/src/header_topic_monitor_component.cpp similarity index 98% rename from rob_topic_monitor/src/header_topic_monitor_component.cpp rename to diagnostic_topic_monitor/src/header_topic_monitor_component.cpp index 93bfbc79b..f9490793f 100644 --- a/rob_topic_monitor/src/header_topic_monitor_component.cpp +++ b/diagnostic_topic_monitor/src/header_topic_monitor_component.cpp @@ -21,7 +21,7 @@ using namespace std::chrono_literals; -namespace rob_topic_monitor +namespace diagnostic_topic_monitor { constexpr const char * TOPICS_PARAM_NAME = "topics"; constexpr const char * MIN_DELAYS_PARAM_NAME = "min_delays"; @@ -228,7 +228,7 @@ void HeaderTopicMonitor::update_topic_subscriptions() topics.size(), known_topics_.size(), subscribed_topics_.size()); } -} // namespace rob_topic_monitor +} // namespace diagnostic_topic_monitor #include // NOLINT: upstream -RCLCPP_COMPONENTS_REGISTER_NODE(rob_topic_monitor::HeaderTopicMonitor) +RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_topic_monitor::HeaderTopicMonitor) diff --git a/rob_topic_monitor/src/topic_monitor_component.cpp b/diagnostic_topic_monitor/src/topic_monitor_component.cpp similarity index 98% rename from rob_topic_monitor/src/topic_monitor_component.cpp rename to diagnostic_topic_monitor/src/topic_monitor_component.cpp index 652a72a65..c5cbb5725 100644 --- a/rob_topic_monitor/src/topic_monitor_component.cpp +++ b/diagnostic_topic_monitor/src/topic_monitor_component.cpp @@ -63,7 +63,7 @@ class ActivityDiagnosticTask : public diagnostic_updater::DiagnosticTask }; } // namespace -namespace rob_topic_monitor +namespace diagnostic_topic_monitor { constexpr const char * TOPICS_PARAM_NAME = "topics"; constexpr const char * MIN_FREQS_PARAM_NAME = "min_freqs"; @@ -277,7 +277,7 @@ void TopicMonitor::update_topic_subscriptions() topics.size(), known_topics_.size(), subscribed_topics_.size()); } -} // namespace rob_topic_monitor +} // namespace diagnostic_topic_monitor #include // NOLINT: upstream -RCLCPP_COMPONENTS_REGISTER_NODE(rob_topic_monitor::TopicMonitor) +RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_topic_monitor::TopicMonitor) diff --git a/rob_topic_monitor/test/test_topic_monitor.py b/diagnostic_topic_monitor/test/test_topic_monitor.py similarity index 100% rename from rob_topic_monitor/test/test_topic_monitor.py rename to diagnostic_topic_monitor/test/test_topic_monitor.py From b6ead6210bbee9f69a1d1ba6928f54743acb1dae Mon Sep 17 00:00:00 2001 From: Tejas Kumar Shastha Date: Wed, 14 May 2025 09:36:52 +0200 Subject: [PATCH 3/6] Refactor code and package (#1) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add topic monitor components to diagnostics Co-authored-by: Tejas Kumar Shastha Signed-off-by: Ingo Lütkebohle --- diagnostic_topic_monitor/CMakeLists.txt | 86 +++-- diagnostic_topic_monitor/README.md | 95 ++++-- .../config/diagnostics.perspective | 221 ------------- .../config/foxglove_bridge.yaml | 34 -- .../config/topic_check.yaml | 45 --- .../activity_diagnostic_task.hpp | 60 ++++ .../generic_topic_monitor.hpp | 222 +++++++++++++ .../topic_age_monitor.hpp | 29 ++ .../topic_frquency_monitor.hpp | 30 ++ .../include/update_functions.hpp | 307 ------------------ .../launch/topic_monitor_launch.py | 76 ----- diagnostic_topic_monitor/package.xml | 5 +- .../src/header_topic_monitor_component.cpp | 234 ------------- .../src/topic_age_monitor_component.cpp | 45 +++ .../src/topic_frequency_monitor_component.cpp | 74 +++++ .../src/topic_monitor_component.cpp | 283 ---------------- .../test/config/topic_age_monitor.yaml | 9 + .../test/config/topic_frequency_monitor.yaml | 12 + .../test/dummy_publishers.py | 58 ++++ .../test/test_topic_age_monitor.py | 222 +++++++++++++ ...tor.py => test_topic_frequency_monitor.py} | 113 +++---- 21 files changed, 917 insertions(+), 1343 deletions(-) delete mode 100644 diagnostic_topic_monitor/config/diagnostics.perspective delete mode 100644 diagnostic_topic_monitor/config/foxglove_bridge.yaml delete mode 100644 diagnostic_topic_monitor/config/topic_check.yaml create mode 100644 diagnostic_topic_monitor/include/diagnostic_topic_monitor/activity_diagnostic_task.hpp create mode 100644 diagnostic_topic_monitor/include/diagnostic_topic_monitor/generic_topic_monitor.hpp create mode 100644 diagnostic_topic_monitor/include/diagnostic_topic_monitor/topic_age_monitor.hpp create mode 100644 diagnostic_topic_monitor/include/diagnostic_topic_monitor/topic_frquency_monitor.hpp delete mode 100644 diagnostic_topic_monitor/include/update_functions.hpp delete mode 100644 diagnostic_topic_monitor/launch/topic_monitor_launch.py delete mode 100644 diagnostic_topic_monitor/src/header_topic_monitor_component.cpp create mode 100644 diagnostic_topic_monitor/src/topic_age_monitor_component.cpp create mode 100644 diagnostic_topic_monitor/src/topic_frequency_monitor_component.cpp delete mode 100644 diagnostic_topic_monitor/src/topic_monitor_component.cpp create mode 100644 diagnostic_topic_monitor/test/config/topic_age_monitor.yaml create mode 100644 diagnostic_topic_monitor/test/config/topic_frequency_monitor.yaml create mode 100755 diagnostic_topic_monitor/test/dummy_publishers.py create mode 100755 diagnostic_topic_monitor/test/test_topic_age_monitor.py rename diagnostic_topic_monitor/test/{test_topic_monitor.py => test_topic_frequency_monitor.py} (68%) diff --git a/diagnostic_topic_monitor/CMakeLists.txt b/diagnostic_topic_monitor/CMakeLists.txt index dbee9335d..18b8f33b3 100644 --- a/diagnostic_topic_monitor/CMakeLists.txt +++ b/diagnostic_topic_monitor/CMakeLists.txt @@ -23,13 +23,13 @@ find_package(rclcpp REQUIRED) find_package(diagnostic_updater REQUIRED) find_package(std_msgs REQUIRED) -add_library(topic_monitor_component SHARED src/topic_monitor_component.cpp) -rclcpp_components_register_node(topic_monitor_component - PLUGIN "rob_topic_monitor::TopicMonitor" - EXECUTABLE topic_monitor +add_library(topic_frequency_monitor_component SHARED src/topic_frequency_monitor_component.cpp) +rclcpp_components_register_node(topic_frequency_monitor_component + PLUGIN "diagnostic_topic_monitor::TopicFrequencyMonitor" + EXECUTABLE topic_frequency_monitor ) -target_include_directories(topic_monitor_component PRIVATE include) -ament_target_dependencies(topic_monitor_component +target_include_directories(topic_frequency_monitor_component PRIVATE include) +ament_target_dependencies(topic_frequency_monitor_component "lifecycle_msgs" "rclcpp" "rclcpp_lifecycle" @@ -37,70 +37,64 @@ ament_target_dependencies(topic_monitor_component "diagnostic_msgs" "diagnostic_updater" ) -ament_export_targets(export_topic_monitor_component) -install(TARGETS topic_monitor_component - EXPORT export_topic_monitor_component +ament_export_targets(export_topic_frequency_monitor_component) +install(TARGETS topic_frequency_monitor_component + EXPORT export_topic_frequency_monitor_component ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) -add_library(header_topic_monitor_component SHARED src/header_topic_monitor_component.cpp) -rclcpp_components_register_node(header_topic_monitor_component - PLUGIN "rob_topic_monitor::HeaderTopicMonitor" - EXECUTABLE header_topic_monitor +add_library(topic_age_monitor_component SHARED src/topic_age_monitor_component.cpp) +rclcpp_components_register_node(topic_age_monitor_component + PLUGIN "diagnostic_topic_monitor::TopicAgeMonitor" + EXECUTABLE topic_age_monitor ) -target_include_directories(header_topic_monitor_component PRIVATE include) -ament_target_dependencies(header_topic_monitor_component +target_include_directories(topic_age_monitor_component PRIVATE include) +ament_target_dependencies(topic_age_monitor_component "lifecycle_msgs" - "std_msgs" "rclcpp" "rclcpp_lifecycle" "rclcpp_components" "diagnostic_msgs" "diagnostic_updater" ) -ament_export_targets(export_header_topic_monitor_component) -install(TARGETS header_topic_monitor_component - EXPORT export_header_topic_monitor_component +ament_export_targets(export_topic_age_monitor_component) +install(TARGETS topic_age_monitor_component + EXPORT export_topic_age_monitor_component ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) - -# Install launch files. -install( - DIRECTORY launch - DESTINATION share/${PROJECT_NAME}/ -) -# Install config files. install( - DIRECTORY config - DESTINATION share/${PROJECT_NAME}/ + DIRECTORY include/ + DESTINATION include ) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() + find_package(launch_testing_ament_cmake REQUIRED) + + # Install the test configs and dummy publisher used for testing + install( + DIRECTORY test + DESTINATION share/${PROJECT_NAME}/ + ) + install( + PROGRAMS test/dummy_publishers.py + DESTINATION lib/${PROJECT_NAME} + ) + + add_launch_test( + test/test_topic_frequency_monitor.py + TARGET test_topic_frequency_monitor + TIMEOUT 20) - find_package(ament_cmake_pytest REQUIRED) - file(GLOB _pytest_tests TESTS test/*.py) - foreach(_test_path ${_pytest_tests}) - get_filename_component(_test_name ${_test_path} NAME_WE) - ament_add_pytest_test(${_test_name} ${_test_path} - APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} - TIMEOUT 60 - WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} - ) - endforeach() + add_launch_test( + test/test_topic_age_monitor.py + TARGET test_topic_age_monitor + TIMEOUT 20) endif() ament_package() diff --git a/diagnostic_topic_monitor/README.md b/diagnostic_topic_monitor/README.md index 2b5aaa3ac..4a4070d9e 100644 --- a/diagnostic_topic_monitor/README.md +++ b/diagnostic_topic_monitor/README.md @@ -1,76 +1,89 @@ -# ROB Topic Monitor +# Diagnostic Topic Monitor -This is a generic topic monitor to check that topic publication rate is as expected, reporting status using diagnostics. +Offers diagnostic tools which monitor topics for health, such as frequency and age of messages. The output can be further processed with the [`diagnostic_aggregator`](https://github.com/ros/diagnostics/tree/ros2/diagnostic_aggregator) for cleaner observation. -## Acknowledgments +## Monitors -Originally developed for TOP-100 "BAUTIRO". +There are two monitors currently available, which can be used as components or standalone nodes: + * topic_frequency_monitor -- this supports _any_ kind of topic and checks for frequency based on receive time + * topic_age_monitor -- this supports only topics with a Header, and it checks that message age is within a given range -## Technical Debt - -This code was originally developed for ROS 2 "Galactic" and contains backports of some functionality that are no longer necessary for later ROS 2 versions. - -Moreover, the header_topic_monitor is largely copy-pasted. +It is totally possible to run both of these at the same time, for different or the same topics, to check for both frequency +and max message age at the same time. ## Usage -The nodes and components use the ROS 2 life-cycle based, which means they won't just activate when run. See `launch/topic_monitor_launch.py` for an example of how to start the nodes. +You typically add the monitor(s) you want to a launch file in your package and configure the parameters to your liking. However, the nodes and components are ROS 2 life-cycle based, which means you need to configure and activate them manually. -### Nodes +See the launch tests in the `test` folder for examples of how start the standalone monitor nodes, and example configs in `test/config`. -There are two nodes which behave almost the same - * topic_monitor_node -- this supports _any_ kind of topic and checks for frequency based on receive time - * header_topic_monitor_node -- this supports only topics with a Header, and it checks that message age is within a given range +> TIP: If you like the brevity and don't mind the added dependency, you can use the [nav2_lifecycle_manager](https://docs.nav2.org/configuration/packages/configuring-lifecycle.html) with parameter `autostart` set to `true` and target your monitor nodes to autostart them. -It is totally possible to run both of these at the same time _for the same topics_, to check for both frequency -and max message age at the same time. +> Example launch snippet: +```python +# Include this node in your LaunchConfiguration +autostart_monitors_node = LifecycleNode( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="autostart_monitors", + parameters=[ + { + "autostart": True, + "node_names": [], + "bond_timeout": 0.0, + } + ], + ) +``` -Some of the parameters are common for both nodes: +> For this, you need to have the `nav2_lifecycle_manager` package available (binary or source code installation), and ideally add it as an `exec_depend` in your package. -#### Limiting which topics to monitor +### Limiting which topics to monitor -By default, the following internal topic names will be ignored, as they are usually irrelevant: +By default, the following internal topic names will be ignored by the monitors, as they are usually irrelevant: * ^/rosout$ * .*/parameter_events * ^/diagnostics$ * .*/transition_event -`monitor_configured_only` - If false (the default), the topic monitor will monitor everything but the internal topics mentioned above. - To instead only monitor those topics which have been explicitly configured, set this to "true" +ROS2 parameter `monitor_configured_only` for both monitors: + +If false (the default), the topic monitor will monitor everything but the internal topics mentioned above. +To instead only monitor those topics which have been explicitly configured, set this to "true" -#### Influencing the output +### Influencing the output -`diag_prefix` - This string will be prefixed to the name of the diagnostic for use within the aggregator. +ROS2 parameter `diag_prefix` for both the monitors: -### `topic_monitor_node` +This string will be prefixed to the name of the diagnostic for use within the aggregator. + +## `topic_frequency_monitor` Without any parameters, this node (component also available) will subscribe to all normal topics in a system and minimally monitor that there is activity on them (defined as "at least one message per second"). For more specific configuration, the following parameters are available. -#### Frequency Monitoring +### Frequency Monitoring Usually, we want to make sure that the frequency of a topic is within certain bounds. -For example, to monitor that the frequency of the "talker" topic is about 25Hz and the frequency of the "cmd_vel" topic is about 10Hz, use the following three parameters and pass them lists in the same order. This means, the second element in "min_freqs" applies to the topic given by the second element of the "topics" parameter. +For example, to monitor that the frequency of the "talker" topic is about 25Hz and the frequency of the "cmd_vel" topic is about 10Hz, use the following three parameters and pass them lists in the same order. This means, the second element in "min_values" applies to the topic given by the second element of the "topics" parameter. topics: [ "talker", "cmd_vel" ] - min_freqs: [ 23.0, 8.0 ] - max_freqs: [ 26.5, 11.0 ] + min_values: [ 23.0, 8.0 ] + max_values: [ 26.5, 11.0 ] Please note that the frequencies will be computed purely based on _arrival_ timing, the header is currently ignored. -### `header_topic_monitor_node` +## `topic_age_monitor` Without any parameters, this node (component also available) will subscribe to all normal topics in a system and minimally monitor that there is activity on them (defined as "at least one message per second"). For more specific configuration, the following parameters are available. -#### Message Age Monitoring +### Message Age Monitoring Usually, we want to make sure that message is not much older than expected (allowing for some jitter due to network transmission), but also not newer than expected (which would indicate issues with timestamp or system time between machines). -Since this relies on a sender-side timestamp, this monitor only works with message that include a Header. +Since this relies on a sender-side timestamp, this monitor only works with message that include a Header. NOTE: You can configure it for any topic and it _will_ interpret the data in there as a timestamp, but unless it really is a header, the results will be rather random ;-) @@ -78,5 +91,17 @@ really is a header, the results will be rather random ;-) Configuration is similar to the frequency monitor, but giving a range for delays instead: topics: [ "talker", "cmd_vel" ] - min_delays: [ 80, 8.0 ] - max_delays: [ 120, 11.0 ] + min_values: [ 80, 8.0 ] + max_values: [ 120, 11.0 ] + monitor_configured_only: True + +> It is highly prudent to only monitor configured topics in the age monitor. If you set it to monitor all, +and you have topics without header fields, you will get a lot of warning messages!! + +## Testing + +To run all tests in this package: + +```bash +colcon test --event-handlers console_direct+ --packages-select diagnostic_topic_monitor && colcon test-result +``` diff --git a/diagnostic_topic_monitor/config/diagnostics.perspective b/diagnostic_topic_monitor/config/diagnostics.perspective deleted file mode 100644 index 45bc8679a..000000000 --- a/diagnostic_topic_monitor/config/diagnostics.perspective +++ /dev/null @@ -1,221 +0,0 @@ -{ - "keys": {}, - "groups": { - "mainwindow": { - "keys": { - "geometry": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb0003000000000f8b0000007000001f9f0000070000000f8b000000ba00001f9f000007000000000100000000180000000f8b000000ba00001f9f00000700')", - "type": "repr(QByteArray.hex)", - "pretty-print": " p " - }, - "state": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd00000001000000030000101500000603fc0100000003fb00000022007200710074005f0074006f0070005f005f0054004f0050005f005f0031005f005f0100000000000002c80000013700fffffffc000002d4000006770000028500fffffffc0200000002fb0000004c007200710074005f0063006f006e0073006f006c0065005f005f0043006f006e0073006f006c0065005f005f0031005f005f0043006f006e0073006f006c00650057006900640067006500740100000022000002e30000029f00fffffffb00000078007200710074005f00720075006e00740069006d0065005f006d006f006e00690074006f0072005f005f00520075006e00740069006d0065004d006f006e00690074006f0072005f005f0031005f005f00520075006e00740069006d0065004d006f006e00690074006f0072005700690064006700650074010000031100000314000000ab00fffffffb0000004c007200710074005f0074006f007000690063005f005f0054006f0070006900630050006c007500670069006e005f005f0031005f005f0054006f0070006900630057006900640067006500740100000957000006be0000011c00ffffff000010150000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", - "type": "repr(QByteArray.hex)", - "pretty-print": " \"rqt_top__TOP__1__ w \" W " - } - }, - "groups": { - "toolbar_areas": { - "keys": { - "MinimizedDockWidgetsToolbar": { - "repr": "8", - "type": "repr" - } - }, - "groups": {} - } - } - }, - "pluginmanager": { - "keys": { - "running-plugins": { - "repr": "{'rqt_console/Console': [1], 'rqt_runtime_monitor/RuntimeMonitor': [1], 'rqt_top/TOP': [1], 'rqt_topic/TopicPlugin': [1]}", - "type": "repr" - } - }, - "groups": { - "plugin__rqt_console__Console__1": { - "keys": {}, - "groups": { - "dock_widget__ConsoleWidget": { - "keys": { - "dock_widget_title": { - "repr": "'Console'", - "type": "repr" - }, - "dockable": { - "repr": "True", - "type": "repr" - }, - "parent": { - "repr": "None", - "type": "repr" - } - }, - "groups": {} - }, - "plugin": { - "keys": { - "exclude_filters": { - "repr": "'severity'", - "type": "repr" - }, - "filter_splitter": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff0000000100000002000000d4000000d40100000009010000000200')", - "type": "repr(QByteArray.hex)", - "pretty-print": " " - }, - "highlight_filters": { - "repr": "'message'", - "type": "repr" - }, - "message_limit": { - "repr": "20000", - "type": "repr" - }, - "paused": { - "repr": "False", - "type": "repr" - }, - "settings_exist": { - "repr": "True", - "type": "repr" - }, - "show_highlighted_only": { - "repr": "False", - "type": "repr" - }, - "table_splitter": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000001000000020000008a000001b10100000009010000000200')", - "type": "repr(QByteArray.hex)", - "pretty-print": " " - } - }, - "groups": { - "exclude_filter_0": { - "keys": { - "enabled": { - "repr": "True", - "type": "repr" - }, - "itemlist": { - "repr": "''", - "type": "repr" - } - }, - "groups": {} - }, - "highlight_filter_0": { - "keys": { - "enabled": { - "repr": "True", - "type": "repr" - }, - "regex": { - "repr": "False", - "type": "repr" - }, - "text": { - "repr": "''", - "type": "repr" - } - }, - "groups": {} - } - } - } - } - }, - "plugin__rqt_runtime_monitor__RuntimeMonitor__1": { - "keys": {}, - "groups": { - "dock_widget__RuntimeMonitorWidget": { - "keys": { - "dock_widget_title": { - "repr": "'Runtime Monitor'", - "type": "repr" - }, - "dockable": { - "repr": "True", - "type": "repr" - }, - "parent": { - "repr": "None", - "type": "repr" - } - }, - "groups": {} - } - } - }, - "plugin__rqt_top__TOP__1": { - "keys": {}, - "groups": { - "dock_widget__": { - "keys": { - "dock_widget_title": { - "repr": "'Process Monitor'", - "type": "repr" - }, - "dockable": { - "repr": "True", - "type": "repr" - }, - "parent": { - "repr": "None", - "type": "repr" - } - }, - "groups": {} - }, - "plugin": { - "keys": { - "filter_text": { - "repr": "''", - "type": "repr" - }, - "is_regex": { - "repr": "0", - "type": "repr" - } - }, - "groups": {} - } - } - }, - "plugin__rqt_topic__TopicPlugin__1": { - "keys": {}, - "groups": { - "dock_widget__TopicWidget": { - "keys": { - "dock_widget_title": { - "repr": "'Topic Monitor'", - "type": "repr" - }, - "dockable": { - "repr": "True", - "type": "repr" - }, - "parent": { - "repr": "None", - "type": "repr" - } - }, - "groups": {} - }, - "plugin": { - "keys": { - "tree_widget_header_state": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff0000000000000001000000000000000501000000000000000000000006200000000100000005000000c8000006980000000601010001000000000000000006000000c8ffffffff000000810000000300000006000001340000000100000003000001d10000000100000003000000c200000001000000030000005400000001000000030000027d0000000100000003000000000000000100000003000003e800000000c8')", - "type": "repr(QByteArray.hex)", - "pretty-print": " " - } - }, - "groups": {} - } - } - } - } - } - } -} \ No newline at end of file diff --git a/diagnostic_topic_monitor/config/foxglove_bridge.yaml b/diagnostic_topic_monitor/config/foxglove_bridge.yaml deleted file mode 100644 index 988d8c2cf..000000000 --- a/diagnostic_topic_monitor/config/foxglove_bridge.yaml +++ /dev/null @@ -1,34 +0,0 @@ -foxglove_bridge_ccu: - ros__parameters: - address: 0.0.0.0 - asset_uri_allowlist: - - ^package://(?:\w+/)*\w+\..*$ - capabilities: - - clientPublish - - connectionGraph - - services - - assets - certfile: '' - client_topic_whitelist: - - .* - include_hidden: false - keyfile: '' - max_qos_depth: 100 - min_qos_depth: 1 - param_whitelist: - - .* - port: 8765 - qos_overrides: - /parameter_events: - publisher: - depth: 1000 - durability: volatile - history: keep_last - reliability: reliable - send_buffer_limit: 50000000 - service_whitelist: - - .* - tls: false - topic_whitelist: - - .* - use_compression: true diff --git a/diagnostic_topic_monitor/config/topic_check.yaml b/diagnostic_topic_monitor/config/topic_check.yaml deleted file mode 100644 index bd1ae1bb9..000000000 --- a/diagnostic_topic_monitor/config/topic_check.yaml +++ /dev/null @@ -1,45 +0,0 @@ -# /diagnostics/topic_frequency_monitor: -# ros__parameters: -# monitor_configured_only: True -# topics: -# - /odom -# - /base_link/odom -# - /rpm/sensors/front/imu/imu/data -# - /rpm/sensors/front/lidar3d/points -# - /rpm/sensors/rear/lidar3d/points -# min_freqs: -# - 49.0 -# - 7.5 -# - 99.0 -# - 9.5 -# - 9.5 -# max_freqs: -# - 51.0 -# - 10.0 -# - 100.0 -# - 10.5 -# - 10.5 - -# /diagnostics/message_age_monitor: -# ros__parameters: -# monitor_configured_only: True -# topics: -# - /odom -# - /base_link/odom -# - /rpm/sensors/front/imu/imu/data -# - /rpm/sensors/front/lidar3d/points -# - /rpm/sensors/rear/lidar3d/points -# min_delays: -# - -0.09 # can occur due to simulated time -# - -0.09 -# - -0.09 -# - 0.0 -# - 0.0 -# max_delays: -# - 0.1 -# - 0.1 -# - 0.1 -# - 0.200 -# - 0.200 - - diff --git a/diagnostic_topic_monitor/include/diagnostic_topic_monitor/activity_diagnostic_task.hpp b/diagnostic_topic_monitor/include/diagnostic_topic_monitor/activity_diagnostic_task.hpp new file mode 100644 index 000000000..a1ea5152a --- /dev/null +++ b/diagnostic_topic_monitor/include/diagnostic_topic_monitor/activity_diagnostic_task.hpp @@ -0,0 +1,60 @@ +// TODO: Insert copyright + +#ifndef ACTIVITY_DIAGNOSTIC_TASK_HPP +#define ACTIVITY_DIAGNOSTIC_TASK_HPP + +#include // NOLINT: upstream + +using namespace std::chrono_literals; + +namespace diagnostic_topic_monitor +{ + +/** + * @brief Very simply DiagnosticTask that just checks whether there has been _any_ + * kind of activity during the last period + */ +class ActivityDiagnosticTask : public diagnostic_updater::DiagnosticTask +{ +public: + ActivityDiagnosticTask( + const std::string & name, rclcpp::Clock::SharedPtr clock, + std::chrono::duration warning_threshold = 1.1s) + : diagnostic_updater::DiagnosticTask(name), + clock_(clock), + last_tick_(0), + warning_threshold_(warning_threshold) + { + } + virtual void tick() {last_tick_ = clock_->now();} + void run(diagnostic_updater::DiagnosticStatusWrapper & stat) override + { + stat.name = getName(); + if (last_tick_.seconds() == 0) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "No data received, yet"; + stat.addf("period", "%f", std::numeric_limits::max()); + } else { + const auto currentTime = clock_->now(); + // more than 1s without tick is considered warning + const auto elapsed = currentTime - last_tick_; + stat.addf("period", "%f", elapsed.seconds()); + if (elapsed <= rclcpp::Duration(warning_threshold_)) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + } else { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "Last data received more than 1s ago"; + } + } + } + +private: + rclcpp::Clock::SharedPtr clock_; + rclcpp::Time last_tick_; + std::chrono::duration warning_threshold_; +}; + +} // namespace diagnostic_topic_monitor + +#endif // ACTIVITY_DIAGNOSTIC_TASK_HPP diff --git a/diagnostic_topic_monitor/include/diagnostic_topic_monitor/generic_topic_monitor.hpp b/diagnostic_topic_monitor/include/diagnostic_topic_monitor/generic_topic_monitor.hpp new file mode 100644 index 000000000..f778655cd --- /dev/null +++ b/diagnostic_topic_monitor/include/diagnostic_topic_monitor/generic_topic_monitor.hpp @@ -0,0 +1,222 @@ +#ifndef GENERIC_TOPIC_MONITOR_HPP +#define GENERIC_TOPIC_MONITOR_HPP + +#include + +#include +#include +#include +#include +#include // NOLINT: upstream +#include // NOLINT: upstream +#include +#include +#include +#include +#include + +#include "diagnostic_msgs/msg/diagnostic_array.hpp" // NOLINT: upstream +#include "diagnostic_topic_monitor/activity_diagnostic_task.hpp" +#include "diagnostic_updater/publisher.hpp" // NOLINT: upstream +#include "diagnostic_updater/update_functions.hpp" // NOLINT: upstream + +namespace diagnostic_topic_monitor +{ +constexpr const char * TOPICS_PARAM_NAME = "topics"; +constexpr const char * MIN_VALUES_PARAM_NAME = "min_values"; +constexpr const char * MAX_VALUES_PARAM_NAME = "max_values"; +constexpr const char * MONITOR_CONFIGURED_ONLY_PARAM_NAME = "monitor_configured_only"; +constexpr const char * DIAG_PREFIX_PARAM_NAME = "diag_prefix"; +typedef rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LCCBReturn; + +/** + * @brief Base abstract class for creating Topic Monitors. + */ +template +class GenericTopicMonitor : public rclcpp_lifecycle::LifecycleNode +{ +public: + explicit GenericTopicMonitor(const std::string & node_name, rclcpp::NodeOptions options); + ~GenericTopicMonitor() {} + LCCBReturn on_configure(const rclcpp_lifecycle::State &) override; + LCCBReturn on_activate(const rclcpp_lifecycle::State & state) override; + LCCBReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + LCCBReturn on_cleanup(const rclcpp_lifecycle::State &) override {return LCCBReturn::SUCCESS;} + virtual CallbackReturn on_shutdown(const rclcpp_lifecycle::State &) override + { + return LCCBReturn::SUCCESS; + } + +protected: + std::string get_prefixed_name(const std::string & topic_name) const; + bool skip_topic(const std::string & topic_name); + virtual void topic_cb( + const std::string & topic_name, const std::shared_ptr & msg) = 0; + virtual void update_topic_subscriptions(); + virtual StatusParamType parse_params(const int index) = 0; + + std::vector hidden_topics_{ + std::regex("^/rosout$"), std::regex(".*/parameter_events"), std::regex("^/diagnostics$"), + std::regex(".*/transition_event"), std::regex("^/clock")}; + std::shared_ptr timer_; + std::shared_ptr updater_; + std::unordered_map> subscribed_topics_; + std::unordered_map> topic_diag_map_; + std::vector topics_; + std::set known_topics_; + std::vector min_values_; + std::vector max_values_; + bool monitor_configured_only_{true}; + std::string diag_prefix_; +}; + +// Implementation of GenericTopicMonitor methods +template +inline GenericTopicMonitor::GenericTopicMonitor( + const std::string & node_name, rclcpp::NodeOptions options) +: rclcpp_lifecycle::LifecycleNode(node_name, options.allow_undeclared_parameters(true)) +{ + auto desc = rcl_interfaces::msg::ParameterDescriptor{}; + desc.description = "Topics to specify expectations for"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; + declare_parameter(TOPICS_PARAM_NAME, std::vector(), desc); + auto desc_min = rcl_interfaces::msg::ParameterDescriptor{}; + desc_min.description = "Minimum values for topic from 'topics'"; + desc_min.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY; + declare_parameter(MIN_VALUES_PARAM_NAME, std::vector(), desc_min); + auto desc_max = rcl_interfaces::msg::ParameterDescriptor{}; + desc_max.description = "Maximum values for topic from 'topics'"; + desc_max.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY; + declare_parameter(MAX_VALUES_PARAM_NAME, std::vector(), desc_max); + // by default, we include all topics + declare_parameter(MONITOR_CONFIGURED_ONLY_PARAM_NAME, false); + // prefix for diagnostics (for use with modules) + declare_parameter(DIAG_PREFIX_PARAM_NAME, ""); +} + +template +inline std::string +diagnostic_topic_monitor::GenericTopicMonitor::get_prefixed_name( + const std::string & topic_name) const +{ + if (topic_name.find(diag_prefix_) == 0) { + return topic_name; + } + return diag_prefix_ + std::string("/") + topic_name; +} + +template +inline bool diagnostic_topic_monitor::GenericTopicMonitor::skip_topic( + const std::string & topic_name) +{ + // skip if we already subscribed + if (subscribed_topics_.find(topic_name) != subscribed_topics_.end()) { + RCLCPP_DEBUG(get_logger(), "Already subscribed to %s, skipping", topic_name.c_str()); + return true; + } + // if it's configured, we monitor it + if (std::find(topics_.begin(), topics_.end(), topic_name) != topics_.end()) { + RCLCPP_DEBUG(get_logger(), "Topic %s is configured, no skip", topic_name.c_str()); + return false; + } + // skip if we have seen this before and ignored it + if (known_topics_.find(topic_name) != known_topics_.end()) { + return true; + } + // check for matches against internal topic names + for (const auto & re : hidden_topics_) { + if (std::regex_match(topic_name, re)) { + known_topics_.insert(topic_name); + RCLCPP_DEBUG(get_logger(), "Topic %s matches ignore list, skip it", topic_name.c_str()); + return true; + } + } + return monitor_configured_only_; +} + +template +inline void diagnostic_topic_monitor::GenericTopicMonitor< + StatusType, StatusParamType>::update_topic_subscriptions() +{ + RCLCPP_DEBUG( + get_logger(), "Examining topic list for changes, currently monitoring %ld topics", + subscribed_topics_.size()); + const auto topics = this->get_topic_names_and_types(); + for (const auto & e : topics) { + const std::string topic_name = e.first; + if (skip_topic(topic_name)) { + continue; + } + // subscribe with statistics enabled + RCLCPP_DEBUG(get_logger(), "Starting to monitor topic %s", topic_name.c_str()); + std::shared_ptr sub = this->create_generic_subscription( + e.first, *e.second.begin(), rclcpp::QoS(10), + [this, topic_name](std::shared_ptr msg) { + this->topic_cb(topic_name, msg); + }); + subscribed_topics_[topic_name] = sub; + } +} + +template +inline LCCBReturn +diagnostic_topic_monitor::GenericTopicMonitor::on_configure( + const rclcpp_lifecycle::State &) +{ + RCLCPP_DEBUG(get_logger(), "Configuring"); + // configure the diagnostics + topics_ = get_parameter(TOPICS_PARAM_NAME).as_string_array(); + min_values_ = get_parameter(MIN_VALUES_PARAM_NAME).as_double_array(); + max_values_ = get_parameter(MAX_VALUES_PARAM_NAME).as_double_array(); + if (topics_.size() != min_values_.size() || topics_.size() != max_values_.size()) { + throw std::invalid_argument("Topics and min/max_values must have same number of arguments"); + } + monitor_configured_only_ = get_parameter(MONITOR_CONFIGURED_ONLY_PARAM_NAME).as_bool(); + diag_prefix_ = get_parameter(DIAG_PREFIX_PARAM_NAME).as_string(); + + RCLCPP_DEBUG( + get_logger(), "Done configuring for %ld topics, config only: %d", topic_diag_map_.size(), + monitor_configured_only_); + return LCCBReturn::SUCCESS; +} + +template +inline LCCBReturn +diagnostic_topic_monitor::GenericTopicMonitor::on_activate( + const rclcpp_lifecycle::State &) +{ + RCLCPP_DEBUG(get_logger(), "Activating"); + updater_ = std::make_shared(this); + char HOSTNAME[1000]; + gethostname(HOSTNAME, 1000); + updater_->setHardwareID(std::string(HOSTNAME)); + for (size_t i = 0; i < topics_.size(); ++i) { + auto param = parse_params(i); + auto diag = std::make_shared(param, get_prefixed_name(topics_[i]), get_clock()); + topic_diag_map_[topics_[i]] = diag; + updater_->add(*diag); + } + // check existing topics + try { + update_topic_subscriptions(); + } catch (const std::exception & ex) { + RCLCPP_ERROR(this->get_logger(), "Failure to update subscriptions: %s", ex.what()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + timer_ = create_wall_timer(1s, [this]() {this->update_topic_subscriptions();}); + return LCCBReturn::SUCCESS; +} + +template +inline LCCBReturn +diagnostic_topic_monitor::GenericTopicMonitor::on_deactivate( + const rclcpp_lifecycle::State & state) +{ + (void)state; + RCLCPP_INFO_STREAM(get_logger(), "Deactivating GenericTopicMonitor"); + return LCCBReturn::SUCCESS; +} + +} // namespace diagnostic_topic_monitor + +#endif // GENERIC_TOPIC_MONITOR_HPP diff --git a/diagnostic_topic_monitor/include/diagnostic_topic_monitor/topic_age_monitor.hpp b/diagnostic_topic_monitor/include/diagnostic_topic_monitor/topic_age_monitor.hpp new file mode 100644 index 000000000..59e1974c6 --- /dev/null +++ b/diagnostic_topic_monitor/include/diagnostic_topic_monitor/topic_age_monitor.hpp @@ -0,0 +1,29 @@ +#ifndef DIAGNOSTIC_TOPIC_MONITOR_TOPIC_FREQUENCY_MONITOR_HPP +#define DIAGNOSTIC_TOPIC_MONITOR_TOPIC_FREQUENCY_MONITOR_HPP + +#include "diagnostic_topic_monitor/generic_topic_monitor.hpp" + +namespace diagnostic_topic_monitor +{ + +class TopicAgeMonitor final + : public GenericTopicMonitor< + diagnostic_updater::TimeStampStatus, diagnostic_updater::TimeStampStatusParam> +{ +public: + TopicAgeMonitor(const std::string & node_name, rclcpp::NodeOptions options); + TopicAgeMonitor(rclcpp::NodeOptions options = rclcpp::NodeOptions()); + ~TopicAgeMonitor() override = default; + + void topic_cb( + const std::string & topic_name, + const std::shared_ptr & msg) override; + diagnostic_updater::TimeStampStatusParam parse_params(const int index) override; + +private: + rclcpp::Serialization _header_serializer; +}; + +} // namespace diagnostic_topic_monitor + +#endif // DIAGNOSTIC_TOPIC_MONITOR_TOPIC_FREQUENCY_MONITOR_HPP diff --git a/diagnostic_topic_monitor/include/diagnostic_topic_monitor/topic_frquency_monitor.hpp b/diagnostic_topic_monitor/include/diagnostic_topic_monitor/topic_frquency_monitor.hpp new file mode 100644 index 000000000..5251a02ce --- /dev/null +++ b/diagnostic_topic_monitor/include/diagnostic_topic_monitor/topic_frquency_monitor.hpp @@ -0,0 +1,30 @@ +#ifndef DIAGNOSTIC_TOPIC_MONITOR_TOPIC_FREQUENCY_MONITOR_HPP +#define DIAGNOSTIC_TOPIC_MONITOR_TOPIC_FREQUENCY_MONITOR_HPP + +#include "diagnostic_topic_monitor/generic_topic_monitor.hpp" + +namespace diagnostic_topic_monitor +{ + +class TopicFrequencyMonitor final + : public GenericTopicMonitor< + diagnostic_updater::FrequencyStatus, diagnostic_updater::FrequencyStatusParam> +{ +public: + TopicFrequencyMonitor(const std::string & node_name, rclcpp::NodeOptions options); + TopicFrequencyMonitor(rclcpp::NodeOptions options = rclcpp::NodeOptions()); + ~TopicFrequencyMonitor() override = default; + + void topic_cb( + const std::string & topic_name, + const std::shared_ptr & msg) override; + diagnostic_updater::FrequencyStatusParam parse_params(const int index) override; + void update_topic_subscriptions() override; + +private: + std::unordered_map> fallback_topic_diag_map_; +}; + +} // namespace diagnostic_topic_monitor + +#endif // DIAGNOSTIC_TOPIC_MONITOR_TOPIC_FREQUENCY_MONITOR_HPP diff --git a/diagnostic_topic_monitor/include/update_functions.hpp b/diagnostic_topic_monitor/include/update_functions.hpp deleted file mode 100644 index c7ce00466..000000000 --- a/diagnostic_topic_monitor/include/update_functions.hpp +++ /dev/null @@ -1,307 +0,0 @@ -// Copyright (c) 2009, Willow Garage, Inc. - -/* Software License Agreement (BSD License) - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - - -#ifndef UPDATE_FUNCTIONS_HPP_ -#define UPDATE_FUNCTIONS_HPP_ - -#include -#include -#include -#include - -#include "diagnostic_updater/diagnostic_updater.hpp" - -// WORKAROUND until FrequencyStatus with sim_time support is available (humble+) -namespace topic_monitor -{ - -using diagnostic_updater::DiagnosticTask; - -struct FrequencyStatusParam -{ - FrequencyStatusParam( - double * min_freq, double * max_freq, - double tolerance = 0.1, int window_size = 5) - : min_freq_(min_freq), max_freq_(max_freq), tolerance_(tolerance), - window_size_(window_size) {} - - double * min_freq_; - - double * max_freq_; - - double tolerance_; - - int window_size_; -}; - -class FrequencyStatus : public DiagnosticTask -{ -private: - const FrequencyStatusParam params_; - - int count_; - std::vector times_; - std::vector seq_nums_; - int hist_indx_; - std::mutex lock_; - rclcpp::Logger debug_logger_; - const rclcpp::Clock::SharedPtr clock_ptr_; - -public: - FrequencyStatus( - const FrequencyStatusParam & params, - std::string name, - const rclcpp::Clock::SharedPtr & clock = std::make_shared()) - : DiagnosticTask(name), params_(params), times_(params_.window_size_), - seq_nums_(params_.window_size_), - debug_logger_(rclcpp::get_logger("FrequencyStatus_debug_logger")), - clock_ptr_(clock) - { - clear(); - } - - explicit FrequencyStatus( - const FrequencyStatusParam & params, - const rclcpp::Clock::SharedPtr & clock = std::make_shared()) - : FrequencyStatus(params, "Frequency Status", clock) - {} - - void clear() - { - std::unique_lock lock(lock_); - rclcpp::Time curtime = clock_ptr_->now(); - count_ = 0; - - for (int i = 0; i < params_.window_size_; i++) { - times_[i] = curtime; - seq_nums_[i] = count_; - } - - hist_indx_ = 0; - } - - void tick() - { - std::unique_lock lock(lock_); - RCLCPP_DEBUG(debug_logger_, "TICK %i", count_); - count_++; - } - - virtual void run(diagnostic_updater::DiagnosticStatusWrapper & stat) - { - std::unique_lock lock(lock_); - rclcpp::Time curtime = clock_ptr_->now(); - - int curseq = count_; - int events = curseq - seq_nums_[hist_indx_]; - double window = curtime.seconds() - times_[hist_indx_].seconds(); - double freq = events / window; - seq_nums_[hist_indx_] = curseq; - times_[hist_indx_] = curtime; - hist_indx_ = (hist_indx_ + 1) % params_.window_size_; - - if (events == 0) { - stat.summary(2, "No events recorded."); - } else if (freq < *params_.min_freq_ * (1 - params_.tolerance_)) { - stat.summary(1, "Frequency too low."); - } else if (freq > *params_.max_freq_ * (1 + params_.tolerance_)) { - stat.summary(1, "Frequency too high."); - } else { - stat.summary(0, "Desired frequency met"); - } - - stat.addf("Events in window", "%d", events); - stat.addf("Events since startup", "%d", count_); - stat.addf("Duration of window (s)", "%f", window); - stat.addf("Actual frequency (Hz)", "%f", freq); - if (*params_.min_freq_ == *params_.max_freq_) { - stat.addf("Target frequency (Hz)", "%f", *params_.min_freq_); - } - if (*params_.min_freq_ > 0) { - stat.addf( - "Minimum acceptable frequency (Hz)", "%f", - *params_.min_freq_ * (1 - params_.tolerance_)); - } - if (std::isfinite(*params_.max_freq_)) { - stat.addf( - "Maximum acceptable frequency (Hz)", "%f", - *params_.max_freq_ * (1 + params_.tolerance_)); - } - } -}; - -struct TimeStampStatusParam -{ - TimeStampStatusParam( - const double min_acceptable = -1, - const double max_acceptable = 5) - : max_acceptable_(max_acceptable), min_acceptable_(min_acceptable) {} - - double max_acceptable_; - - double min_acceptable_; -}; - -static TimeStampStatusParam DefaultTimeStampStatusParam = - TimeStampStatusParam(); - -class TimeStampStatus : public DiagnosticTask -{ -private: - void init() - { - early_count_ = 0; - late_count_ = 0; - zero_count_ = 0; - zero_seen_ = false; - max_delta_ = 0; - min_delta_ = 0; - deltas_valid_ = false; - } - -public: - TimeStampStatus( - const TimeStampStatusParam & params, - std::string name, - const rclcpp::Clock::SharedPtr & clock = std::make_shared()) - : DiagnosticTask(name), params_(params), clock_ptr_(clock) - { - init(); - } - - explicit TimeStampStatus( - const TimeStampStatusParam & params, - const rclcpp::Clock::SharedPtr & clock = std::make_shared()) - : DiagnosticTask("Timestamp Status"), params_(params), clock_ptr_(clock) - { - init(); - } - - explicit TimeStampStatus( - const rclcpp::Clock::SharedPtr & clock = std::make_shared()) - : DiagnosticTask("Timestamp Status"), clock_ptr_(clock) {init();} - - void tick(double stamp) - { - std::unique_lock lock(lock_); - - if (stamp == 0) { - zero_seen_ = true; - } else { - const double delta = clock_ptr_->now().seconds() - stamp; - - if (!deltas_valid_ || delta > max_delta_) { - max_delta_ = delta; - } - - if (!deltas_valid_ || delta < min_delta_) { - min_delta_ = delta; - } - - deltas_valid_ = true; - } - } - - void tick(const rclcpp::Time t) {tick(t.seconds());} - - virtual void run(diagnostic_updater::DiagnosticStatusWrapper & stat) - { - std::unique_lock lock(lock_); - - stat.summary(0, "Timestamps are reasonable."); - if (!deltas_valid_) { - stat.summary(1, "No data since last update."); - } else { - if (min_delta_ < params_.min_acceptable_) { - stat.summary(2, "Timestamps too far in future seen."); - early_count_++; - } - - if (max_delta_ > params_.max_acceptable_) { - stat.summary(2, "Timestamps too far in past seen."); - late_count_++; - } - - if (zero_seen_) { - stat.summary(2, "Zero timestamp seen."); - zero_count_++; - } - } - - stat.addf("Earliest timestamp delay:", "%f", min_delta_); - stat.addf("Latest timestamp delay:", "%f", max_delta_); - stat.addf( - "Earliest acceptable timestamp delay:", "%f", - params_.min_acceptable_); - stat.addf( - "Latest acceptable timestamp delay:", "%f", - params_.max_acceptable_); - stat.add("Late diagnostic update count:", late_count_); - stat.add("Early diagnostic update count:", early_count_); - stat.add("Zero seen diagnostic update count:", zero_count_); - - deltas_valid_ = false; - min_delta_ = 0; - max_delta_ = 0; - zero_seen_ = false; - } - -private: - TimeStampStatusParam params_; - int early_count_; - int late_count_; - int zero_count_; - bool zero_seen_; - double max_delta_; - double min_delta_; - bool deltas_valid_; - const rclcpp::Clock::SharedPtr clock_ptr_; - std::mutex lock_; -}; - -class Heartbeat : public DiagnosticTask -{ -public: - Heartbeat() - : DiagnosticTask("Heartbeat") {} - - virtual void run(diagnostic_updater::DiagnosticStatusWrapper & stat) - { - stat.summary(0, "Alive"); - } -}; -} // namespace topic_monitor - -#endif // UPDATE_FUNCTIONS_HPP_ diff --git a/diagnostic_topic_monitor/launch/topic_monitor_launch.py b/diagnostic_topic_monitor/launch/topic_monitor_launch.py deleted file mode 100644 index 0b0477716..000000000 --- a/diagnostic_topic_monitor/launch/topic_monitor_launch.py +++ /dev/null @@ -1,76 +0,0 @@ -# Copyright 2024 Robert Bosch GmbH and its subsidiaries -# -# All rights reserved, also regarding any disposal, exploitation, reproduction, -# editing, distribution, as well as in the event of applications for industrial -# property rights. - -from launch import LaunchDescription -from launch.actions import GroupAction -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable -from launch.substitutions import PathJoinSubstitution, LaunchConfiguration -from launch_ros.actions import Node, PushRosNamespace -from launch_ros.substitutions import FindPackageShare - -DIAG_NAMESPACE = "/diagnostics" -FREQ_MONITOR_NAME = "topic_frequency_monitor" -AGE_MONITOR_NAME = "message_age_monitor" - - -def generate_launch_description(): - - return LaunchDescription([ - DeclareLaunchArgument( - "use_sim_time", default_value="False", description="Whether to use sim_time" - ), - SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"), - DeclareLaunchArgument( - "namespace", default_value="", description="Top-level namespace" - ), - GroupAction( - actions=[ - PushRosNamespace(namespace=DIAG_NAMESPACE), - Node( - package="rob_topic_monitor", - executable="topic_monitor", - name=FREQ_MONITOR_NAME, - parameters=[ - {'use_sim_time': LaunchConfiguration('use_sim_time'), - 'monitor_configured_only': True}, - PathJoinSubstitution([ - FindPackageShare("rob_topic_monitor"), - "config", - "topic_check.yaml"]) - ] - ), - Node( - package="rob_topic_monitor", - executable="header_topic_monitor", - name=AGE_MONITOR_NAME, - parameters=[ - {'use_sim_time': LaunchConfiguration('use_sim_time'), - 'monitor_configured_only': False}, - PathJoinSubstitution([ - FindPackageShare("rob_topic_monitor"), - "config", - "topic_check.yaml"]) - ] - ), - Node( - package="nav2_lifecycle_manager", - executable="lifecycle_manager", - namespace="", - name="lifecycle_manager_module_diag", - output="both", - parameters=[ - {'use_sim_time': LaunchConfiguration('use_sim_time'), - "autostart": True, - "node_names": [ - [DIAG_NAMESPACE, "/", FREQ_MONITOR_NAME], - [DIAG_NAMESPACE, "/", AGE_MONITOR_NAME] - ], - 'bond_timeout': 0.0}, - ], - ), - ] - ) - ]) diff --git a/diagnostic_topic_monitor/package.xml b/diagnostic_topic_monitor/package.xml index a68b62df1..af5116c6a 100644 --- a/diagnostic_topic_monitor/package.xml +++ b/diagnostic_topic_monitor/package.xml @@ -5,6 +5,8 @@ 1.0.0 Generic Topic Monitor Ingo Lütkebohle + Ingo Lütkebohle + Tejas Kumar Shastha BIOSv4 ament_cmake @@ -24,9 +26,6 @@ ament_lint_common ament_cmake_pytest launch_testing - examples_rclcpp_minimal_publisher - examples_rclcpp_minimal_action_server - example_interfaces ament_cmake diff --git a/diagnostic_topic_monitor/src/header_topic_monitor_component.cpp b/diagnostic_topic_monitor/src/header_topic_monitor_component.cpp deleted file mode 100644 index f9490793f..000000000 --- a/diagnostic_topic_monitor/src/header_topic_monitor_component.cpp +++ /dev/null @@ -1,234 +0,0 @@ -// Copyright 2024 Robert Bosch GmbH and its subsidiaries -// All rights reserved, also regarding any disposal, exploitation, -// reproduction, editing, distribution, as well as in the event of applications -// for industrial property rights. - -#include -#include -#include -#include -#include -#include -#include -#include -#include // NOLINT: upstream -#include // NOLINT: upstream -#include // NOLINT: upstream -#include // NOLINT: upstream -#include // NOLINT: upstream -#include "rclcpp/serialization.hpp" -#include "update_functions.hpp" - -using namespace std::chrono_literals; - -namespace diagnostic_topic_monitor -{ -constexpr const char * TOPICS_PARAM_NAME = "topics"; -constexpr const char * MIN_DELAYS_PARAM_NAME = "min_delays"; -constexpr const char * MAX_DELAYS_PARAM_NAME = "max_delays"; -constexpr const char * MONITOR_CONFIGURED_ONLY_PARAM_NAME = "monitor_configured_only"; -constexpr const char * DIAG_PREFIX_PARAM_NAME = "diag_prefix"; - - -using namespace std::chrono_literals; -class HeaderTopicMonitor : public rclcpp_lifecycle::LifecycleNode -{ -public: - explicit HeaderTopicMonitor(const std::string & node_name, rclcpp::NodeOptions options) - : rclcpp_lifecycle::LifecycleNode(node_name, options.allow_undeclared_parameters(true)) - { - auto desc = rcl_interfaces::msg::ParameterDescriptor{}; - desc.description = "Topics to specify expectations for"; - desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; - declare_parameter(TOPICS_PARAM_NAME, std::vector(), desc); - auto desc_min = rcl_interfaces::msg::ParameterDescriptor{}; - desc_min.description = "Minimum delay for topic from 'topics'"; - desc_min.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY; - declare_parameter(MIN_DELAYS_PARAM_NAME, std::vector(), desc_min); - auto desc_max = rcl_interfaces::msg::ParameterDescriptor{}; - desc_max.description = "Maximum delay for topic from 'topics'"; - desc_max.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY; - declare_parameter(MAX_DELAYS_PARAM_NAME, std::vector(), desc_max); - // by default, we include all topics - declare_parameter(MONITOR_CONFIGURED_ONLY_PARAM_NAME, false); - // prefix for diagnostics (for use with modules) - declare_parameter(DIAG_PREFIX_PARAM_NAME, ""); - } - explicit HeaderTopicMonitor(const rclcpp::NodeOptions & options) - : HeaderTopicMonitor("header_topic_monitor", options) - { - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State &) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State & state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State & state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State &) override - { - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_shutdown(const rclcpp_lifecycle::State &) override - { - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - void topic_cb( - const std::string & topic_name, - const std::shared_ptr & msg) - { - std_msgs::msg::Header header; - const auto diag = topic_diag_map_.find(topic_name); - _header_serializer.deserialize_message(msg.get(), &header); - - if (diag != topic_diag_map_.end()) { - diag->second->tick(header.stamp); - } else { - RCLCPP_WARN(get_logger(), "No diagnostic found for topic %s", topic_name.c_str()); - } - } - -protected: - void update_topic_subscriptions(); - bool skip_topic(const std::string & name); - std::string get_prefixed_name(const std::string & topic_name) const; - -private: - std::vector hidden_topics_{std::regex("^/rosout$"), std::regex(".*/parameter_events"), - std::regex("^/diagnostics$"), std::regex(".*/transition_event"), std::regex("^/clock")}; - std::shared_ptr timer_; - std::shared_ptr updater_; - std::unordered_map> subscribed_topics_; - std::set known_topics_; - std::unordered_map> topic_diag_map_; - std::vector topics_; - std::vector min_delays; - std::vector max_delays; - bool monitor_configured_only_{true}; - std::string diag_prefix_; - rclcpp::Serialization _header_serializer; -}; - -std::string HeaderTopicMonitor::get_prefixed_name(const std::string & topic_name) const -{ - if (topic_name.find(diag_prefix_) == 0) { - return topic_name; - } - return diag_prefix_ + std::string("/") + topic_name; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -HeaderTopicMonitor::on_configure(const rclcpp_lifecycle::State &) -{ - RCLCPP_DEBUG(get_logger(), "Configuring"); - // configure the diagnostics - topics_ = get_parameter(TOPICS_PARAM_NAME).as_string_array(); - min_delays = get_parameter(MIN_DELAYS_PARAM_NAME).as_double_array(); - max_delays = get_parameter(MAX_DELAYS_PARAM_NAME).as_double_array(); - if (topics_.size() != min_delays.size() || topics_.size() != max_delays.size()) { - throw std::invalid_argument("Topics and min/max_delays must have same number of arguments"); - } - monitor_configured_only_ = get_parameter(MONITOR_CONFIGURED_ONLY_PARAM_NAME).as_bool(); - diag_prefix_ = get_parameter(DIAG_PREFIX_PARAM_NAME).as_string(); - - RCLCPP_DEBUG( - get_logger(), "Done configuring for %ld topics, config only: %d", topic_diag_map_.size(), - monitor_configured_only_); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -HeaderTopicMonitor::on_activate(const rclcpp_lifecycle::State &) -{ - RCLCPP_DEBUG(get_logger(), "Activating"); - updater_ = std::make_shared(this); - char HOSTNAME[1000]; - gethostname(HOSTNAME, 1000); - updater_->setHardwareID(std::string(HOSTNAME)); - for (size_t i = 0; i < topics_.size(); ++i) { - topic_monitor::TimeStampStatusParam param(min_delays[i], max_delays[i]); - auto diag = std::make_shared( - param, get_prefixed_name(topics_[i]), get_clock()); - topic_diag_map_[topics_[i]] = diag; - updater_->add(*diag); - } - // check existing topics - try { - update_topic_subscriptions(); - } catch (const std::exception & ex) { - RCLCPP_ERROR(this->get_logger(), "Failure to update subscriptions: %s", ex.what()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - } - timer_ = create_wall_timer(1s, [this]() {this->update_topic_subscriptions();}); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -HeaderTopicMonitor::on_deactivate(const rclcpp_lifecycle::State &) -{ - RCLCPP_DEBUG(get_logger(), "Deactivating"); - timer_.reset(); - subscribed_topics_.clear(); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -bool HeaderTopicMonitor::skip_topic(const std::string & topic_name) -{ - // skip if we already subscribed - if (subscribed_topics_.find(topic_name) != subscribed_topics_.end()) { - RCLCPP_DEBUG(get_logger(), "Already subscribed to %s, skipping", topic_name.c_str()); - return true; - } - // if it's configured, we monitor it - if (std::find(topics_.begin(), topics_.end(), topic_name) != topics_.end()) { - RCLCPP_DEBUG(get_logger(), "Topic %s is configured, no skip", topic_name.c_str()); - return false; - } - // skip if we have seen this before and ignored it - if (known_topics_.find(topic_name) != known_topics_.end()) { - return true; - } - // check for matches against internal topic names - for (const auto & re : hidden_topics_) { - if (std::regex_match(topic_name, re)) { - known_topics_.insert(topic_name); - RCLCPP_DEBUG(get_logger(), "Topic %s matches ignore list, skip it", topic_name.c_str()); - return true; - } - } - return monitor_configured_only_; -} - -void HeaderTopicMonitor::update_topic_subscriptions() -{ - RCLCPP_DEBUG( - get_logger(), "Examining topic list for changes, currently monitoring %ld topics", - subscribed_topics_.size()); - const auto topics = this->get_topic_names_and_types(); - for (const auto & e : topics) { - const std::string topic_name = e.first; - if (skip_topic(topic_name)) { - continue; - } - // subscribe with statistics enabled - RCLCPP_DEBUG(get_logger(), "Starting to monitor topic %s", topic_name.c_str()); - std::shared_ptr sub = this->create_generic_subscription( - e.first, *e.second.begin(), rclcpp::QoS(10), - [this, topic_name](std::shared_ptr msg) { - this->topic_cb(topic_name, msg); - }); - subscribed_topics_[topic_name] = sub; - } - RCLCPP_DEBUG( - get_logger(), "Done updating topic list. Found %ld, hidden %ld, now monitoring %ld", - topics.size(), - known_topics_.size(), subscribed_topics_.size()); -} -} // namespace diagnostic_topic_monitor - -#include // NOLINT: upstream -RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_topic_monitor::HeaderTopicMonitor) diff --git a/diagnostic_topic_monitor/src/topic_age_monitor_component.cpp b/diagnostic_topic_monitor/src/topic_age_monitor_component.cpp new file mode 100644 index 000000000..35119c5f7 --- /dev/null +++ b/diagnostic_topic_monitor/src/topic_age_monitor_component.cpp @@ -0,0 +1,45 @@ +#include "diagnostic_topic_monitor/topic_age_monitor.hpp" + +namespace diagnostic_topic_monitor +{ + +TopicAgeMonitor::TopicAgeMonitor(const std::string & node_name, rclcpp::NodeOptions options) +: GenericTopicMonitor(node_name, options) +{ +} + +TopicAgeMonitor::TopicAgeMonitor(rclcpp::NodeOptions options) +: GenericTopicMonitor("topic_age_monitor", options) +{ +} + +diagnostic_updater::TimeStampStatusParam diagnostic_topic_monitor::TopicAgeMonitor::parse_params( + const int index) +{ + return diagnostic_updater::TimeStampStatusParam(min_values_[index], max_values_[index]); +} + +void TopicAgeMonitor::topic_cb( + const std::string & topic_name, const std::shared_ptr & msg) +{ + std_msgs::msg::Header header; + const auto diag = topic_diag_map_.find(topic_name); + try { + _header_serializer.deserialize_message(msg.get(), &header); + } catch (const std::exception & e) { + RCLCPP_WARN( + get_logger(), "Failed to deserialize message from topic %s, skipping it", topic_name.c_str()); + return; + } + + if (diag != topic_diag_map_.end()) { + diag->second->tick(header.stamp); + } else { + RCLCPP_WARN(get_logger(), "No diagnostic found for topic %s", topic_name.c_str()); + } +} + +} // namespace diagnostic_topic_monitor + +#include // NOLINT: upstream +RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_topic_monitor::TopicAgeMonitor) diff --git a/diagnostic_topic_monitor/src/topic_frequency_monitor_component.cpp b/diagnostic_topic_monitor/src/topic_frequency_monitor_component.cpp new file mode 100644 index 000000000..a0ac36b87 --- /dev/null +++ b/diagnostic_topic_monitor/src/topic_frequency_monitor_component.cpp @@ -0,0 +1,74 @@ +#include "diagnostic_topic_monitor/topic_frquency_monitor.hpp" + +namespace diagnostic_topic_monitor +{ + +TopicFrequencyMonitor::TopicFrequencyMonitor( + const std::string & node_name, rclcpp::NodeOptions options) +: GenericTopicMonitor(node_name, options) +{ +} + +TopicFrequencyMonitor::TopicFrequencyMonitor(rclcpp::NodeOptions options) +: GenericTopicMonitor("topic_frequency_monitor", options) +{ +} + +diagnostic_updater::FrequencyStatusParam +diagnostic_topic_monitor::TopicFrequencyMonitor::parse_params(const int index) +{ + return diagnostic_updater::FrequencyStatusParam(&min_values_[index], &max_values_[index]); +} + +void TopicFrequencyMonitor::topic_cb( + const std::string & topic_name, const std::shared_ptr &) +{ + const auto diag = topic_diag_map_.find(topic_name); + if (diag != topic_diag_map_.end()) { + diag->second->tick(); + } else { + const auto fallback_diag = fallback_topic_diag_map_.find(topic_name); + if (fallback_diag != fallback_topic_diag_map_.end()) { + fallback_diag->second->tick(); + } else { + RCLCPP_WARN(get_logger(), "No diagnostic found for topic %s", topic_name.c_str()); + } + } +} + +void diagnostic_topic_monitor::TopicFrequencyMonitor::update_topic_subscriptions() +{ + RCLCPP_DEBUG( + get_logger(), "Examining topic list for changes, currently monitoring %ld topics", + subscribed_topics_.size()); + const auto topics = this->get_topic_names_and_types(); + for (const auto & e : topics) { + const std::string topic_name = e.first; + if (skip_topic(topic_name)) { + continue; + } + // subscribe with statistics enabled + RCLCPP_DEBUG(get_logger(), "Starting to monitor topic %s", topic_name.c_str()); + std::shared_ptr sub = this->create_generic_subscription( + e.first, *e.second.begin(), rclcpp::QoS(10), + [this, topic_name](std::shared_ptr msg) { + this->topic_cb(topic_name, msg); + }); + subscribed_topics_[topic_name] = sub; + // add a default diagnostic if none is configured + if (topic_diag_map_.find(topic_name) == topic_diag_map_.end()) { + auto fallback_diag = + std::make_shared(get_prefixed_name(topic_name), get_clock()); + fallback_topic_diag_map_[topic_name] = fallback_diag; + updater_->add(*fallback_diag); + } + } + RCLCPP_DEBUG( + get_logger(), "Done updating topic list. Found %ld, hidden %ld, now monitoring %ld", + topics.size(), known_topics_.size(), subscribed_topics_.size()); +} + +} // namespace diagnostic_topic_monitor + +#include // NOLINT: upstream +RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_topic_monitor::TopicFrequencyMonitor) diff --git a/diagnostic_topic_monitor/src/topic_monitor_component.cpp b/diagnostic_topic_monitor/src/topic_monitor_component.cpp deleted file mode 100644 index c5cbb5725..000000000 --- a/diagnostic_topic_monitor/src/topic_monitor_component.cpp +++ /dev/null @@ -1,283 +0,0 @@ -// Copyright 2024 Robert Bosch GmbH and its subsidiaries -// All rights reserved, also regarding any disposal, exploitation, -// reproduction, editing, distribution, as well as in the event of applications -// for industrial property rights. - -#include -#include -#include -#include -#include -#include -#include -#include -#include // NOLINT: upstream -#include // NOLINT: upstream -#include // NOLINT: upstream -#include // NOLINT: upstream -#include // NOLINT: upstream -#include "update_functions.hpp" - -using namespace std::chrono_literals; - -namespace -{ -// very simply DiagnosticTask that just checks whether there has been _any_ -// kind of activity during the last period -class ActivityDiagnosticTask : public diagnostic_updater::DiagnosticTask -{ -public: - ActivityDiagnosticTask(const std::string & name, rclcpp::Clock::SharedPtr clock) - : diagnostic_updater::DiagnosticTask(name), clock_(clock), last_tick_(0) - { - } - virtual void tick() - { - last_tick_ = clock_->now(); - } - void run(diagnostic_updater::DiagnosticStatusWrapper & stat) override - { - stat.name = getName(); - if (last_tick_.seconds() == 0) { - stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - stat.message = "No data received, yet"; - stat.addf("period", "%f", std::numeric_limits::max()); - } else { - const auto currentTime = clock_->now(); - // more than 1s without tick is considered warning - const auto elapsed = currentTime - last_tick_; - stat.addf("period", "%f", elapsed.seconds()); - if (elapsed <= rclcpp::Duration(1.1s)) { - stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - stat.message = "OK"; - } else { - stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - stat.message = "Last data received more than 1s ago"; - } - } - } - -private: - rclcpp::Clock::SharedPtr clock_; - rclcpp::Time last_tick_; -}; -} // namespace - -namespace diagnostic_topic_monitor -{ -constexpr const char * TOPICS_PARAM_NAME = "topics"; -constexpr const char * MIN_FREQS_PARAM_NAME = "min_freqs"; -constexpr const char * MAX_FREQS_PARAM_NAME = "max_freqs"; -constexpr const char * MONITOR_CONFIGURED_ONLY_PARAM_NAME = "monitor_configured_only"; -constexpr const char * DIAG_PREFIX_PARAM_NAME = "diag_prefix"; - -using namespace std::chrono_literals; -class TopicMonitor : public rclcpp_lifecycle::LifecycleNode -{ -public: - explicit TopicMonitor(const std::string & node_name, rclcpp::NodeOptions options) - : rclcpp_lifecycle::LifecycleNode(node_name, options.allow_undeclared_parameters(true)) - { - auto desc = rcl_interfaces::msg::ParameterDescriptor{}; - desc.description = "Topics to specify expectations for"; - desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; - declare_parameter(TOPICS_PARAM_NAME, std::vector(), desc); - auto desc_min = rcl_interfaces::msg::ParameterDescriptor{}; - desc_min.description = "Minimum frequency for topic from 'topics'"; - desc_min.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY; - declare_parameter(MIN_FREQS_PARAM_NAME, std::vector(), desc_min); - auto desc_max = rcl_interfaces::msg::ParameterDescriptor{}; - desc_max.description = "Maximum frequency for topic from 'topics'"; - desc_max.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY; - declare_parameter(MAX_FREQS_PARAM_NAME, std::vector(), desc_max); - // by default, we include all topics - declare_parameter(MONITOR_CONFIGURED_ONLY_PARAM_NAME, false); - // prefix for diagnostics (for use with modules) - declare_parameter(DIAG_PREFIX_PARAM_NAME, ""); - } - explicit TopicMonitor(const rclcpp::NodeOptions & options) - : TopicMonitor("topic_monitor", options) - { - } - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State &) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State & state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State & state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State &) override - { - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_shutdown(const rclcpp_lifecycle::State &) override - { - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - void topic_cb(const std::string & topic_name, const std::shared_ptr &) - { - const auto diag = topic_diag_map_.find(topic_name); - if (diag != topic_diag_map_.end()) { - diag->second->tick(); - } else { - const auto fallback_diag = fallback_topic_diag_map_.find(topic_name); - if (fallback_diag != fallback_topic_diag_map_.end()) { - fallback_diag->second->tick(); - } else { - RCLCPP_WARN(get_logger(), "No diagnostic found for topic %s", topic_name.c_str()); - } - } - } - -protected: - void update_topic_subscriptions(); - bool skip_topic(const std::string & name); - std::string get_prefixed_name(const std::string & topic_name) const; - -private: - std::vector hidden_topics_{std::regex("^/rosout$"), std::regex(".*/parameter_events"), - std::regex("^/diagnostics$"), std::regex(".*/transition_event"), std::regex("^/clock")}; - std::shared_ptr timer_; - std::shared_ptr updater_; - std::unordered_map> subscribed_topics_; - std::set known_topics_; - std::unordered_map> topic_diag_map_; - std::unordered_map> fallback_topic_diag_map_; - std::vector topics_; - std::vector min_freqs; - std::vector max_freqs; - bool monitor_configured_only_{true}; - std::string diag_prefix_; -}; - -std::string TopicMonitor::get_prefixed_name(const std::string & topic_name) const -{ - if (topic_name.find(diag_prefix_) == 0) { - return topic_name; - } - return diag_prefix_ + std::string("/") + topic_name; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -TopicMonitor::on_configure(const rclcpp_lifecycle::State &) -{ - RCLCPP_DEBUG(get_logger(), "Configuring"); - // configure the diagnostics - topics_ = get_parameter(TOPICS_PARAM_NAME).as_string_array(); - min_freqs = get_parameter(MIN_FREQS_PARAM_NAME).as_double_array(); - max_freqs = get_parameter(MAX_FREQS_PARAM_NAME).as_double_array(); - if (topics_.size() != min_freqs.size() || topics_.size() != max_freqs.size()) { - throw std::invalid_argument("Topics and min/max_freqs must have same number of arguments"); - } - monitor_configured_only_ = get_parameter(MONITOR_CONFIGURED_ONLY_PARAM_NAME).as_bool(); - diag_prefix_ = get_parameter(DIAG_PREFIX_PARAM_NAME).as_string(); - - RCLCPP_DEBUG( - get_logger(), "Done configuring for %ld topics, config only: %d", topic_diag_map_.size(), - monitor_configured_only_); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -TopicMonitor::on_activate(const rclcpp_lifecycle::State &) -{ - RCLCPP_DEBUG(get_logger(), "Activating"); - updater_ = std::make_shared(this); - char HOSTNAME[1000]; - gethostname(HOSTNAME, 1000); - updater_->setHardwareID(std::string(HOSTNAME)); - for (size_t i = 0; i < topics_.size(); ++i) { - topic_monitor::FrequencyStatusParam param(&(min_freqs[i]), &(max_freqs[i])); - auto diag = std::make_shared( - param, get_prefixed_name(topics_[i]), get_clock()); - topic_diag_map_[topics_[i]] = diag; - updater_->add(*diag); - } - // check existing topics - try { - update_topic_subscriptions(); - } catch (const std::exception & ex) { - RCLCPP_ERROR(this->get_logger(), "Failure to update subscriptions: %s", ex.what()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - } - timer_ = create_wall_timer(1s, [this]() {this->update_topic_subscriptions();}); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -TopicMonitor::on_deactivate(const rclcpp_lifecycle::State &) -{ - RCLCPP_DEBUG(get_logger(), "Deactivating"); - timer_.reset(); - subscribed_topics_.clear(); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -bool TopicMonitor::skip_topic(const std::string & topic_name) -{ - // skip if we already subscribed - if (subscribed_topics_.find(topic_name) != subscribed_topics_.end()) { - RCLCPP_DEBUG(get_logger(), "Already subscribed to %s, skipping", topic_name.c_str()); - return true; - } - // if it's configured, we monitor it - if (std::find(topics_.begin(), topics_.end(), topic_name) != topics_.end()) { - RCLCPP_DEBUG(get_logger(), "Topic %s is configured, no skip", topic_name.c_str()); - return false; - } - // skip if we have seen this before and ignored it - if (known_topics_.find(topic_name) != known_topics_.end()) { - return true; - } - // check for matches against internal topic names - for (const auto & re : hidden_topics_) { - if (std::regex_match(topic_name, re)) { - known_topics_.insert(topic_name); - RCLCPP_DEBUG(get_logger(), "Topic %s matches ignore list, skip it", topic_name.c_str()); - return true; - } - } - return monitor_configured_only_; -} - -void TopicMonitor::update_topic_subscriptions() -{ - RCLCPP_DEBUG( - get_logger(), "Examining topic list for changes, currently monitoring %ld topics", - subscribed_topics_.size()); - const auto topics = this->get_topic_names_and_types(); - for (const auto & e : topics) { - const std::string topic_name = e.first; - if (skip_topic(topic_name)) { - continue; - } - // subscribe with statistics enabled - RCLCPP_DEBUG(get_logger(), "Starting to monitor topic %s", topic_name.c_str()); - std::shared_ptr sub = this->create_generic_subscription( - e.first, *e.second.begin(), rclcpp::QoS(10), - [this, topic_name](std::shared_ptr msg) { - this->topic_cb(topic_name, msg); - }); - subscribed_topics_[topic_name] = sub; - // add a default diagnostic if none is configured - if (topic_diag_map_.find(topic_name) == topic_diag_map_.end()) { - auto fallback_diag = std::make_shared( - get_prefixed_name( - topic_name), get_clock()); - fallback_topic_diag_map_[topic_name] = fallback_diag; - updater_->add(*fallback_diag); - } - } - RCLCPP_DEBUG( - get_logger(), "Done updating topic list. Found %ld, hidden %ld, now monitoring %ld", - topics.size(), - known_topics_.size(), subscribed_topics_.size()); -} -} // namespace diagnostic_topic_monitor - -#include // NOLINT: upstream -RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_topic_monitor::TopicMonitor) diff --git a/diagnostic_topic_monitor/test/config/topic_age_monitor.yaml b/diagnostic_topic_monitor/test/config/topic_age_monitor.yaml new file mode 100644 index 000000000..850c1fbc0 --- /dev/null +++ b/diagnostic_topic_monitor/test/config/topic_age_monitor.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + monitor_configured_only: True + topics: + - /dummy_header_topic + min_values: + - -0.01 + max_values: + - 0.2 diff --git a/diagnostic_topic_monitor/test/config/topic_frequency_monitor.yaml b/diagnostic_topic_monitor/test/config/topic_frequency_monitor.yaml new file mode 100644 index 000000000..37236c4f3 --- /dev/null +++ b/diagnostic_topic_monitor/test/config/topic_frequency_monitor.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + monitor_configured_only: True + topics: + - /dummy_string_topic1 + - /dummy_string_topic2 + min_values: + - 5.0 + - 5.0 + max_values: + - 15.0 + - 15.0 diff --git a/diagnostic_topic_monitor/test/dummy_publishers.py b/diagnostic_topic_monitor/test/dummy_publishers.py new file mode 100755 index 000000000..e6087c7a4 --- /dev/null +++ b/diagnostic_topic_monitor/test/dummy_publishers.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +from sensor_msgs.msg import CameraInfo + +class DummyPublisherNode(Node): + def __init__(self): + super().__init__('dummy_publisher_node') + + # Publishers + self.string_pub1 = self.create_publisher(String, '/dummy_string_topic1', 10) + self.string_pub2 = self.create_publisher(String, '/dummy_string_topic2', 10) + self.string_pub2 = self.create_publisher(String, '/dummy_string_topic3', 10) + self.header_pub = self.create_publisher(CameraInfo, "/dummy_header_topic", 10) + + # Timer to publish at 10Hz + self.timer = self.create_timer(0.1, self.publish_messages) + + self.get_logger().info('DummyPublisherNode has been started.') + + def publish_messages(self): + # Publish to /string_topic1 + msg1 = String() + msg1.data = 'Hello from string_topic1' + self.string_pub1.publish(msg1) + + # Publish to /string_topic2 + msg2 = String() + msg2.data = 'Hello from string_topic2' + self.string_pub2.publish(msg2) + + # Publish to /string_topic3 + msg3 = String() + msg3.data = 'Hello from string_topic3' + self.string_pub2.publish(msg3) + + # Publish to /header_topic + header_msg = CameraInfo() + header_msg.header.stamp = self.get_clock().now().to_msg() + header_msg.header.frame_id = "dummy_frame" + self.header_pub.publish(header_msg) + + self.get_logger().debug('Published dummy messages to all topics.') + + +def main(args=None): + rclpy.init(args=args) + node = DummyPublisherNode() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + node.get_logger().info('Shutting down DummyPublisherNode...') + + +if __name__ == '__main__': + main() diff --git a/diagnostic_topic_monitor/test/test_topic_age_monitor.py b/diagnostic_topic_monitor/test/test_topic_age_monitor.py new file mode 100755 index 000000000..4512d1a4f --- /dev/null +++ b/diagnostic_topic_monitor/test/test_topic_age_monitor.py @@ -0,0 +1,222 @@ +#! /usr/bin/env python3 + +# Copyright 2024 Robert Bosch GmbH and its subsidiaries +# +# All rights reserved, also regarding any disposal, exploitation, reproduction, +# editing, distribution, as well as in the event of applications for industrial +# property rights. + +# # +# # Copyright 2019 Open Source Robotics Foundation, Inc. +# # +# # Licensed under the Apache License, Version 2.0 (the "License"); +# # you may not use this file except in compliance with the License. +# # You may obtain a copy of the License at +# # +# # http://www.apache.org/licenses/LICENSE-2.0 +# # +# # Unless required by applicable law or agreed to in writing, software +# # distributed under the License is distributed on an "AS IS" BASIS, +# # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# # See the License for the specific language governing permissions and +# # limitations under the License. + +import time +import unittest +import pytest + +import launch +from launch.actions import EmitEvent, RegisterEventHandler, LogInfo, SetEnvironmentVariable +import launch.event_handlers.on_process_start + +from launch_ros.actions import LifecycleNode, Node +from launch_ros.events.lifecycle import ChangeState +import launch_ros.events.lifecycle + +import launch_testing +import launch_testing.actions +import launch_testing.asserts + +from lifecycle_msgs.msg import Transition +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus + +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + +import rclpy +from rclpy.time import Time, Duration + +# Helper functions to activate the lifecycle monitor nodes +def create_change_state(target, target_state): + return EmitEvent( + event=ChangeState( + lifecycle_node_matcher=launch.events.matches_action(target), + transition_id=target_state, + ) + ) + + +def create_register_configure(target_action): + return RegisterEventHandler( + launch.event_handlers.on_process_start.OnProcessStart( + target_action=target_action, + on_start=[ + LogInfo(msg=f"Emitting configure event for {target_action}"), + create_change_state( + target_action, Transition.TRANSITION_CONFIGURE + ) + ], + ) + ) + + +def create_register_activate(target_action): + return RegisterEventHandler( + launch_ros.event_handlers.OnStateTransition( + target_lifecycle_node=target_action, + start_state="configuring", + goal_state="inactive", + entities=[ + LogInfo(msg=f"Emitting activate event for {target_action}"), + create_change_state( + target_action, Transition.TRANSITION_ACTIVATE + ) + ], + ) + ) + + +CONFIG_MONITOR_NAME = "monitor_configured_topics_node" + + +@pytest.mark.launch_test +def generate_test_description(): + # Node that publishes the topics we want to monitor + talker_node = Node( + package="diagnostic_topic_monitor", + executable="dummy_publishers.py", + output="log", + name="talker", + arguments=["--ros-args", "--log-level", "talker:=warn"], + ) + # Monitor with configuration file + monitor_config_node = LifecycleNode( + package="diagnostic_topic_monitor", + executable="topic_age_monitor", + name=CONFIG_MONITOR_NAME, + output="both", + namespace="", + parameters=[ + PathJoinSubstitution( + [ + FindPackageShare("diagnostic_topic_monitor"), + "test", + "config", + "topic_age_monitor.yaml", + ] + ), + ], + arguments=["--ros-args", "--log-level", "monitor_configured_topics_node:=INFO"], + ) + + return launch.LaunchDescription( + [ + SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"), + talker_node, + monitor_config_node, + # Right after the monitor starts, make it take the 'configure' transition. + create_register_configure(monitor_config_node), + # When the monitor reaches the 'inactive' state, 'activate'. + create_register_activate(monitor_config_node), + # When the monitor node reaches the 'active' state, we're ready for testing + RegisterEventHandler( + launch_ros.event_handlers.OnStateTransition( + target_lifecycle_node=monitor_config_node, + start_state="activating", + goal_state="active", + entities=[ + LogInfo(msg="Monitor reached active state"), + launch_testing.actions.ReadyToTest(), + ], + ) + ), + ] + ) + + +class TestMonitor(unittest.TestCase): + TIMEOUT = 30 + + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node("test_topic_age_monitor_node") + self.log = self.node.get_logger() + self.sub = self.node.create_subscription( + DiagnosticArray, "/diagnostics", self.stat_cb, 10 + ) + self.pub_count = self.node.count_publishers("/diagnostics") + self.log.info( + f"Number of publishers for /diagnostics: {self.pub_count}. Listening for messages..." + ) + self.age_messages = [] # Store messages from configured age monitor + + start_time = time.time() + while len(self.age_messages) < 3: + rclpy.spin_once(self.node, timeout_sec=1.0) + self.log.debug(f"Got {len(self.age_messages)} age msgs msgs") + if (time.time() - start_time) > self.TIMEOUT: + self.fail("Timed out waiting for message in /diagnostics topic") + self.log.info(f"Stored {len(self.age_messages)} age msgs") + + def tearDown(self): + self.node.destroy_node() + del self.node + + def stat_cb(self, msg): + """Store message for future processing.""" + if len(msg.status) == 0: + return + if CONFIG_MONITOR_NAME in msg.status[0].name: + self.age_messages.append(msg) + + def test_diag_msg(self): + """Check that diagnostics messages contain the right content.""" + last_msg = self.age_messages.pop() + # header + current_time = self.node.get_clock().now() + header_time = Time.from_msg(last_msg.header.stamp) + self.assertLess(current_time - header_time, Duration(seconds=0.1)) + last_status = last_msg.status[0] + # status should be OK + self.assertEqual(last_status.level, DiagnosticStatus.OK) + + def test_age_diag_msg(self): + """Check that the age diagnostic works.""" + last_msg = self.age_messages.pop() + self.assertTrue(len(last_msg.status) > 0) + status = last_msg.status[0] + # check some fields for present/content + self.assertTrue(CONFIG_MONITOR_NAME in status.name) + keys = [kv.key for kv in status.values] + self.assertTrue("Earliest timestamp delay:" in keys) + + def test_ignore_unconfigured(self): + """Check that we ignore the topic we don't monitor.""" + last_msg = self.age_messages.pop() + self.assertEqual(len(last_msg.status), 1) # We monitor 1 topic only + names = [status.name for status in last_msg.status] + # This topic should be monitored + self.assertIn(f"{CONFIG_MONITOR_NAME}: /dummy_header_topic", names, f"{names}") + # This topic should not be monitored + self.assertNotIn( + f"{CONFIG_MONITOR_NAME}: /dummy_string_topic1", + names, + f"{names}", + ) diff --git a/diagnostic_topic_monitor/test/test_topic_monitor.py b/diagnostic_topic_monitor/test/test_topic_frequency_monitor.py similarity index 68% rename from diagnostic_topic_monitor/test/test_topic_monitor.py rename to diagnostic_topic_monitor/test/test_topic_frequency_monitor.py index 24ccbea0c..ca848acbb 100755 --- a/diagnostic_topic_monitor/test/test_topic_monitor.py +++ b/diagnostic_topic_monitor/test/test_topic_frequency_monitor.py @@ -40,10 +40,13 @@ from lifecycle_msgs.msg import Transition from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + import rclpy from rclpy.time import Time, Duration - +# Helper functions to activate the lifecycle monitor nodes def create_change_state(target, target_state): return EmitEvent( event=ChangeState( @@ -83,66 +86,65 @@ def create_register_activate(target_action): ) -ALL_MONITOR_NAME = "all_monitor" -FREQ_MONITOR_NAME = "frequency_monitor" +ALL_MONITOR_NAME = "monitor_all_topics_node" +CONFIG_MONITOR_NAME = "monitor_configured_topics_node" @pytest.mark.launch_test def generate_test_description(): - monitor_node = LifecycleNode( - package="rob_topic_monitor", - executable="topic_monitor", + # Node that publishes the topics we want to monitor + talker_node = Node( + package="diagnostic_topic_monitor", + executable="dummy_publishers.py", + output="log", + name="talker", + arguments=["--ros-args", "--log-level", "talker:=warn"], + ) + # Un-configured frequency monitor that checks all topics + monitor_all_node = LifecycleNode( + package="diagnostic_topic_monitor", + executable="topic_frequency_monitor", name=ALL_MONITOR_NAME, output="both", namespace="", - arguments=['--ros-args', '--log-level', 'all_monitor:=INFO'] + arguments=["--ros-args", "--log-level", "all_monitor:=INFO"], ) - frequency_monitor_node = LifecycleNode( - package="rob_topic_monitor", - executable="topic_monitor", - name=FREQ_MONITOR_NAME, + # Frequency monitor with configuration file + monitor_config_node = LifecycleNode( + package="diagnostic_topic_monitor", + executable="topic_frequency_monitor", + name=CONFIG_MONITOR_NAME, output="both", namespace="", - parameters=[{"topics": ["/topic"], - 'min_freqs': [1.8], - 'max_freqs': [2.1], - 'diag_prefix': 'freq', - 'monitor_configured_only': True, - 'diagnostic_updater.use_fqn': True}], - arguments=['--ros-args', '--log-level', 'frequency_monitor:=INFO'] - ) - talker_node = Node( - package="examples_rclcpp_minimal_publisher", - executable="publisher_lambda", - output="log", - name="talker", - arguments=['--ros-args', '--log-level', 'talker:=warn'] - ) - talker2_node = Node( - package="examples_rclcpp_minimal_publisher", - executable="publisher_lambda", - name="talker2", - output="log", - remappings=[("topic", "ignore_topic")], - arguments=['--ros-args', '--log-level', 'talker2:=warn'] + parameters=[ + PathJoinSubstitution( + [ + FindPackageShare("diagnostic_topic_monitor"), + "test", + "config", + "topic_frequency_monitor.yaml", + ] + ), + ], + arguments=["--ros-args", "--log-level", "monitor_configured_topics_node:=INFO"], ) + return launch.LaunchDescription( [ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"), talker_node, - talker2_node, - monitor_node, - frequency_monitor_node, + monitor_all_node, + monitor_config_node, # Right after the monitor starts, make it take the 'configure' transition. - create_register_configure(monitor_node), - create_register_configure(frequency_monitor_node), + create_register_configure(monitor_all_node), + create_register_configure(monitor_config_node), # When the monitor reaches the 'inactive' state, 'activate'. - create_register_activate(monitor_node), - create_register_activate(frequency_monitor_node), + create_register_activate(monitor_all_node), + create_register_activate(monitor_config_node), # When the monitor node reaches the 'active' state, we're ready for testing RegisterEventHandler( launch_ros.event_handlers.OnStateTransition( - target_lifecycle_node=monitor_node, + target_lifecycle_node=monitor_all_node, start_state="activating", goal_state="active", entities=[ @@ -155,7 +157,6 @@ def generate_test_description(): ) -@pytest.mark.skip(reason="Brittle on CI as long as we don't have proper resource constraints") class TestMonitor(unittest.TestCase): TIMEOUT = 30 @@ -168,13 +169,15 @@ def tearDownClass(cls): rclpy.shutdown() def setUp(self): - self.node = rclpy.create_node("test_topic_monitor_node") + self.node = rclpy.create_node("test_topic_frequency_monitor_node") self.log = self.node.get_logger() self.sub = self.node.create_subscription( DiagnosticArray, "/diagnostics", self.stat_cb, 10 ) self.pub_count = self.node.count_publishers("/diagnostics") - self.log.info(f"Number of publishers for /diagnostics: {self.pub_count}") + self.log.info( + f"Number of publishers for /diagnostics: {self.pub_count}. Listening for messages..." + ) self.messages = [] self.freq_messages = [] @@ -194,7 +197,7 @@ def stat_cb(self, msg): """Store message for future processing.""" if len(msg.status) == 0: return - if FREQ_MONITOR_NAME in msg.status[0].name: + if CONFIG_MONITOR_NAME in msg.status[0].name: self.freq_messages.append(msg) else: self.messages.append(msg) @@ -209,17 +212,12 @@ def test_diag_msg(self): last_status = last_msg.status[0] # status should be OK self.assertEqual(last_status.level, DiagnosticStatus.OK) - # period estimate should be 4 Hz keys = [value.key for value in last_status.values] self.assertTrue("period" in keys) - # disabled until we can get deterministic measurement on CI - # self.assertAlmostEqual( - # float(last_status.values[keys.index("period")].value), 0.25, delta=0.02) - # check that the two topics of this test are present - # (a bit more complex, since on CI other topics may be running in parallel) names = [status.name for status in last_msg.status] - self.assertIn(f"{ALL_MONITOR_NAME}: /topic", names, f"{names}") - self.assertIn(f"{ALL_MONITOR_NAME}: /ignore_topic", names, f"{names}") + # The all_topics monitor should have all topics + self.assertIn(f"{ALL_MONITOR_NAME}: /dummy_header_topic", names, f"{names}") + self.assertIn(f"{ALL_MONITOR_NAME}: /dummy_string_topic1", names, f"{names}") def test_frequency_diag_msg(self): """Check that the frequency diagnostic works.""" @@ -228,14 +226,11 @@ def test_frequency_diag_msg(self): self.assertTrue(len(last_msg.status) > 0) status = last_msg.status[0] # check some fields for present/content - self.assertTrue(FREQ_MONITOR_NAME in status.name) + self.assertTrue(CONFIG_MONITOR_NAME in status.name) keys = [kv.key for kv in status.values] self.assertIn("Actual frequency (Hz)", keys) - # disabled until we can get deterministic measurement on CI - # freq = [kv.value for kv in status.values if kv.key == "Actual frequency (Hz)"][0] - # self.assertAlmostEqual(float(freq), 2.0, delta=0.02) def test_ignore_unconfigured(self): """Check that we ignore the topic we don't monitor.""" last_msg = self.freq_messages.pop() - self.assertEqual(len(last_msg.status), 1) + self.assertEqual(len(last_msg.status), 2) # We monitor 2 topics From a20910c2d3cd7e834384b0af0d188d768bc7d17b Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/ASD1)" Date: Thu, 15 May 2025 10:37:07 +0200 Subject: [PATCH 4/6] Fix license tag --- diagnostic_topic_monitor/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diagnostic_topic_monitor/package.xml b/diagnostic_topic_monitor/package.xml index af5116c6a..811ca5301 100644 --- a/diagnostic_topic_monitor/package.xml +++ b/diagnostic_topic_monitor/package.xml @@ -7,7 +7,7 @@ Ingo Lütkebohle Ingo Lütkebohle Tejas Kumar Shastha - BIOSv4 + BSD-3-Clause ament_cmake From 555b69f2742c4e9f447e812dad83fa0f8d8f9422 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/ASD1)" Date: Thu, 15 May 2025 11:55:09 +0200 Subject: [PATCH 5/6] Add new package to lint and test --- .github/workflows/lint.yaml | 1 + .github/workflows/test.yaml | 1 + 2 files changed, 2 insertions(+) diff --git a/.github/workflows/lint.yaml b/.github/workflows/lint.yaml index 28bcfcb2e..2b38f779a 100644 --- a/.github/workflows/lint.yaml +++ b/.github/workflows/lint.yaml @@ -43,6 +43,7 @@ jobs: diagnostic_common_diagnostics diagnostic_remote_logging diagnostic_updater + diagnostic_topic_monitor self_test check_licenses: diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml index d92ce5f4c..1a6ab123f 100644 --- a/.github/workflows/test.yaml +++ b/.github/workflows/test.yaml @@ -18,6 +18,7 @@ jobs: diagnostic_aggregator, diagnostic_common_diagnostics, diagnostic_remote_logging, + diagnostic_topic_monitor, diagnostic_updater, self_test, ] From a8c558108fc3c71feeb63c2c59665e3eea6ba672 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/ASD1)" Date: Thu, 15 May 2025 12:12:54 +0200 Subject: [PATCH 6/6] Fix copyright notices --- .../activity_diagnostic_task.hpp | 4 +++- .../generic_topic_monitor.hpp | 4 ++++ .../topic_age_monitor.hpp | 4 ++++ ...onitor.hpp => topic_frequency_monitor.hpp} | 4 ++++ .../src/topic_frequency_monitor_component.cpp | 6 +++++- .../test/dummy_publishers.py | 5 +++++ .../test/test_topic_age_monitor.py | 21 ++----------------- .../test/test_topic_frequency_monitor.py | 21 ++----------------- 8 files changed, 29 insertions(+), 40 deletions(-) rename diagnostic_topic_monitor/include/diagnostic_topic_monitor/{topic_frquency_monitor.hpp => topic_frequency_monitor.hpp} (91%) diff --git a/diagnostic_topic_monitor/include/diagnostic_topic_monitor/activity_diagnostic_task.hpp b/diagnostic_topic_monitor/include/diagnostic_topic_monitor/activity_diagnostic_task.hpp index a1ea5152a..e88dc2ba5 100644 --- a/diagnostic_topic_monitor/include/diagnostic_topic_monitor/activity_diagnostic_task.hpp +++ b/diagnostic_topic_monitor/include/diagnostic_topic_monitor/activity_diagnostic_task.hpp @@ -1,4 +1,6 @@ -// TODO: Insert copyright +// Copyright (c) 2024, 2025 Robert Bosch GmbH +// +// See the top-level LICENSE file for licensing terms. #ifndef ACTIVITY_DIAGNOSTIC_TASK_HPP #define ACTIVITY_DIAGNOSTIC_TASK_HPP diff --git a/diagnostic_topic_monitor/include/diagnostic_topic_monitor/generic_topic_monitor.hpp b/diagnostic_topic_monitor/include/diagnostic_topic_monitor/generic_topic_monitor.hpp index f778655cd..3a55d7c73 100644 --- a/diagnostic_topic_monitor/include/diagnostic_topic_monitor/generic_topic_monitor.hpp +++ b/diagnostic_topic_monitor/include/diagnostic_topic_monitor/generic_topic_monitor.hpp @@ -1,3 +1,7 @@ +// Copyright (c) 2024, 2025 Robert Bosch GmbH +// +// See the top-level LICENSE file for licensing terms. + #ifndef GENERIC_TOPIC_MONITOR_HPP #define GENERIC_TOPIC_MONITOR_HPP diff --git a/diagnostic_topic_monitor/include/diagnostic_topic_monitor/topic_age_monitor.hpp b/diagnostic_topic_monitor/include/diagnostic_topic_monitor/topic_age_monitor.hpp index 59e1974c6..d210717fd 100644 --- a/diagnostic_topic_monitor/include/diagnostic_topic_monitor/topic_age_monitor.hpp +++ b/diagnostic_topic_monitor/include/diagnostic_topic_monitor/topic_age_monitor.hpp @@ -1,3 +1,7 @@ +// Copyright (c) 2024, 2025 Robert Bosch GmbH +// +// See the top-level LICENSE file for licensing terms. + #ifndef DIAGNOSTIC_TOPIC_MONITOR_TOPIC_FREQUENCY_MONITOR_HPP #define DIAGNOSTIC_TOPIC_MONITOR_TOPIC_FREQUENCY_MONITOR_HPP diff --git a/diagnostic_topic_monitor/include/diagnostic_topic_monitor/topic_frquency_monitor.hpp b/diagnostic_topic_monitor/include/diagnostic_topic_monitor/topic_frequency_monitor.hpp similarity index 91% rename from diagnostic_topic_monitor/include/diagnostic_topic_monitor/topic_frquency_monitor.hpp rename to diagnostic_topic_monitor/include/diagnostic_topic_monitor/topic_frequency_monitor.hpp index 5251a02ce..17db075ed 100644 --- a/diagnostic_topic_monitor/include/diagnostic_topic_monitor/topic_frquency_monitor.hpp +++ b/diagnostic_topic_monitor/include/diagnostic_topic_monitor/topic_frequency_monitor.hpp @@ -1,3 +1,7 @@ +// Copyright (c) 2024, 2025 Robert Bosch GmbH +// +// See the top-level LICENSE file for licensing terms. + #ifndef DIAGNOSTIC_TOPIC_MONITOR_TOPIC_FREQUENCY_MONITOR_HPP #define DIAGNOSTIC_TOPIC_MONITOR_TOPIC_FREQUENCY_MONITOR_HPP diff --git a/diagnostic_topic_monitor/src/topic_frequency_monitor_component.cpp b/diagnostic_topic_monitor/src/topic_frequency_monitor_component.cpp index a0ac36b87..4295c7725 100644 --- a/diagnostic_topic_monitor/src/topic_frequency_monitor_component.cpp +++ b/diagnostic_topic_monitor/src/topic_frequency_monitor_component.cpp @@ -1,4 +1,8 @@ -#include "diagnostic_topic_monitor/topic_frquency_monitor.hpp" +// Copyright (c) 2024, 2025 Robert Bosch GmbH +// +// See the top-level LICENSE file for licensing terms. + +#include "diagnostic_topic_monitor/topic_frequency_monitor.hpp" namespace diagnostic_topic_monitor { diff --git a/diagnostic_topic_monitor/test/dummy_publishers.py b/diagnostic_topic_monitor/test/dummy_publishers.py index e6087c7a4..8ac1a833d 100755 --- a/diagnostic_topic_monitor/test/dummy_publishers.py +++ b/diagnostic_topic_monitor/test/dummy_publishers.py @@ -1,4 +1,9 @@ #!/usr/bin/env python3 + +# Copyright (c) 2024, 2025 Robert Bosch GmbH +# +# See the top-level LICENSE file for licensing terms. + import rclpy from rclpy.node import Node from std_msgs.msg import String diff --git a/diagnostic_topic_monitor/test/test_topic_age_monitor.py b/diagnostic_topic_monitor/test/test_topic_age_monitor.py index 4512d1a4f..f3bdb35bd 100755 --- a/diagnostic_topic_monitor/test/test_topic_age_monitor.py +++ b/diagnostic_topic_monitor/test/test_topic_age_monitor.py @@ -1,25 +1,8 @@ #! /usr/bin/env python3 -# Copyright 2024 Robert Bosch GmbH and its subsidiaries +# Copyright (c) 2024, 2025 Robert Bosch GmbH # -# All rights reserved, also regarding any disposal, exploitation, reproduction, -# editing, distribution, as well as in the event of applications for industrial -# property rights. - -# # -# # Copyright 2019 Open Source Robotics Foundation, Inc. -# # -# # Licensed under the Apache License, Version 2.0 (the "License"); -# # you may not use this file except in compliance with the License. -# # You may obtain a copy of the License at -# # -# # http://www.apache.org/licenses/LICENSE-2.0 -# # -# # Unless required by applicable law or agreed to in writing, software -# # distributed under the License is distributed on an "AS IS" BASIS, -# # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# # See the License for the specific language governing permissions and -# # limitations under the License. +# See the top-level LICENSE file for licensing terms. import time import unittest diff --git a/diagnostic_topic_monitor/test/test_topic_frequency_monitor.py b/diagnostic_topic_monitor/test/test_topic_frequency_monitor.py index ca848acbb..0072d4405 100755 --- a/diagnostic_topic_monitor/test/test_topic_frequency_monitor.py +++ b/diagnostic_topic_monitor/test/test_topic_frequency_monitor.py @@ -1,25 +1,8 @@ #! /usr/bin/env python3 -# Copyright 2024 Robert Bosch GmbH and its subsidiaries +# Copyright (c) 2024, 2025 Robert Bosch GmbH # -# All rights reserved, also regarding any disposal, exploitation, reproduction, -# editing, distribution, as well as in the event of applications for industrial -# property rights. - -# # -# # Copyright 2019 Open Source Robotics Foundation, Inc. -# # -# # Licensed under the Apache License, Version 2.0 (the "License"); -# # you may not use this file except in compliance with the License. -# # You may obtain a copy of the License at -# # -# # http://www.apache.org/licenses/LICENSE-2.0 -# # -# # Unless required by applicable law or agreed to in writing, software -# # distributed under the License is distributed on an "AS IS" BASIS, -# # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# # See the License for the specific language governing permissions and -# # limitations under the License. +# See the top-level LICENSE file for licensing terms. import time import unittest