diff --git a/.github/workflows/lint.yaml b/.github/workflows/lint.yaml index 010a90157..4f926eef6 100644 --- a/.github/workflows/lint.yaml +++ b/.github/workflows/lint.yaml @@ -18,17 +18,23 @@ jobs: cppcheck, cpplint, flake8, - pep257, + # pep257, TODO: enable when we fixed + # Error: diagnostic_common_diagnostics/diagnostic_common_diagnostics/ntp_monitor.py:113 in public method `ntp_diag`: D417: Missing argument descriptions in the docstring (argument(s) st are missing descriptions in 'ntp_diag' docstring) + # using ros-rolling-ament-pep257 amd64 0.18.0-1noble.20240426.150718 uncrustify, xmllint, ] - runs-on: ubuntu-22.04 + include: + - distro: rolling + os: ubuntu-24.04 + runs-on: ${{ matrix.os }} env: AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: 1 steps: - uses: actions/checkout@v1 - uses: ros-tooling/setup-ros@master - - run: sudo pip install pydocstyle==6.1.1 # downgrade to fix https://github.com/ament/ament_lint/pull/428 + with: + required-ros-distributions: ${{ matrix.distro }} - uses: ros-tooling/action-ros-lint@master with: linter: ${{ matrix.linter }} diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml index 2fe97d371..224a0cf3a 100644 --- a/.github/workflows/test.yaml +++ b/.github/workflows/test.yaml @@ -14,28 +14,21 @@ jobs: strategy: fail-fast: false matrix: - package: - [ + package: [ diagnostic_aggregator, diagnostic_common_diagnostics, diagnostic_updater, self_test, ] - distro: [humble, iron, rolling] include: - - distro: humble - os: ubuntu-22.04 - - distro: iron - os: ubuntu-22.04 - distro: rolling - os: ubuntu-22.04 - runs-on: ${{ matrix.os }} + os: 24.04 + runs-on: ubuntu-latest + container: ubuntu:${{ matrix.os }} steps: - uses: ros-tooling/setup-ros@master - - run: | - sudo pip install pydocstyle==6.1.1 # downgrade to fix https://github.com/ament/ament_lint/pull/428 - sudo pip install pip --upgrade - sudo pip install pyopenssl --upgrade # fix for AttributeError: module 'lib' has no attribute 'X509_V_FLAG_CB_ISSUER_CHECK' + with: + required-ros-distributions: ${{ matrix.distro }} - uses: ros-tooling/action-ros-ci@master with: target-ros2-distro: ${{ matrix.distro }} diff --git a/README.md b/README.md index 521414bf7..3b7041626 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,8 @@ -[![Test diagnostics](https://img.shields.io/github/actions/workflow/status/ros/diagnostics/test.yaml?label=test&style=flat-square)](https://github.com/ros/diagnostics/actions/workflows/test.yaml) [![Lint diagnostics](https://img.shields.io/github/actions/workflow/status/ros/diagnostics/lint.yaml?label=lint&style=flat-square)](https://github.com/ros/diagnostics/actions/workflows/lint.yaml) [![ROS2 Humble](https://img.shields.io/ros/v/humble/diagnostics.svg?style=flat-square)](https://index.ros.org/r/diagnostics/#humble) [![ROS2 Iron](https://img.shields.io/ros/v/iron/diagnostics.svg?style=flat-square)](https://index.ros.org/r/diagnostics/#iron) [![ROS2 Rolling](https://img.shields.io/ros/v/rolling/diagnostics.svg?style=flat-square)](https://index.ros.org/r/diagnostics/#rolling) +[![Test diagnostics](https://img.shields.io/github/actions/workflow/status/ros/diagnostics/test.yaml?label=test&style=flat-square)](https://github.com/ros/diagnostics/actions/workflows/test.yaml) [![Lint diagnostics](https://img.shields.io/github/actions/workflow/status/ros/diagnostics/lint.yaml?label=lint&style=flat-square)](https://github.com/ros/diagnostics/actions/workflows/lint.yaml) [![ROS2 Humble](https://img.shields.io/ros/v/humble/diagnostics.svg?style=flat-square)](https://index.ros.org/r/diagnostics/#humble) [![ROS2 Iron](https://img.shields.io/ros/v/iron/diagnostics.svg?style=flat-square)](https://index.ros.org/r/diagnostics/#iron) [![ROS2 Jazzy](https://img.shields.io/ros/v/jazzy/diagnostics.svg?style=flat-square)](https://index.ros.org/r/diagnostics/#jazzy) [![ROS2 Rolling](https://img.shields.io/ros/v/rolling/diagnostics.svg?style=flat-square)](https://index.ros.org/r/diagnostics/#rolling) # Overview -The diagnostics system collects information about hardware drivers and robot hardware to make them availaible to users and operators. +The diagnostics system collects information about hardware drivers and robot hardware to make them available to users and operators. The diagnostics system contains tools to collect and analyze this data. The diagnostics system is build around the `/diagnostics` topic. The topic is used for `diagnostic_msgs/DiagnosticArray` messages. @@ -34,11 +34,82 @@ Diagnostics messages that are not aggregated can be visualized by [`rqt_runtime_ # Target Distribution -The [`ros2` branch](https://github.com/ros/diagnostics/tree/ros2) targets +- **Rolling Ridley** by the [`ros2` branch](https://github.com/ros/diagnostics/tree/ros2) +- **Humble Hawksbill** by the [`ros2-humble` branch](https://github.com/ros/diagnostics/tree/ros2-humble) +- **Iron Irwini** by the [`ros2-iron` branch](https://github.com/ros/diagnostics/tree/ros2-iron) +- **Jazzy Jalisco** by the [`ros2-jazzy` branch](https://github.com/ros/diagnostics/tree/ros2-jazzy) + +## Workflow + +New features are to be developed in custom branches and then merged into the `ros2` branch. + +From there, the changes are backported to the other branches. + +## Backport Tooling + +This tool has proven to be useful: [backport](https://www.npmjs.com/package/backport) + +Use this command to port a given PR of `PR_NUMBER` to the other branches: + +```bash +backport --pr PR_NUMBER -b ros2-humble ros2-iron ros2-jazzy +``` + +## Versioning and Releases + +- (__X__.0.0) We use the major version number to indicate a breaking change. +- (0.__Y__.0) The minor version number is used to differentiate between different ROS distributions: + - x.__0__.z: Humble Hawksbill + - x.__1__.z: Iron Irwini + - x.__2__.z: Jazzy Jalisco + - x.__3__.z: Rolling Ridley + - Future releases (Kilted Kaiju 05/25) will get x.__3__.z and _Rolling_ will be incremented accordingly. +- (0.0.__Z__) The patch version number is used for changes in the current ROS distribution that do not affect the API. + +## Buildfarm Statuses + +### diagnostic_aggregator + +| | H | I | J | R | +| ------- | - | - | - | - | +| src, ubuntu | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hsrc_uJ__diagnostic_aggregator__ubuntu_jammy__source&style=ball-32x32)](https://build.ros2.org/job/Hsrc_uJ__diagnostic_aggregator__ubuntu_jammy__source) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Isrc_uJ__diagnostic_aggregator__ubuntu_jammy__source&style=ball-32x32)](https://build.ros2.org/job/Isrc_uJ__diagnostic_aggregator__ubuntu_jammy__source) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jsrc_uN__diagnostic_aggregator__ubuntu_noble__source&style=ball-32x32)](https://build.ros2.org/job/Jsrc_uN__diagnostic_aggregator__ubuntu_noble__source) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rsrc_uN__diagnostic_aggregator__ubuntu_noble__source&style=ball-32x32)](https://build.ros2.org/job/Rsrc_uN__diagnostic_aggregator__ubuntu_noble__source) | +| src, rhel | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hsrc_el8__diagnostic_aggregator__rhel_8__source&style=ball-32x32)](https://build.ros2.org/job/Hsrc_el8__diagnostic_aggregator__rhel_8__source) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Isrc_el9__diagnostic_aggregator__rhel_9__source&style=ball-32x32)](https://build.ros2.org/job/Isrc_el9__diagnostic_aggregator__rhel_9__source) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jsrc_el9__diagnostic_aggregator__rhel_9__source&style=ball-32x32)](https://build.ros2.org/job/Jsrc_el9__diagnostic_aggregator__rhel_9__source) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rsrc_el9__diagnostic_aggregator__rhel_9__source&style=ball-32x32)](https://build.ros2.org/job/Rsrc_el9__diagnostic_aggregator__rhel_9__source) | +| bin, ubuntu, amd64 | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__diagnostic_aggregator__ubuntu_jammy_amd64__binary&style=ball-32x32)](https://build.ros2.org/job/Hbin_uJ64__diagnostic_aggregator__ubuntu_jammy_amd64__binary) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__diagnostic_aggregator__ubuntu_jammy_amd64__binary&style=ball-32x32)](https://build.ros2.org/job/Ibin_uJ64__diagnostic_aggregator__ubuntu_jammy_amd64__binary) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__diagnostic_aggregator__ubuntu_noble_amd64__binary&style=ball-32x32)](https://build.ros2.org/job/Jbin_uN64__diagnostic_aggregator__ubuntu_noble_amd64__binary) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__diagnostic_aggregator__ubuntu_noble_amd64__binary&style=ball-32x32)](https://build.ros2.org/job/Rbin_uN64__diagnostic_aggregator__ubuntu_noble_amd64__binary) | +| bin, ubuntu, arm64 | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hbin_ujv8_uJv8__diagnostic_aggregator__ubuntu_jammy_arm64__binary&style=ball-32x32)](https://build.ros2.org/job/Hbin_ujv8_uJv8__diagnostic_aggregator__ubuntu_jammy_arm64__binary) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Ibin_ujv8_uJv8__diagnostic_aggregator__ubuntu_jammy_arm64__binary&style=ball-32x32)](https://build.ros2.org/job/Ibin_ujv8_uJv8__diagnostic_aggregator__ubuntu_jammy_arm64__binary) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jbin_unv8_uNv8__diagnostic_aggregator__ubuntu_noble_arm64__binary&style=ball-32x32)](https://build.ros2.org/job/Jbin_unv8_uNv8__diagnostic_aggregator__ubuntu_noble_arm64__binary) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rbin_unv8_uNv8__diagnostic_aggregator__ubuntu_noble_arm64__binary&style=ball-32x32)](https://build.ros2.org/job/Rbin_unv8_uNv8__diagnostic_aggregator__ubuntu_noble_arm64__binary) | +| bin, rhel | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hbin_rhel_el864__diagnostic_aggregator__rhel_8_x86_64__binary&style=ball-32x32)](https://build.ros2.org/job/Hbin_rhel_el864__diagnostic_aggregator__rhel_8_x86_64__binary) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Ibin_rhel_el964__diagnostic_aggregator__rhel_9_x86_64__binary&style=ball-32x32)](https://build.ros2.org/job/Ibin_rhel_el964__diagnostic_aggregator__rhel_9_x86_64__binary) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jbin_rhel_el964__diagnostic_aggregator__rhel_9_x86_64__binary&style=ball-32x32)](https://build.ros2.org/job/Jbin_rhel_el964__diagnostic_aggregator__rhel_9_x86_64__binary) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rbin_rhel_el964__diagnostic_aggregator__rhel_9_x86_64__binary&style=ball-32x32)](https://build.ros2.org/job/Rbin_rhel_el964__diagnostic_aggregator__rhel_9_x86_64__binary) | + + +### diagnostic_common_diagnostics + +| | H | I | J | R | +| ------- | - | - | - | - | +| src, ubuntu | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hsrc_uJ__diagnostic_common_diagnostics__ubuntu_jammy__source&style=ball-32x32)](https://build.ros2.org/job/Hsrc_uJ__diagnostic_common_diagnostics__ubuntu_jammy__source) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Isrc_uJ__diagnostic_common_diagnostics__ubuntu_jammy__source&style=ball-32x32)](https://build.ros2.org/job/Isrc_uJ__diagnostic_common_diagnostics__ubuntu_jammy__source) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jsrc_uN__diagnostic_common_diagnostics__ubuntu_noble__source&style=ball-32x32)](https://build.ros2.org/job/Jsrc_uN__diagnostic_common_diagnostics__ubuntu_noble__source) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rsrc_uN__diagnostic_common_diagnostics__ubuntu_noble__source&style=ball-32x32)](https://build.ros2.org/job/Rsrc_uN__diagnostic_common_diagnostics__ubuntu_noble__source) | +| src, rhel | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hsrc_el8__diagnostic_common_diagnostics__rhel_8__source&style=ball-32x32)](https://build.ros2.org/job/Hsrc_el8__diagnostic_common_diagnostics__rhel_8__source) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Isrc_el9__diagnostic_common_diagnostics__rhel_9__source&style=ball-32x32)](https://build.ros2.org/job/Isrc_el9__diagnostic_common_diagnostics__rhel_9__source) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jsrc_el9__diagnostic_common_diagnostics__rhel_9__source&style=ball-32x32)](https://build.ros2.org/job/Jsrc_el9__diagnostic_common_diagnostics__rhel_9__source) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rsrc_el9__diagnostic_common_diagnostics__rhel_9__source&style=ball-32x32)](https://build.ros2.org/job/Rsrc_el9__diagnostic_common_diagnostics__rhel_9__source) | +| bin, ubuntu, amd64 | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__diagnostic_common_diagnostics__ubuntu_jammy_amd64__binary&style=ball-32x32)](https://build.ros2.org/job/Hbin_uJ64__diagnostic_common_diagnostics__ubuntu_jammy_amd64__binary) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__diagnostic_common_diagnostics__ubuntu_jammy_amd64__binary&style=ball-32x32)](https://build.ros2.org/job/Ibin_uJ64__diagnostic_common_diagnostics__ubuntu_jammy_amd64__binary) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__diagnostic_common_diagnostics__ubuntu_noble_amd64__binary&style=ball-32x32)](https://build.ros2.org/job/Jbin_uN64__diagnostic_common_diagnostics__ubuntu_noble_amd64__binary) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__diagnostic_common_diagnostics__ubuntu_noble_amd64__binary&style=ball-32x32)](https://build.ros2.org/job/Rbin_uN64__diagnostic_common_diagnostics__ubuntu_noble_amd64__binary) | +| bin, ubuntu, arm64 | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hbin_ujv8_uJv8__diagnostic_common_diagnostics__ubuntu_jammy_arm64__binary&style=ball-32x32)](https://build.ros2.org/job/Hbin_ujv8_uJv8__diagnostic_common_diagnostics__ubuntu_jammy_arm64__binary) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Ibin_ujv8_uJv8__diagnostic_common_diagnostics__ubuntu_jammy_arm64__binary&style=ball-32x32)](https://build.ros2.org/job/Ibin_ujv8_uJv8__diagnostic_common_diagnostics__ubuntu_jammy_arm64__binary) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jbin_unv8_uNv8__diagnostic_common_diagnostics__ubuntu_noble_arm64__binary&style=ball-32x32)](https://build.ros2.org/job/Jbin_unv8_uNv8__diagnostic_common_diagnostics__ubuntu_noble_arm64__binary) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rbin_unv8_uNv8__diagnostic_common_diagnostics__ubuntu_noble_arm64__binary&style=ball-32x32)](https://build.ros2.org/job/Rbin_unv8_uNv8__diagnostic_common_diagnostics__ubuntu_noble_arm64__binary) | +| bin, rhel | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hbin_rhel_el864__diagnostic_common_diagnostics__rhel_8_x86_64__binary&style=ball-32x32)](https://build.ros2.org/job/Hbin_rhel_el864__diagnostic_common_diagnostics__rhel_8_x86_64__binary) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Ibin_rhel_el964__diagnostic_common_diagnostics__rhel_9_x86_64__binary&style=ball-32x32)](https://build.ros2.org/job/Ibin_rhel_el964__diagnostic_common_diagnostics__rhel_9_x86_64__binary) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jbin_rhel_el964__diagnostic_common_diagnostics__rhel_9_x86_64__binary&style=ball-32x32)](https://build.ros2.org/job/Jbin_rhel_el964__diagnostic_common_diagnostics__rhel_9_x86_64__binary) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rbin_rhel_el964__diagnostic_common_diagnostics__rhel_9_x86_64__binary&style=ball-32x32)](https://build.ros2.org/job/Rbin_rhel_el964__diagnostic_common_diagnostics__rhel_9_x86_64__binary) | + + +### diagnostic_updater + +| | H | I | J | R | +| ------- | - | - | - | - | +| src, ubuntu | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hsrc_uJ__diagnostic_updater__ubuntu_jammy__source&style=ball-32x32)](https://build.ros2.org/job/Hsrc_uJ__diagnostic_updater__ubuntu_jammy__source) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Isrc_uJ__diagnostic_updater__ubuntu_jammy__source&style=ball-32x32)](https://build.ros2.org/job/Isrc_uJ__diagnostic_updater__ubuntu_jammy__source) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jsrc_uN__diagnostic_updater__ubuntu_noble__source&style=ball-32x32)](https://build.ros2.org/job/Jsrc_uN__diagnostic_updater__ubuntu_noble__source) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rsrc_uN__diagnostic_updater__ubuntu_noble__source&style=ball-32x32)](https://build.ros2.org/job/Rsrc_uN__diagnostic_updater__ubuntu_noble__source) | +| src, rhel | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hsrc_el8__diagnostic_updater__rhel_8__source&style=ball-32x32)](https://build.ros2.org/job/Hsrc_el8__diagnostic_updater__rhel_8__source) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Isrc_el9__diagnostic_updater__rhel_9__source&style=ball-32x32)](https://build.ros2.org/job/Isrc_el9__diagnostic_updater__rhel_9__source) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jsrc_el9__diagnostic_updater__rhel_9__source&style=ball-32x32)](https://build.ros2.org/job/Jsrc_el9__diagnostic_updater__rhel_9__source) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rsrc_el9__diagnostic_updater__rhel_9__source&style=ball-32x32)](https://build.ros2.org/job/Rsrc_el9__diagnostic_updater__rhel_9__source) | +| bin, ubuntu, amd64 | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__diagnostic_updater__ubuntu_jammy_amd64__binary&style=ball-32x32)](https://build.ros2.org/job/Hbin_uJ64__diagnostic_updater__ubuntu_jammy_amd64__binary) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__diagnostic_updater__ubuntu_jammy_amd64__binary&style=ball-32x32)](https://build.ros2.org/job/Ibin_uJ64__diagnostic_updater__ubuntu_jammy_amd64__binary) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__diagnostic_updater__ubuntu_noble_amd64__binary&style=ball-32x32)](https://build.ros2.org/job/Jbin_uN64__diagnostic_updater__ubuntu_noble_amd64__binary) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__diagnostic_updater__ubuntu_noble_amd64__binary&style=ball-32x32)](https://build.ros2.org/job/Rbin_uN64__diagnostic_updater__ubuntu_noble_amd64__binary) | +| bin, ubuntu, arm64 | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hbin_ujv8_uJv8__diagnostic_updater__ubuntu_jammy_arm64__binary&style=ball-32x32)](https://build.ros2.org/job/Hbin_ujv8_uJv8__diagnostic_updater__ubuntu_jammy_arm64__binary) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Ibin_ujv8_uJv8__diagnostic_updater__ubuntu_jammy_arm64__binary&style=ball-32x32)](https://build.ros2.org/job/Ibin_ujv8_uJv8__diagnostic_updater__ubuntu_jammy_arm64__binary) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jbin_unv8_uNv8__diagnostic_updater__ubuntu_noble_arm64__binary&style=ball-32x32)](https://build.ros2.org/job/Jbin_unv8_uNv8__diagnostic_updater__ubuntu_noble_arm64__binary) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rbin_unv8_uNv8__diagnostic_updater__ubuntu_noble_arm64__binary&style=ball-32x32)](https://build.ros2.org/job/Rbin_unv8_uNv8__diagnostic_updater__ubuntu_noble_arm64__binary) | +| bin, rhel | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hbin_rhel_el864__diagnostic_updater__rhel_8_x86_64__binary&style=ball-32x32)](https://build.ros2.org/job/Hbin_rhel_el864__diagnostic_updater__rhel_8_x86_64__binary) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Ibin_rhel_el964__diagnostic_updater__rhel_9_x86_64__binary&style=ball-32x32)](https://build.ros2.org/job/Ibin_rhel_el964__diagnostic_updater__rhel_9_x86_64__binary) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jbin_rhel_el964__diagnostic_updater__rhel_9_x86_64__binary&style=ball-32x32)](https://build.ros2.org/job/Jbin_rhel_el964__diagnostic_updater__rhel_9_x86_64__binary) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rbin_rhel_el964__diagnostic_updater__rhel_9_x86_64__binary&style=ball-32x32)](https://build.ros2.org/job/Rbin_rhel_el964__diagnostic_updater__rhel_9_x86_64__binary) | + +### self_test + +| | H | I | J | R | +| ------- | - | - | - | - | +| src, ubuntu | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hsrc_uJ__self_test__ubuntu_jammy__source&style=ball-32x32)](https://build.ros2.org/job/Hsrc_uJ__self_test__ubuntu_jammy__source) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Isrc_uJ__self_test__ubuntu_jammy__source&style=ball-32x32)](https://build.ros2.org/job/Isrc_uJ__self_test__ubuntu_jammy__source) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jsrc_uN__self_test__ubuntu_noble__source&style=ball-32x32)](https://build.ros2.org/job/Jsrc_uN__self_test__ubuntu_noble__source) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rsrc_uN__self_test__ubuntu_noble__source&style=ball-32x32)](https://build.ros2.org/job/Rsrc_uN__self_test__ubuntu_noble__source) | +| src, rhel | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hsrc_el8__self_test__rhel_8__source&style=ball-32x32)](https://build.ros2.org/job/Hsrc_el8__self_test__rhel_8__source) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Isrc_el9__self_test__rhel_9__source&style=ball-32x32)](https://build.ros2.org/job/Isrc_el9__self_test__rhel_9__source) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jsrc_el9__self_test__rhel_9__source&style=ball-32x32)](https://build.ros2.org/job/Jsrc_el9__self_test__rhel_9__source) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rsrc_el9__self_test__rhel_9__source&style=ball-32x32)](https://build.ros2.org/job/Rsrc_el9__self_test__rhel_9__source) | +| bin, ubuntu, amd64 | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__self_test__ubuntu_jammy_amd64__binary&style=ball-32x32)](https://build.ros2.org/job/Hbin_uJ64__self_test__ubuntu_jammy_amd64__binary) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__self_test__ubuntu_jammy_amd64__binary&style=ball-32x32)](https://build.ros2.org/job/Ibin_uJ64__self_test__ubuntu_jammy_amd64__binary) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__self_test__ubuntu_noble_amd64__binary&style=ball-32x32)](https://build.ros2.org/job/Jbin_uN64__self_test__ubuntu_noble_amd64__binary) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__self_test__ubuntu_noble_amd64__binary&style=ball-32x32)](https://build.ros2.org/job/Rbin_uN64__self_test__ubuntu_noble_amd64__binary) | +| bin, ubuntu, arm64 | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hbin_ujv8_uJv8__self_test__ubuntu_jammy_arm64__binary&style=ball-32x32)](https://build.ros2.org/job/Hbin_ujv8_uJv8__self_test__ubuntu_jammy_arm64__binary) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Ibin_ujv8_uJv8__self_test__ubuntu_jammy_arm64__binary&style=ball-32x32)](https://build.ros2.org/job/Ibin_ujv8_uJv8__self_test__ubuntu_jammy_arm64__binary) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jbin_unv8_uNv8__self_test__ubuntu_noble_arm64__binary&style=ball-32x32)](https://build.ros2.org/job/Jbin_unv8_uNv8__self_test__ubuntu_noble_arm64__binary) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rbin_unv8_uNv8__self_test__ubuntu_noble_arm64__binary&style=ball-32x32)](https://build.ros2.org/job/Rbin_unv8_uNv8__self_test__ubuntu_noble_arm64__binary) | +| bin, rhel | [![Humble](https://build.ros2.org/buildStatus/icon?job=Hbin_rhel_el864__self_test__rhel_8_x86_64__binary&style=ball-32x32)](https://build.ros2.org/job/Hbin_rhel_el864__self_test__rhel_8_x86_64__binary) | [![Iron](https://build.ros2.org/buildStatus/icon?job=Ibin_rhel_el964__self_test__rhel_9_x86_64__binary&style=ball-32x32)](https://build.ros2.org/job/Ibin_rhel_el964__self_test__rhel_9_x86_64__binary) | [![Jazzy](https://build.ros2.org/buildStatus/icon?job=Jbin_rhel_el964__self_test__rhel_9_x86_64__binary&style=ball-32x32)](https://build.ros2.org/job/Jbin_rhel_el964__self_test__rhel_9_x86_64__binary) | [![Rolling](https://build.ros2.org/buildStatus/icon?job=Rbin_rhel_el964__self_test__rhel_9_x86_64__binary&style=ball-32x32)](https://build.ros2.org/job/Rbin_rhel_el964__self_test__rhel_9_x86_64__binary) | -- *Humble Hawksbill* -- *Iron Irwini* and -- *Rolling Ridley* # License diff --git a/diagnostic_aggregator/CHANGELOG.rst b/diagnostic_aggregator/CHANGELOG.rst index 9c48d2f13..023c8bd87 100644 --- a/diagnostic_aggregator/CHANGELOG.rst +++ b/diagnostic_aggregator/CHANGELOG.rst @@ -2,6 +2,25 @@ Changelog for package diagnostic_aggregator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.3.1 (2024-07-30) +------------------ + +3.2.1 (2024-06-27) +------------------ +* Add add_analyzer functionality (`#329 `_) +* Aggregator: publish diagnostics_toplevel_state immediately on every degradation (`#324 `_) +* Contributors: MartinCornelis2, Tim Clephas + +3.2.0 (2024-03-22) +------------------ +* Avoid rolling up an ERROR state when empty GenericAnalyzer blocks are marked discard_stale, or when all of their items are STALE. (`#315 `_) +* formatting fixes from PR324 (`#327 `_) +* Debugging instability introduced by `#317 `_ (`#323 `_) +* feat: publish top level msg when error is received (`#317 `_) +* Empty default aggregator base_path (`#305 `_) +* using defined state for stale (`#298 `_) +* Contributors: Andrew Symington, Christian Henkel, outrider-jhulas + 3.1.2 (2023-03-24) ------------------ diff --git a/diagnostic_aggregator/CMakeLists.txt b/diagnostic_aggregator/CMakeLists.txt index 8b17257ec..e42b71e60 100644 --- a/diagnostic_aggregator/CMakeLists.txt +++ b/diagnostic_aggregator/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED) find_package(diagnostic_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) +find_package(rcl_interfaces REQUIRED) find_package(std_msgs REQUIRED) find_package(rclcpp_components REQUIRED) @@ -72,6 +73,10 @@ target_link_libraries(aggregator_node rclcpp_components_register_nodes(${PROJECT_NAME} "diagnostic_aggregator::Aggregator") +# Add analyzer +add_executable(add_analyzer src/add_analyzer.cpp) +ament_target_dependencies(add_analyzer rclcpp rcl_interfaces) + # Testing macro if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -82,6 +87,7 @@ if(BUILD_TESTING) find_package(launch_testing_ament_cmake REQUIRED) file(TO_CMAKE_PATH "${CMAKE_INSTALL_PREFIX}/lib/${PROJECT_NAME}/aggregator_node" AGGREGATOR_NODE) + file(TO_CMAKE_PATH "${CMAKE_INSTALL_PREFIX}/lib/${PROJECT_NAME}/add_analyzer" ADD_ANALYZER) file(TO_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test/test_listener.py" TEST_LISTENER) set(create_analyzers_tests "primitive_analyzers" @@ -129,10 +135,36 @@ if(BUILD_TESTING) ) endforeach() + set(add_analyzers_tests + "all_analyzers") + + foreach(test_name ${add_analyzers_tests}) + file(TO_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test/default.yaml" PARAMETER_FILE) + file(TO_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test/${test_name}.yaml" ADD_PARAMETER_FILE) + file(TO_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test/expected_output/add_${test_name}" EXPECTED_OUTPUT) + + configure_file( + "test/add_analyzers.launch.py.in" + "test_add_${test_name}.launch.py" + @ONLY + ) + add_launch_test( + "${CMAKE_CURRENT_BINARY_DIR}/test_add_${test_name}.launch.py" + TARGET "test_add_${test_name}" + TIMEOUT 30 + ENV + ) + endforeach() + add_launch_test( test/test_critical_pub.py TIMEOUT 30 ) + + ament_add_pytest_test(test_discard_behavior + "${CMAKE_CURRENT_SOURCE_DIR}/test/test_discard_behavior.py" + TIMEOUT 60 + ) endif() install( @@ -140,6 +172,11 @@ install( DESTINATION lib/${PROJECT_NAME} ) +install( + TARGETS add_analyzer + DESTINATION lib/${PROJECT_NAME} +) + install( TARGETS ${PROJECT_NAME} ${ANALYZERS} EXPORT ${PROJECT_NAME}Targets @@ -157,6 +194,7 @@ ament_python_install_package(${PROJECT_NAME}) # Install Example set(ANALYZER_PARAMS_FILEPATH "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/example_analyzers.yaml") +set(ADD_ANALYZER_PARAMS_FILEPATH "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/example_add_analyzers.yaml") configure_file(example/example.launch.py.in example.launch.py @ONLY) install( # launch descriptor FILES ${CMAKE_CURRENT_BINARY_DIR}/example.launch.py @@ -167,7 +205,7 @@ install( # example publisher DESTINATION lib/${PROJECT_NAME} ) install( # example aggregator configration - FILES example/example_analyzers.yaml + FILES example/example_analyzers.yaml example/example_add_analyzers.yaml DESTINATION share/${PROJECT_NAME} ) diff --git a/diagnostic_aggregator/README.md b/diagnostic_aggregator/README.md index e7f63c2b7..fa0eea7a9 100644 --- a/diagnostic_aggregator/README.md +++ b/diagnostic_aggregator/README.md @@ -14,7 +14,7 @@ The robot has two of each, one on each side. The robot also 4 camera sensors, one left and one right and one in the front and one in the back. These are all the available diagnostic sources: -``` +``` /arms/left/motor /arms/right/motor /legs/left/motor @@ -119,6 +119,9 @@ Additional parameters depend on the type of the analyzer. Any diagnostic item that is not matched by any analyzer will be published by an "Other" analyzer. Items created by the "Other" analyzer will go stale after 5 seconds. +The `critical` parameter makes the aggregator react immediately to a degradation in diagnostic state. +This is useful if the toplevel state is parsed by a watchdog for example. + ## Launching You can launch the `aggregator_node` like this (see [example.launch.py.in](example/example.launch.py.in)): ``` python @@ -132,6 +135,33 @@ You can launch the `aggregator_node` like this (see [example.launch.py.in](examp ]) ``` +You can add analyzers at runtime using the `add_analyzer` node like this (see [example.launch.py.in](example/example.launch.py.in)): +``` + add_analyzer = launch_ros.actions.Node( + package='diagnostic_aggregator', + executable='add_analyzer', + output='screen', + parameters=[add_analyzer_params_filepath]) + return launch.LaunchDescription([ + add_analyzer, + ]) +``` +This node updates the parameters of the `aggregator_node` by calling the service `/analyzers/set_parameters_atomically`. +The `aggregator_node` will detect when a `parameter-event` has introduced new parameters to it. +When this happens the `aggregator_node` will reload all analyzers based on its new set of parameters. +Adding analyzers this way can be done at runtime and can be made conditional. + +In the example, `add_analyzer` will add an analyzer for diagnostics that are marked optional: +``` yaml +/**: + ros__parameters: + optional: + type: diagnostic_aggregator/GenericAnalyzer + path: Optional + startswith: [ '/optional' ] +``` +This will move the `/optional/runtime/analyzer` diagnostic from the "Other" to "Aggregation" where it will not go stale after 5 seconds and will be taken into account for the toplevel state. + # Basic analyzers The `diagnostic_aggregator` package provides a few basic analyzers that you can use to aggregate your diagnostics. @@ -186,4 +216,4 @@ This means that things that are ignored by the `IgnoreAnalyzer` will still be pu - `analyzers` (map, default: {}) - The analyzers that will be used to aggregate the diagnostics # Tutorials -TODO: Port tutorials #contributions-welcome \ No newline at end of file +TODO: Port tutorials #contributions-welcome diff --git a/diagnostic_aggregator/example/README.md b/diagnostic_aggregator/example/README.md index 10e9b2574..27c593be9 100644 --- a/diagnostic_aggregator/example/README.md +++ b/diagnostic_aggregator/example/README.md @@ -1,5 +1,7 @@ # Aggregator Example -This is a simple example to show the diagnostic_aggregator in action. It involves one python script producing dummy diagnostic data ([example_pub.py](./example_pub.py)), and one diagnostic aggregator configuration ([example.yaml](./example.yaml)) that provides analyzers aggregating it. +This is a simple example to show the diagnostic_aggregator and add_analyzer in action. It involves one python script producing dummy diagnostic data ([example_pub.py](./example_pub.py)), one diagnostic aggregator configuration ([example_analyzers.yaml](./example_analyzers.yaml)) and one add_analyzer configuration ([example_add_analyzers.yaml](./example_add_analyzers.yaml)). + +The aggregator will launch and load all the analyzers listed in ([example_analyzers.yaml](./example_analyzers.yaml)). Then the aggregator will be notified that there are additional analyzers that we also want to load in ([example_add_analyzers.yaml](./example_add_analyzers.yaml)). After this reload all analyzers will be active. Run the example with `ros2 launch diagnostic_aggregator example.launch.py` diff --git a/diagnostic_aggregator/example/example.launch.py.in b/diagnostic_aggregator/example/example.launch.py.in index 48cd62f66..81a749220 100644 --- a/diagnostic_aggregator/example/example.launch.py.in +++ b/diagnostic_aggregator/example/example.launch.py.in @@ -4,6 +4,7 @@ import launch import launch_ros.actions analyzer_params_filepath = "@ANALYZER_PARAMS_FILEPATH@" +add_analyzer_params_filepath = "@ADD_ANALYZER_PARAMS_FILEPATH@" def generate_launch_description(): @@ -12,11 +13,18 @@ def generate_launch_description(): executable='aggregator_node', output='screen', parameters=[analyzer_params_filepath]) + add_analyzer = launch_ros.actions.Node( + package='diagnostic_aggregator', + executable='add_analyzer', + output='screen', + parameters=[add_analyzer_params_filepath] + ) diag_publisher = launch_ros.actions.Node( package='diagnostic_aggregator', executable='example_pub.py') return launch.LaunchDescription([ aggregator, + add_analyzer, diag_publisher, launch.actions.RegisterEventHandler( event_handler=launch.event_handlers.OnProcessExit( diff --git a/diagnostic_aggregator/example/example_add_analyzers.yaml b/diagnostic_aggregator/example/example_add_analyzers.yaml new file mode 100644 index 000000000..1c6c264c7 --- /dev/null +++ b/diagnostic_aggregator/example/example_add_analyzers.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + optional: + type: diagnostic_aggregator/GenericAnalyzer + path: Optional + contains: [ '/optional' ] diff --git a/diagnostic_aggregator/example/example_pub.py b/diagnostic_aggregator/example/example_pub.py index 887dc18df..0c1b10436 100755 --- a/diagnostic_aggregator/example/example_pub.py +++ b/diagnostic_aggregator/example/example_pub.py @@ -81,6 +81,10 @@ def __init__(self): name='/sensors/front/cam', message='OK'), DiagnosticStatus(level=DiagnosticStatus.OK, name='/sensors/rear/cam', message='OK'), + + # Optional + DiagnosticStatus(level=DiagnosticStatus.OK, + name='/optional/runtime/analyzer', message='OK'), ] def timer_callback(self): diff --git a/diagnostic_aggregator/include/diagnostic_aggregator/aggregator.hpp b/diagnostic_aggregator/include/diagnostic_aggregator/aggregator.hpp index 002cd30c2..f041abdfa 100644 --- a/diagnostic_aggregator/include/diagnostic_aggregator/aggregator.hpp +++ b/diagnostic_aggregator/include/diagnostic_aggregator/aggregator.hpp @@ -133,6 +133,8 @@ class Aggregator : public rclcpp::Node rclcpp::Service::SharedPtr add_srv_; /// DiagnosticArray, /diagnostics rclcpp::Subscription::SharedPtr diag_sub_; + /// ParameterEvent, /parameter_events + rclcpp::Subscription::SharedPtr param_sub_; /// DiagnosticArray, /diagnostics_agg rclcpp::Publisher::SharedPtr agg_pub_; /// DiagnosticStatus, /diagnostics_toplevel_state @@ -165,6 +167,16 @@ class Aggregator : public rclcpp::Node /// Records all ROS warnings. No warnings are repeated. std::set ros_warnings_; + /* + *!\brief Checks for new parameters to trigger reinitialization of the AnalyzerGroup and OtherAnalyzer + */ + void parameterCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr param_msg); + + /* + *!\brief (re)initializes the AnalyzerGroup and OtherAnalyzer + */ + void initAnalyzers(); + /* *!\brief Checks timestamp of message, and warns if timestamp is 0 (not set) */ diff --git a/diagnostic_aggregator/include/diagnostic_aggregator/generic_analyzer_base.hpp b/diagnostic_aggregator/include/diagnostic_aggregator/generic_analyzer_base.hpp index 11a5b4f43..d7cb8a485 100644 --- a/diagnostic_aggregator/include/diagnostic_aggregator/generic_analyzer_base.hpp +++ b/diagnostic_aggregator/include/diagnostic_aggregator/generic_analyzer_base.hpp @@ -178,7 +178,7 @@ class GenericAnalyzerBase : public Analyzer auto header_status = std::make_shared(); header_status->name = path_; - header_status->level = 0; + header_status->level = diagnostic_msgs::msg::DiagnosticStatus::OK; header_status->message = "OK"; std::vector> processed; @@ -224,22 +224,28 @@ class GenericAnalyzerBase : public Analyzer // Header is not stale unless all subs are if (all_stale) { - header_status->level = diagnostic_msgs::msg::DiagnosticStatus::STALE; + // If we elect to discard stale items, then it signals that the absence of an item + // is not considered problematic, so we should allow empty queues to roll up as OK. + if (discard_stale_) { + header_status->level = diagnostic_msgs::msg::DiagnosticStatus::OK; + } else { + header_status->level = diagnostic_msgs::msg::DiagnosticStatus::STALE; + } } else if (header_status->level == diagnostic_msgs::msg::DiagnosticStatus::STALE) { - header_status->level = 2; + header_status->level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; } header_status->message = valToMsg(header_status->level); // If we expect a given number of items, check that we have this number if (num_items_expected_ == 0 && items_.empty()) { - header_status->level = 0; + header_status->level = diagnostic_msgs::msg::DiagnosticStatus::OK; header_status->message = "OK"; } else if ( // NOLINT num_items_expected_ > 0 && static_cast(items_.size()) != num_items_expected_) { // NOLINT - int8_t lvl = 2; + int8_t lvl = diagnostic_msgs::msg::DiagnosticStatus::ERROR; header_status->level = std::max(lvl, static_cast(header_status->level)); std::stringstream expec, item; diff --git a/diagnostic_aggregator/include/diagnostic_aggregator/other_analyzer.hpp b/diagnostic_aggregator/include/diagnostic_aggregator/other_analyzer.hpp index 3cb9162e4..67d0f1b0a 100644 --- a/diagnostic_aggregator/include/diagnostic_aggregator/other_analyzer.hpp +++ b/diagnostic_aggregator/include/diagnostic_aggregator/other_analyzer.hpp @@ -145,7 +145,7 @@ class OtherAnalyzer : public GenericAnalyzerBase processed.begin(); for (; it != processed.end(); ++it) { if ((*it)->name == path_) { - (*it)->level = 2; + (*it)->level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; (*it)->message = "Unanalyzed items found in \"Other\""; break; } diff --git a/diagnostic_aggregator/mainpage.dox b/diagnostic_aggregator/mainpage.dox index f7827de57..b720b86e4 100644 --- a/diagnostic_aggregator/mainpage.dox +++ b/diagnostic_aggregator/mainpage.dox @@ -16,7 +16,7 @@ See Analyzer for more information on the base class. \subsubsection generic_analyzer GenericAnalyzer -\b generic_analyzer holds the GenericAnalyzer class, which is the most basic of the Analyzer's. It is used by the diagnostic_aggregator/Aggregator to store, process and republish diagnostics data. The GenericAnalyzer is loaded by the pluginlib as a Analyzer plugin. It is the most basic of all Analyzer's. +\b generic_analyzer holds the GenericAnalyzer class, which is the most basic of the Analyzer's. It is used by the diagnostic_aggregator/Aggregator to store, process and republish diagnostics data. The GenericAnalyzer is loaded by the pluginlib as a Analyzer plugin. It is the most basic of all Analyzer's. \subsubsection analyzer_group AnalyzerGroup @@ -37,10 +37,10 @@ aggregator_node subscribes to "/diagnostics" and publishes an aggregated set of \subsubsection topics ROS topics Subscribes to: -- \b "/diagnostics": [diagnostics_msgs/DiagnosticArray] +- \b "/diagnostics": [diagnostics_msgs/DiagnosticArray] Publishes to: -- \b "/diagnostics_agg": [diagnostics_msgs/DiagnosticArray] +- \b "/diagnostics_agg": [diagnostics_msgs/DiagnosticArray] \subsubsection parameters ROS parameters @@ -49,6 +49,7 @@ Reads the following parameters from the parameter server - \b "~pub_rate" : \b double [optional] Rate that output diagnostics published - \b "~base_path" : \b double [optional] Prepended to all analyzed output - \b "~analyzers" : \b {} Configuration for loading analyzers +- \b "~critical" : \b bool [optional] React immediately to a degradation in diagnostic state \subsection analyzer_loader analyzer_loader @@ -61,4 +62,4 @@ Reads the following parameters from the parameter server - \b "~analyzers" : \b {} Configuration for loading and testing analyzers -*/ \ No newline at end of file +*/ diff --git a/diagnostic_aggregator/package.xml b/diagnostic_aggregator/package.xml index 31095edd0..b63f01646 100644 --- a/diagnostic_aggregator/package.xml +++ b/diagnostic_aggregator/package.xml @@ -2,7 +2,7 @@ diagnostic_aggregator - 3.1.2 + 4.3.1 diagnostic_aggregator Austin Hendrix Brice Rebsamen @@ -12,7 +12,7 @@ BSD-3-Clause http://www.ros.org/wiki/diagnostic_aggregator - + Kevin Watts Brice Rebsamen Arne Nordmann @@ -22,6 +22,7 @@ diagnostic_msgs pluginlib + rcl_interfaces rclcpp std_msgs @@ -31,6 +32,7 @@ ament_cmake_pytest ament_lint_auto ament_lint_common + launch_pytest launch_testing_ament_cmake launch_testing_ros diff --git a/diagnostic_aggregator/src/add_analyzer.cpp b/diagnostic_aggregator/src/add_analyzer.cpp new file mode 100644 index 000000000..42ff2b0aa --- /dev/null +++ b/diagnostic_aggregator/src/add_analyzer.cpp @@ -0,0 +1,110 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Nobleo Technology + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/**< \author Martin Cornelis */ + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rcl_interfaces/srv/set_parameters_atomically.hpp" +#include "rcl_interfaces/msg/parameter.hpp" + +using namespace std::chrono_literals; + +class AddAnalyzer : public rclcpp::Node +{ +public: + AddAnalyzer() + : Node("add_analyzer_node", "", rclcpp::NodeOptions().allow_undeclared_parameters( + true).automatically_declare_parameters_from_overrides(true)) + { + client_ = this->create_client( + "/analyzers/set_parameters_atomically"); + } + + void send_request() + { + while (!client_->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO_ONCE(this->get_logger(), "service not available, waiting ..."); + } + auto request = std::make_shared(); + std::map parameters; + + if (!this->get_parameters("", parameters)) { + RCLCPP_ERROR(this->get_logger(), "Failed to retrieve parameters"); + } + for (const auto & [param_name, param] : parameters) { + // Find the suffix + size_t suffix_start = param_name.find_last_of('.'); + // Remove suffix if it exists + if (suffix_start != std::string::npos) { + std::string stripped_param_name = param_name.substr(0, suffix_start); + // Check in map if the stripped param name with the added suffix "path" exists + // This indicates the parameter is part of an analyzer description + if (parameters.count(stripped_param_name + ".path") > 0) { + auto parameter_msg = param.to_parameter_msg(); + request->parameters.push_back(parameter_msg); + } + } + } + + auto result = client_->async_send_request(request); + // Wait for the result. + if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(this->get_logger(), "Parameters succesfully set"); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to set parameters"); + } + } + +private: + rclcpp::Client::SharedPtr client_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto add_analyzer = std::make_shared(); + add_analyzer->send_request(); + rclcpp::shutdown(); + + return 0; +} diff --git a/diagnostic_aggregator/src/aggregator.cpp b/diagnostic_aggregator/src/aggregator.cpp index a6305bdb4..487e0834c 100644 --- a/diagnostic_aggregator/src/aggregator.cpp +++ b/diagnostic_aggregator/src/aggregator.cpp @@ -67,6 +67,36 @@ Aggregator::Aggregator(rclcpp::NodeOptions options) last_top_level_state_(DiagnosticStatus::STALE) { RCLCPP_DEBUG(logger_, "constructor"); + initAnalyzers(); + + diag_sub_ = n_->create_subscription( + "/diagnostics", rclcpp::SystemDefaultsQoS().keep_last(history_depth_), + std::bind(&Aggregator::diagCallback, this, _1)); + agg_pub_ = n_->create_publisher("/diagnostics_agg", 1); + toplevel_state_pub_ = + n_->create_publisher("/diagnostics_toplevel_state", 1); + + int publish_rate_ms = 1000 / pub_rate_; + publish_timer_ = n_->create_wall_timer( + std::chrono::milliseconds(publish_rate_ms), + std::bind(&Aggregator::publishData, this)); + + param_sub_ = n_->create_subscription( + "/parameter_events", 1, std::bind(&Aggregator::parameterCallback, this, _1)); +} + +void Aggregator::parameterCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr msg) +{ + if (msg->node == "/" + std::string(n_->get_name())) { + if (msg->new_parameters.size() != 0) { + base_path_ = ""; + initAnalyzers(); + } + } +} + +void Aggregator::initAnalyzers() +{ bool other_as_errors = false; std::map parameters; @@ -99,15 +129,18 @@ Aggregator::Aggregator(rclcpp::NodeOptions options) RCLCPP_DEBUG( logger_, "Aggregator critical publisher configured to: %s", (critical_ ? "true" : "false")); - n_ = std::shared_ptr(this, [](rclcpp::Node *) {}); - analyzer_group_ = std::make_unique(); - if (!analyzer_group_->init(base_path_, "", n_)) { - RCLCPP_ERROR(logger_, "Analyzer group for diagnostic aggregator failed to initialize!"); - } + { // lock the mutex while analyzer_group_ and other_analyzer_ are being updated + std::lock_guard lock(mutex_); + n_ = std::shared_ptr(this, [](rclcpp::Node *) {}); + analyzer_group_ = std::make_unique(); + if (!analyzer_group_->init(base_path_, "", n_)) { + RCLCPP_ERROR(logger_, "Analyzer group for diagnostic aggregator failed to initialize!"); + } - // Last analyzer handles remaining data - other_analyzer_ = std::make_unique(other_as_errors); - other_analyzer_->init(base_path_); // This always returns true + // Last analyzer handles remaining data + other_analyzer_ = std::make_unique(other_as_errors); + other_analyzer_->init(base_path_); // This always returns true + } diag_sub_ = create_subscription( "/diagnostics", rclcpp::SystemDefaultsQoS().keep_last(history_depth_), @@ -150,28 +183,11 @@ void Aggregator::diagCallback(const DiagnosticArray::SharedPtr diag_msg) checkTimestamp(diag_msg); bool analyzed = false; + bool immediate_report = false; { // lock the whole loop to ensure nothing in the analyzer group changes during it. std::lock_guard lock(mutex_); for (auto j = 0u; j < diag_msg->status.size(); ++j) { analyzed = false; - - const bool top_level_state_transition_to_error = - (last_top_level_state_ != DiagnosticStatus::ERROR) && - (diag_msg->status[j].level == DiagnosticStatus::ERROR); - - if (critical_ && top_level_state_transition_to_error) { - RCLCPP_DEBUG( - logger_, "Received error message: %s, publishing error immediately", - diag_msg->status[j].name.c_str()); - DiagnosticStatus diag_toplevel_state; - diag_toplevel_state.name = "toplevel_state_critical"; - diag_toplevel_state.level = diag_msg->status[j].level; - toplevel_state_pub_->publish(diag_toplevel_state); - - // store the last published state - last_top_level_state_ = diag_toplevel_state.level; - } - auto item = std::make_shared(&diag_msg->status[j]); if (analyzer_group_->match(item->getName())) { @@ -181,8 +197,17 @@ void Aggregator::diagCallback(const DiagnosticArray::SharedPtr diag_msg) if (!analyzed) { other_analyzer_->analyze(item); } + + // In case there is a degraded state, publish immediately + if (critical_ && item->getLevel() > last_top_level_state_) { + immediate_report = true; + } } } + + if (immediate_report) { + publishData(); + } } Aggregator::~Aggregator() diff --git a/diagnostic_aggregator/test/add_analyzers.launch.py.in b/diagnostic_aggregator/test/add_analyzers.launch.py.in new file mode 100644 index 000000000..8e4eae8da --- /dev/null +++ b/diagnostic_aggregator/test/add_analyzers.launch.py.in @@ -0,0 +1,74 @@ +import os + +import unittest + +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch.events import matches_action +from launch.events.process import ShutdownProcess + +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing.util +import launch_testing_ros + + +def generate_test_description(): + os.environ['OSPL_VERBOSITY'] = '8' + os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}' + + aggregator_node = ExecuteProcess( + cmd=[ + "@AGGREGATOR_NODE@", + "--ros-args", + "--params-file", + "@PARAMETER_FILE@" + ], + name='aggregator_node', + emulate_tty=True, + output='screen') + + add_analyzer = ExecuteProcess( + cmd=[ + "@ADD_ANALYZER@", + "--ros-args", + "--params-file", + "@ADD_PARAMETER_FILE@" + ], + name='add_analyzer', + emulate_tty=True, + output='screen') + + launch_description = LaunchDescription() + launch_description.add_action(aggregator_node) + launch_description.add_action(add_analyzer) + launch_description.add_action(launch_testing.util.KeepAliveProc()) + launch_description.add_action(launch_testing.actions.ReadyToTest()) + return launch_description, locals() + +class TestAggregator(unittest.TestCase): + + def test_processes_output(self, proc_output, aggregator_node): + """Check aggregator logging output for expected strings.""" + + from launch_testing.tools.output import get_default_filtered_prefixes + output_filter = launch_testing_ros.tools.basic_output_filter( + filtered_prefixes=get_default_filtered_prefixes() + ['service not available, waiting...'], + filtered_rmw_implementation='@rmw_implementation@' + ) + proc_output.assertWaitFor( + expected_output=launch_testing.tools.expected_output_from_file(path="@EXPECTED_OUTPUT@"), + process=aggregator_node, + output_filter=output_filter, + timeout=15 + ) + + import time + time.sleep(1) + +@launch_testing.post_shutdown_test() +class TestAggregatorShutdown(unittest.TestCase): + + def test_last_process_exit_code(self, proc_info, aggregator_node): + launch_testing.asserts.assertExitCodes(proc_info, process=aggregator_node) diff --git a/diagnostic_aggregator/test/all_analyzers.yaml b/diagnostic_aggregator/test/all_analyzers.yaml index 84b330e34..4cb012a83 100644 --- a/diagnostic_aggregator/test/all_analyzers.yaml +++ b/diagnostic_aggregator/test/all_analyzers.yaml @@ -1,4 +1,4 @@ -analyzers: +/**: ros__parameters: path: BASIC prefix1: diff --git a/diagnostic_aggregator/test/analyzer_group.yaml b/diagnostic_aggregator/test/analyzer_group.yaml index 72bb5639d..14c938fff 100644 --- a/diagnostic_aggregator/test/analyzer_group.yaml +++ b/diagnostic_aggregator/test/analyzer_group.yaml @@ -1,4 +1,4 @@ -analyzers: +/**: ros__parameters: path: TEST primary: diff --git a/diagnostic_aggregator/test/default.yaml b/diagnostic_aggregator/test/default.yaml new file mode 100644 index 000000000..2da82b92f --- /dev/null +++ b/diagnostic_aggregator/test/default.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + path: BASIC + prefix0: + type: diagnostic_aggregator/GenericAnalyzer + path: Zeroth + contains: [ + 'contain0a', + 'contain0b' ] \ No newline at end of file diff --git a/diagnostic_aggregator/test/empty_root_path.yaml b/diagnostic_aggregator/test/empty_root_path.yaml index 391de4e99..b4b25509f 100644 --- a/diagnostic_aggregator/test/empty_root_path.yaml +++ b/diagnostic_aggregator/test/empty_root_path.yaml @@ -1,4 +1,4 @@ -analyzers: +/**: ros__parameters: primary: type: 'diagnostic_aggregator/AnalyzerGroup' diff --git a/diagnostic_aggregator/test/expected_output/add_all_analyzers.txt b/diagnostic_aggregator/test/expected_output/add_all_analyzers.txt new file mode 100644 index 000000000..9c5c2fc23 --- /dev/null +++ b/diagnostic_aggregator/test/expected_output/add_all_analyzers.txt @@ -0,0 +1,6 @@ +/BASIC/Zeroth +prefix0 +/BASIC/First +prefix1 +/BASIC/Third +prefix3 \ No newline at end of file diff --git a/diagnostic_aggregator/test/expected_stale_analyzers.yaml b/diagnostic_aggregator/test/expected_stale_analyzers.yaml index 9110f84d6..9546204d5 100644 --- a/diagnostic_aggregator/test/expected_stale_analyzers.yaml +++ b/diagnostic_aggregator/test/expected_stale_analyzers.yaml @@ -1,4 +1,4 @@ -analyzers: +/**: my_path: type: diagnostic_aggregator/GenericAnalyzer path: My Path diff --git a/diagnostic_aggregator/test/multiple_match_analyzers.yaml b/diagnostic_aggregator/test/multiple_match_analyzers.yaml index 46153c6a7..3f0ec91f4 100644 --- a/diagnostic_aggregator/test/multiple_match_analyzers.yaml +++ b/diagnostic_aggregator/test/multiple_match_analyzers.yaml @@ -1,4 +1,4 @@ -analyzers: +/**: my_path: type: diagnostic_aggregator/GenericAnalyzer path: Header1 diff --git a/diagnostic_aggregator/test/primitive_analyzers.yaml b/diagnostic_aggregator/test/primitive_analyzers.yaml index 9601cce77..fc98e2ee8 100644 --- a/diagnostic_aggregator/test/primitive_analyzers.yaml +++ b/diagnostic_aggregator/test/primitive_analyzers.yaml @@ -1,4 +1,4 @@ -analyzers: +/**: ros__parameters: log_level: debug primary: diff --git a/diagnostic_aggregator/test/test_critical_pub.py b/diagnostic_aggregator/test/test_critical_pub.py index 279e82957..adad7ed7d 100644 --- a/diagnostic_aggregator/test/test_critical_pub.py +++ b/diagnostic_aggregator/test/test_critical_pub.py @@ -67,41 +67,56 @@ def publish_message(self, level): rclpy.spin_once(self.node) return self.node.get_clock().now() - def test_critical_publisher(self): + def critical_publisher_test( + self, initial_state=DiagnosticStatus.OK, new_state=DiagnosticStatus.ERROR + ): # Publish the ok message and wait till the toplevel state is received - state = DiagnosticStatus.OK - time_0 = self.publish_message(state) + time_0 = self.publish_message(initial_state) - assert (self.received_state[0] == state), \ + assert (self.received_state[0] == initial_state), \ ('Received state is not the same as the sent state:' - + f"'{self.received_state[0]}' != '{state}'") + + f"'{self.received_state[0]}' != '{initial_state}'") self.received_state.clear() # Publish the ok message and expect the toplevel state after 1 second period - time_1 = self.publish_message(state) + time_1 = self.publish_message(initial_state) assert (time_1 - time_0 > rclpy.duration.Duration(seconds=0.99)), \ 'OK message received too early' - assert (self.received_state[0] == state), \ + assert (self.received_state[0] == initial_state), \ ('Received state is not the same as the sent state:' - + f"'{self.received_state[0]}' != '{state}'") + + f"'{self.received_state[0]}' != '{initial_state}'") self.received_state.clear() # Publish the message and expect the critical error message immediately - state = DiagnosticStatus.ERROR - time_2 = self.publish_message(state) + time_2 = self.publish_message(new_state) assert (time_2 - time_1 < rclpy.duration.Duration(seconds=0.1)), \ 'Critical error message not received within 0.1 second' - assert (self.received_state[0] == state), \ + assert (self.received_state[0] == new_state), \ ('Received state is not the same as the sent state:' - + f"'{self.received_state[0]}' != '{state}'") + + f"'{self.received_state[0]}' != '{new_state}'") self.received_state.clear() # Next error message should be sent at standard 1 second rate - time_3 = self.publish_message(state) + time_3 = self.publish_message(new_state) assert (time_3 - time_1 > rclpy.duration.Duration(seconds=0.99)), \ 'Periodic error message received too early' - assert (self.received_state[0] == state), \ + assert (self.received_state[0] == new_state), \ ('Received state is not the same as the sent state:' - + f"'{self.received_state[0]}' != '{state}'") + + f"'{self.received_state[0]}' != '{new_state}'") + + def test_critical_publisher_ok_error(self): + self.critical_publisher_test( + initial_state=DiagnosticStatus.OK, new_state=DiagnosticStatus.ERROR + ) + + def test_critical_publisher_ok_warn(self): + self.critical_publisher_test( + initial_state=DiagnosticStatus.OK, new_state=DiagnosticStatus.WARN + ) + + def test_critical_publisher_warn_error(self): + self.critical_publisher_test( + initial_state=DiagnosticStatus.WARN, new_state=DiagnosticStatus.ERROR + ) diff --git a/diagnostic_aggregator/test/test_discard_behavior.py b/diagnostic_aggregator/test/test_discard_behavior.py new file mode 100644 index 000000000..16dee0162 --- /dev/null +++ b/diagnostic_aggregator/test/test_discard_behavior.py @@ -0,0 +1,299 @@ +# Copyright 2023 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. +# +# DESCRIPTION +# This test ensures that a parent AnalyzerGroup does not roll up an ERROR state when a +# GenericAnalyzer child block is marked with discard_stale: true and either of these +# conditions are met: +# +# 1. There are no statuses that match any of the GenericAnalyzer child blocks. +# 2. Every matching status in the GenericAnalyzer child block has been marked stale +# +# In this example, if foo and bar have no matching statuses or all of their statuses +# are STALE, they will roll up as OK because the discard_stale: true flag implies that +# stale statuses are disposable. +# +# analyzer: +# ros__parameters: +# path: 'agg' +# pub_rate: 1.0 +# analyzers: +# part: +# type: 'diagnostic_aggregator/AnalyzerGroup' +# path: 'part' +# foo: +# type: 'diagnostic_aggregator/GenericAnalyzer' +# path: 'foo' +# find_and_remove_prefix: ['/foo:'] +# num_items: 1 +# bar: +# type: 'diagnostic_aggregator/GenericAnalyzer' +# path: 'bar' +# find_and_remove_prefix: ['/bar:'] +# discard_stale: true + +# Python includes. +from collections import namedtuple +import tempfile + +# ROS2 includes. +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus +import launch +import launch_pytest +import launch_ros +import pytest +import rclpy +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from rclpy.task import Future + + +# All tests take a common structure. +TestMetadata = namedtuple( + 'TestMetadata', + ['foo_discard', 'foo_status', 'bar_discard', 'bar_status', 'agg_expected'], +) + +# A status value of 'None' means that the state is never sent (it's missing). +TEST_METADATA = [ + # CASE 1: both 'foo' and 'bar' are marked discard_stale := true + TestMetadata( + foo_discard=True, + foo_status=None, + bar_discard=True, + bar_status=None, + agg_expected=DiagnosticStatus.OK, + ), + TestMetadata( + foo_discard=True, + foo_status=DiagnosticStatus.STALE, + bar_discard=True, + bar_status=DiagnosticStatus.STALE, + agg_expected=DiagnosticStatus.OK, + ), + TestMetadata( + foo_discard=True, + foo_status=None, + bar_discard=True, + bar_status=DiagnosticStatus.STALE, + agg_expected=DiagnosticStatus.OK, + ), + # CASE 2: both 'foo' and 'bar' are marked discard_stale := false + TestMetadata( + foo_discard=False, + foo_status=None, + bar_discard=False, + bar_status=None, + agg_expected=DiagnosticStatus.STALE, + ), + TestMetadata( + foo_discard=False, + foo_status=DiagnosticStatus.STALE, + bar_discard=False, + bar_status=DiagnosticStatus.STALE, + agg_expected=DiagnosticStatus.STALE, + ), + TestMetadata( + foo_discard=False, + foo_status=None, + bar_discard=False, + bar_status=DiagnosticStatus.STALE, + agg_expected=DiagnosticStatus.STALE, + ), + # CASE 3: one of 'foo' or 'bar' are marked discard_stale := true + TestMetadata( + foo_discard=True, + foo_status=None, + bar_discard=False, + bar_status=None, + agg_expected=DiagnosticStatus.ERROR, # <-- This is the case we are testing for. + # if one of the children is *not* marked discard_stale := true and + # there are no statuses, then the parent should roll up to ERROR. + ), + TestMetadata( + foo_discard=True, + foo_status=DiagnosticStatus.OK, + bar_discard=False, + bar_status=None, + agg_expected=DiagnosticStatus.ERROR, + ), + TestMetadata( + foo_discard=True, + foo_status=None, + bar_discard=False, + bar_status=DiagnosticStatus.OK, + agg_expected=DiagnosticStatus.OK, # <-- This is the case we are testing for. + # but if a child is marked discard_stale := true and there are no statuses, + # the parent should roll up to OK. + ), + TestMetadata( + foo_discard=True, + foo_status=DiagnosticStatus.OK, + bar_discard=False, + bar_status=DiagnosticStatus.OK, + agg_expected=DiagnosticStatus.OK, + ), +] + + +class DiagnosticsTestNode(Node): + """Class that publishes raw diagnostics and listens for aggregated diagnostics.""" + + def __init__(self, foo_status, bar_status, agg_expected): + super().__init__(node_name='diagnostics_listener_node') + self.foo_status = foo_status + self.bar_status = bar_status + self.agg_expected = agg_expected + self.agg_received = None + self.counter = 0 + self.future = Future() + self.subscriber = self.create_subscription( + msg_type=DiagnosticArray, + topic='/diagnostics_agg', + callback=self.diagnostics_aggregated_callback, + qos_profile=10, + ) + self.publisher = self.create_publisher( + msg_type=DiagnosticArray, topic='/diagnostics', qos_profile=10 + ) + self.timer = self.create_timer(0.1, self.timer_callback) + + def timer_callback(self): + """Call from a timer to send off raw diagnostics.""" + msg = DiagnosticArray() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = 'robot' + if self.foo_status is not None: + msg.status.append( + DiagnosticStatus( + name='/foo', level=self.foo_status, message='Foo', hardware_id='foo' + ) + ) + if self.bar_status is not None: + msg.status.append( + DiagnosticStatus( + name='/bar', level=self.bar_status, message='Bar', hardware_id='bar' + ) + ) + self.publisher.publish(msg) + + def diagnostics_aggregated_callback(self, msg): + """Call from a subscriber providing aggregated diagnostics.""" + for status in msg.status: + if status.name == '/robot/agg': + self.agg_received = status.level + self.counter += 1 + if self.agg_expected == status.level: + # Diagnostics may take a few iterations to 'settle' into the right + # state because of how the STALE logic is applied. So, we keep checking + # the aggregator result until it reaches the value we are expecting, + # and then trigger the future. + self.future.set_result(self.counter) + + +@pytest.fixture(scope='function') +def test_metadata(request): + """Enable parameter indirection, so we can pass a parameterization into fixtures.""" + return request.param + + +@pytest.fixture(scope='function') +def yaml_file(test_metadata): + """Generate a YAML file to test a specific configuration state.""" + with tempfile.NamedTemporaryFile(delete=False) as fp: + fp.write( + bytes( + f""" +diagnostic_aggregator: + ros__parameters: + path: 'robot' + pub_rate: 1.0 + analyzers: + part: + type: 'diagnostic_aggregator/AnalyzerGroup' + path: 'agg' + timeout: 2.0 + foo: + type: 'diagnostic_aggregator/GenericAnalyzer' + path: 'foo' + find_and_remove_prefix: ['/foo'] + discard_stale: {test_metadata.foo_discard} + bar: + type: 'diagnostic_aggregator/GenericAnalyzer' + path: 'bar' + find_and_remove_prefix: ['/bar'] + discard_stale: {test_metadata.bar_discard} +""", + 'utf-8', + ) + ) + return fp.name + + +@pytest.fixture(scope='function') +def diagnostic_aggregator_node(): + """Declare an aggregator that uses a global configuration set by the launch.""" + return launch_ros.actions.Node( + name='diagnostic_aggregator', + package='diagnostic_aggregator', + executable='aggregator_node', + ) + + +@launch_pytest.fixture(scope='function') +def launch_description(yaml_file, diagnostic_aggregator_node): + """Declare what should be launched in each test.""" + return launch.LaunchDescription( + [ + launch_ros.actions.SetParametersFromFile(yaml_file), + diagnostic_aggregator_node, + launch_pytest.actions.ReadyToTest(), + ] + ) + + +@pytest.mark.parametrize('test_metadata', TEST_METADATA, indirect=True) +@pytest.mark.launch(fixture=launch_description) +def test_discard_behavior(test_metadata, launch_context): + """Run a launch test for each test in our set of tests.""" + rclpy.init() + + node = DiagnosticsTestNode( + foo_status=test_metadata.foo_status, + bar_status=test_metadata.bar_status, + agg_expected=test_metadata.agg_expected, + ) + + executor = SingleThreadedExecutor() + executor.add_node(node) + + try: + executor.spin_until_future_complete(future=node.future, timeout_sec=10.0) + print( + f""" + The test produced the following result: + + foo_level: {test_metadata.foo_status} (discard: {test_metadata.foo_discard}) + + bar_level: {test_metadata.bar_status} (discard: {test_metadata.bar_discard}) + Expected level: {test_metadata.agg_expected} + Received level: {node.agg_received} + """ + ) + assert node.future.done(), 'Launch timed out without producing aggregation' + assert ( + node.agg_received == test_metadata.agg_expected + ), 'Unexpected parent status level' + print(f'It took {node.future.result()} aggregations to find the correct status') + + finally: + rclpy.try_shutdown() diff --git a/diagnostic_common_diagnostics/CHANGELOG.rst b/diagnostic_common_diagnostics/CHANGELOG.rst index 506989c68..727be8108 100644 --- a/diagnostic_common_diagnostics/CHANGELOG.rst +++ b/diagnostic_common_diagnostics/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package diagnostic_common_diagnostics ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.3.1 (2024-07-30) +------------------ +* fixing pep257 problems introduced by `#334 `_ (`#384 `_) +* Port hd_monitor to ROS2 (`#334 `_) +* Contributors: Antoine Lima, Christian Henkel + +3.2.1 (2024-06-27) +------------------ +* refactor(sensors_monitor): ros2 port `#339 `_ +* refactor(ram_monitor): ros2 port (`#338 `_) +* NTP monitor improvements (`#342 `_) +* Using ubuntu ntp server in systemtest (`#346 `_) +* Fixing ntp launchtest (`#330 `_) +* Contributors: Christian Henkel, Rein Appeldoorn, Tony Najjar + +3.2.0 (2024-03-22) +------------------ +* Port cpu_monitor to ROS2 (`#326 `_) +* Debugging instability introduced by `#317 `_ (`#323 `_) +* not testing on foxy any more (`#310 `_) +* Iron support (`#304 `_) +* Contributors: Christian Henkel, Richard + 3.1.2 (2023-03-24) ------------------ * replacing ntpdate with ntplib (`#289 `_) diff --git a/diagnostic_common_diagnostics/CMakeLists.txt b/diagnostic_common_diagnostics/CMakeLists.txt index 65720a08e..261253671 100644 --- a/diagnostic_common_diagnostics/CMakeLists.txt +++ b/diagnostic_common_diagnostics/CMakeLists.txt @@ -8,19 +8,32 @@ find_package(ament_cmake_python REQUIRED) ament_python_install_package(${PROJECT_NAME}) install(PROGRAMS + ${PROJECT_NAME}/cpu_monitor.py ${PROJECT_NAME}/ntp_monitor.py + ${PROJECT_NAME}/ram_monitor.py + ${PROJECT_NAME}/sensors_monitor.py + ${PROJECT_NAME}/hd_monitor.py DESTINATION lib/${PROJECT_NAME} ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_pytest REQUIRED) ament_add_pytest_test( - test_ntp_monitor - test/systemtest/test_ntp_monitor.py + test_cpu_monitor + test/systemtest/test_cpu_monitor.py TIMEOUT 10) + add_launch_test( + test/systemtest/test_ntp_monitor_launchtest.py + TARGET ntp_monitor_launchtest + TIMEOUT 20) + add_launch_test( + test/systemtest/test_hd_monitor_launchtest.py + TARGET hd_monitor_launchtest + TIMEOUT 20) endif() -ament_package() \ No newline at end of file +ament_package() diff --git a/diagnostic_common_diagnostics/README.md b/diagnostic_common_diagnostics/README.md index 452d163ed..fdece7f82 100644 --- a/diagnostic_common_diagnostics/README.md +++ b/diagnostic_common_diagnostics/README.md @@ -7,8 +7,31 @@ Currently only the NTP monitor is ported to ROS2. # Nodes +## cpu_monitor.py +The `cpu_monitor` module allows users to monitor the CPU usage of their system in real-time. +It publishes the usage percentage in a diagnostic message. + +* Name of the node is "cpu_monitor_" + hostname. +* Uses the following args: + * warning_percentage: If the CPU usage is > warning_percentage, a WARN status will be publised. + * window: the maximum length of the used collections.deque for queuing CPU readings. + +### Published Topics +#### /diagnostics +diagnostic_msgs/DiagnosticArray +The diagnostics information. + +### Parameters +#### warning_percentage +(default: 90) +warning percentage threshold. + +#### window +(default: 1) +Length of CPU readings queue. + ## ntp_monitor.py -Runs 'ntpdate' to check if the system clock is synchronized with the NTP server. +Runs 'ntpdate' to check if the system clock is synchronized with the NTP server. * If the offset is smaller than `offset-tolerance`, an `OK` status will be published. * If the offset is larger than the configured `offset-tolerance`, a `WARN` status will be published, * if it is bigger than `error-offset-tolerance`, an `ERROR` status will be published. @@ -20,7 +43,7 @@ diagnostic_msgs/DiagnosticArray The diagnostics information. ### Parameters -#### ntp_hostname +#### ntp_hostname (default: "pool.ntp.org") Hostname of NTP server. @@ -44,16 +67,59 @@ Computer name in diagnostics output (ex: 'c1') Disable self test. ## hd_monitor.py -**To be ported** +Runs 'shutil.disk_usage' to check if there is enough space left on a given device. +* Above 5% of free space left, an `OK` status will be published. +* Between 5% and 1%, a `WARN` status will be published, +* Below 1%, an `ERROR` status will be published. -## cpu_monitor.py -**To be ported** +### Published Topics +#### /diagnostics +diagnostic_msgs/DiagnosticArray +The diagnostics information. + +### Parameters +#### path +(default: home directory "~") +Path in which to check remaining space. ## ram_monitor.py -**To be ported** +The `ram_monitor` module allows users to monitor the RAM usage of their system in real-time. +It publishes the usage percentage in a diagnostic message. + +* Name of the node is "ram_monitor_" + hostname. +* Uses the following args: + * warning_percentage: If the RAM usage is > warning_percentage, a WARN status will be published. + * window: the maximum length of the used collections.deque for queuing RAM readings. + +### Published Topics +#### /diagnostics +diagnostic_msgs/DiagnosticArray +The diagnostics information. + +### Parameters +#### warning_percentage +(default: 90) +warning percentage threshold. + +#### window +(default: 1) +Length of RAM readings queue. ## sensors_monitor.py -**To be ported** +The `sensors_monitor` module allows users to monitor the temperature, volt and fan speeds of their system in real-time. +It uses the [`LM Sensors` package](https://packages.debian.org/sid/utils/lm-sensors) to get the data. + +* Name of the node is "sensors_monitor_" + hostname. + +### Published Topics +#### /diagnostics +diagnostic_msgs/DiagnosticArray +The diagnostics information. + +### Parameters +#### ignore_fans +(default: false) +Whether to ignore the fan speed. ## tf_monitor.py -**To be ported** \ No newline at end of file +**To be ported** diff --git a/diagnostic_common_diagnostics/diagnostic_common_diagnostics/cpu_monitor.py b/diagnostic_common_diagnostics/diagnostic_common_diagnostics/cpu_monitor.py new file mode 100755 index 000000000..976a57437 --- /dev/null +++ b/diagnostic_common_diagnostics/diagnostic_common_diagnostics/cpu_monitor.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2017, TNO IVS, Helmond, Netherlands +# 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 TNO IVS nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# \author Rein Appeldoorn + +from collections import deque +import socket +import traceback + +from diagnostic_msgs.msg import DiagnosticStatus + +from diagnostic_updater import DiagnosticTask, Updater + +import psutil + +import rclpy +from rclpy.node import Node + + +class CpuTask(DiagnosticTask): + + def __init__(self, warning_percentage=90, window=1): + DiagnosticTask.__init__(self, 'CPU Information') + + self._warning_percentage = int(warning_percentage) + self._readings = deque(maxlen=window) + + def _get_average_reading(self): + def avg(lst): + return float(sum(lst)) / len(lst) if lst else float('nan') + + return [avg(cpu_percentages) + for cpu_percentages in zip(*self._readings)] + + def run(self, stat): + self._readings.append(psutil.cpu_percent(percpu=True)) + cpu_percentages = self._get_average_reading() + cpu_average = sum(cpu_percentages) / len(cpu_percentages) + + stat.add('CPU Load Average', f'{cpu_average:.2f}') + + warn = False + for idx, cpu_percentage in enumerate(cpu_percentages): + stat.add(f'CPU {idx} Load', f'{cpu_percentage:.2f}') + if cpu_percentage > self._warning_percentage: + warn = True + + if warn: + stat.summary(DiagnosticStatus.WARN, + f'At least one CPU exceeds {self._warning_percentage} percent') + else: + stat.summary(DiagnosticStatus.OK, + f'CPU Average {cpu_average:.2f} percent') + + return stat + + +def get_cpu_diagnostics_node() -> Node: + """Get cpu diagnostics node.""" + # Create the node + hostname = socket.gethostname() + node = Node('cpu_monitor') + + # Declare and get parameters + node.declare_parameter('warning_percentage', 90) + node.declare_parameter('window', 1) + + warning_percentage = node.get_parameter( + 'warning_percentage').get_parameter_value().integer_value + window = node.get_parameter('window').get_parameter_value().integer_value + + # Create diagnostic updater with default updater rate of 1 hz + updater = Updater(node) + updater.setHardwareID(hostname) + updater.add(CpuTask(warning_percentage=warning_percentage, window=window)) + + return node + + +def main(args=None): + rclpy.init(args=args) + node = get_cpu_diagnostics_node() + rclpy.spin(node) + + +if __name__ == '__main__': + try: + main() + except KeyboardInterrupt: + pass + except Exception: + traceback.print_exc() diff --git a/diagnostic_common_diagnostics/diagnostic_common_diagnostics/hd_monitor.py b/diagnostic_common_diagnostics/diagnostic_common_diagnostics/hd_monitor.py new file mode 100755 index 000000000..ac4eae748 --- /dev/null +++ b/diagnostic_common_diagnostics/diagnostic_common_diagnostics/hd_monitor.py @@ -0,0 +1,156 @@ +#! /usr/bin/env python3 +"""Hard Drive (or any other memory) monitor. Contains a the monitor node and its main function.""" +# -*- coding: utf-8 -*- +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2009, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# \author Kevin Watts +# \author Antoine Lima + +from pathlib import Path +from shutil import disk_usage +from socket import gethostname +from typing import List + +from diagnostic_msgs.msg import DiagnosticStatus, KeyValue +from diagnostic_updater import Updater +from rcl_interfaces.msg import SetParametersResult +import rclpy +from rclpy.node import Node + + +FREE_PERCENT_LOW = 0.05 +FREE_PERCENT_CRIT = 0.01 +DICT_STATUS = { + DiagnosticStatus.OK: 'OK', + DiagnosticStatus.WARN: 'Warning', + DiagnosticStatus.ERROR: 'Error', +} +DICT_USAGE = { + DiagnosticStatus.OK: 'OK', + DiagnosticStatus.WARN: 'Low Disk Space', + DiagnosticStatus.ERROR: 'Very Low Disk Space', +} + + +class HDMonitor(Node): + """ + Diagnostic node checking the remaining space on the specified hard drive. + + Three ROS parameters: + - path: Path on the filesystem to check (string, default: home directory) + - free_percent_low: Percentage at which to consider the space left as low + - free_percent_crit: Percentage at which to consider the space left as critical + """ + + def __init__(self): + hostname = gethostname().replace('.', '_').replace('-', '_') + super().__init__(f'hd_monitor_{hostname}') + + self._path = '~' + self._free_percent_low = 0.05 + self._free_percent_crit = 0.01 + + self.add_on_set_parameters_callback(self.callback_config) + self.declare_parameter('path', self._path) + self.declare_parameter('free_percent_low', self._free_percent_low) + self.declare_parameter('free_percent_crit', self._free_percent_crit) + + self._updater = Updater(self) + self._updater.setHardwareID(hostname) + self._updater.add(f'{hostname} HD Usage', self.check_disk_usage) + + def callback_config(self, params: List[rclpy.Parameter]): + """ + Retrieve ROS parameters. + + see the class documentation for declared parameters. + """ + for param in params: + match param.name: + case 'path': + self._path = str( + Path(param.value).expanduser().resolve(strict=True) + ) + case 'free_percent_low': + self._free_percent_low = param.value + case 'free_percent_crit': + self._free_percent_crit = param.value + + return SetParametersResult(successful=True) + + def check_disk_usage(self, diag: DiagnosticStatus) -> DiagnosticStatus: + """ + Compute the disk usage and derive a status from it. + + Task periodically ran by the diagnostic updater. + """ + diag.level = DiagnosticStatus.OK + + total, _, free = disk_usage(self._path) + percent = free / total + + if percent > self._free_percent_low: + diag.level = DiagnosticStatus.OK + elif percent > self._free_percent_crit: + diag.level = DiagnosticStatus.WARN + else: + diag.level = DiagnosticStatus.ERROR + + total_go = total // (1024 * 1024) + diag.values.extend( + [ + KeyValue(key='Name', value=self._path), + KeyValue(key='Status', value=DICT_STATUS[diag.level]), + KeyValue(key='Total (Go)', value=str(total_go)), + KeyValue(key='Available (%)', value=str(round(percent, 2))), + ] + ) + + diag.message = DICT_USAGE[diag.level] + return diag + + +def main(args=None): + """Run the HDMonitor class.""" + rclpy.init(args=args) + + node = HDMonitor() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + + +if __name__ == '__main__': + main() diff --git a/diagnostic_common_diagnostics/diagnostic_common_diagnostics/ntp_monitor.py b/diagnostic_common_diagnostics/diagnostic_common_diagnostics/ntp_monitor.py index 4d52e9e84..461d17bde 100755 --- a/diagnostic_common_diagnostics/diagnostic_common_diagnostics/ntp_monitor.py +++ b/diagnostic_common_diagnostics/diagnostic_common_diagnostics/ntp_monitor.py @@ -37,9 +37,7 @@ import threading import diagnostic_updater as DIAG - import ntplib - import rclpy from rclpy.node import Node @@ -47,13 +45,16 @@ class NTPMonitor(Node): """A diagnostic task that monitors the NTP offset of the system clock.""" - def __init__(self, ntp_hostname, offset=500, self_offset=500, + def __init__(self, ntp_hostname, ntp_port, offset=500, self_offset=500, diag_hostname=None, error_offset=5000000, do_self_test=True): """Initialize the NTPMonitor.""" super().__init__(__class__.__name__) + self.declare_parameter('frequency', 10.0) + frequency = self.get_parameter('frequency').get_parameter_value().double_value self.ntp_hostname = ntp_hostname + self.ntp_port = ntp_port self.offset = offset self.self_offset = self_offset self.diag_hostname = diag_hostname @@ -67,7 +68,8 @@ def __init__(self, ntp_hostname, offset=500, self_offset=500, self.stat = DIAG.DiagnosticStatus() self.stat.level = DIAG.DiagnosticStatus.OK self.stat.name = 'NTP offset from ' + \ - self.diag_hostname + ' to ' + self.ntp_hostname + self.diag_hostname + ' to ' + self.ntp_hostname + \ + ':' + str(self.ntp_port) self.stat.message = 'OK' self.stat.hardware_id = self.hostname self.stat.values = [] @@ -85,8 +87,8 @@ def __init__(self, ntp_hostname, offset=500, self_offset=500, # we need to periodically republish this self.current_msg = None - self.pubtimer = self.create_timer(0.1, self.pubCB) - self.checktimer = self.create_timer(0.1, self.checkCB) + self.pubtimer = self.create_timer(1/frequency, self.pubCB) + self.checktimer = self.create_timer(1/frequency, self.checkCB) def pubCB(self): with self.mutex: @@ -95,6 +97,7 @@ def pubCB(self): def checkCB(self): new_msg = DIAG.DiagnosticArray() + new_msg.header.stamp = self.get_clock().now().to_msg() st = self.ntp_diag(self.stat) if st is not None: @@ -127,7 +130,10 @@ def add_kv(stat_values, key, value): ntp_client = ntplib.NTPClient() response = None try: - response = ntp_client.request(self.ntp_hostname, version=3) + response = ntp_client.request( + self.ntp_hostname, + port=self.ntp_port, + version=3) except ntplib.NTPException as e: self.get_logger().error(f'NTP Error: {e}') st.level = DIAG.DiagnosticStatus.ERROR @@ -140,26 +146,32 @@ def add_kv(stat_values, key, value): if (abs(measured_offset) > self.offset): st.level = DIAG.DiagnosticStatus.WARN st.message = \ - f'NTP offset above threshold: {measured_offset}>'\ + f'NTP offset above threshold: abs({measured_offset})>'\ f'{self.offset} us' if (abs(measured_offset) > self.error_offset): st.level = DIAG.DiagnosticStatus.ERROR st.message = \ - f'NTP offset above error threshold: {measured_offset}>'\ + f'NTP offset above error threshold: abs({measured_offset})>'\ f'{self.error_offset} us' if (abs(measured_offset) < self.offset): st.level = DIAG.DiagnosticStatus.OK - st.message = f'NTP Offset OK: {measured_offset} us' + st.message = f'NTP Offset OK: abs({measured_offset}) us' return st def ntp_monitor_main(argv=sys.argv[1:]): + # filter out ROS args + argv = argv[:argv.index('--ros-args')] if '--ros-args' in argv else argv + import argparse parser = argparse.ArgumentParser() parser.add_argument('--ntp_hostname', action='store', default='0.pool.ntp.org', type=str) + parser.add_argument('--ntp_port', + action='store', default=123, + type=int) parser.add_argument('--offset-tolerance', dest='offset_tol', action='store', default=500, help='Offset from NTP host [us]', metavar='OFFSET-TOL', @@ -188,7 +200,8 @@ def ntp_monitor_main(argv=sys.argv[1:]): assert offset < error_offset, \ 'Offset tolerance must be less than error offset tolerance' - ntp_monitor = NTPMonitor(args.ntp_hostname, offset, self_offset, + ntp_monitor = NTPMonitor(args.ntp_hostname, args.ntp_port, + offset, self_offset, args.diag_hostname, error_offset, args.do_self_test) diff --git a/diagnostic_common_diagnostics/diagnostic_common_diagnostics/ram_monitor.py b/diagnostic_common_diagnostics/diagnostic_common_diagnostics/ram_monitor.py new file mode 100755 index 000000000..ea91c3f4f --- /dev/null +++ b/diagnostic_common_diagnostics/diagnostic_common_diagnostics/ram_monitor.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python3 +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2017, TNO IVS, Helmond, Netherlands +# 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 TNO IVS nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# \author Rein Appeldoorn + +import collections +import socket + +from diagnostic_msgs.msg import DiagnosticStatus + +from diagnostic_updater import DiagnosticTask, Updater + +import psutil + +import rclpy +from rclpy.node import Node + + +class RamTask(DiagnosticTask): + + def __init__(self, warning_percentage, window): + DiagnosticTask.__init__(self, 'RAM Information') + self._warning_percentage = int(warning_percentage) + self._readings = collections.deque(maxlen=window) + + def run(self, stat): + self._readings.append(psutil.virtual_memory().percent) + ram_average = sum(self._readings) / len(self._readings) + + stat.add('RAM Load Average', f'{ram_average:.2f}') + + if ram_average > self._warning_percentage: + stat.summary( + DiagnosticStatus.WARN, + f'RAM Average exceeds {self._warning_percentage:d} percent', + ) + else: + stat.summary(DiagnosticStatus.OK, f'RAM Average {ram_average:.2f} percent') + + return stat + + +def get_ram_diagnostics_node() -> Node: + """Get ram diagnostics node.""" + hostname = socket.gethostname() + + node = rclpy.create_node('ram_monitor') + + updater = Updater(node) + updater.setHardwareID(hostname) + updater.add( + RamTask( + node.declare_parameter('warning_percentage', 90).value, + node.declare_parameter('window', 1).value, + ) + ) + + return node + + +def main(): + rclpy.init() + node = get_ram_diagnostics_node() + rclpy.spin(node) + + +if __name__ == '__main__': + try: + main() + except KeyboardInterrupt: + pass diff --git a/diagnostic_common_diagnostics/diagnostic_common_diagnostics/sensors_monitor.py b/diagnostic_common_diagnostics/diagnostic_common_diagnostics/sensors_monitor.py new file mode 100755 index 000000000..c6733e119 --- /dev/null +++ b/diagnostic_common_diagnostics/diagnostic_common_diagnostics/sensors_monitor.py @@ -0,0 +1,255 @@ +#!/usr/bin/env python3 + +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from __future__ import division, with_statement + +from io import StringIO +import math +import re +import socket +import subprocess + +from diagnostic_msgs.msg import DiagnosticStatus + +import diagnostic_updater as DIAG + +import rclpy +from rclpy.node import Node + + +class Sensor(object): + + def __init__(self): + self.critical = None + self.min = None + self.max = None + self.input = None + self.name = None + self.type = None + self.high = None + self.alarm = None + + def __repr__(self): + return 'Sensor object (name: {}, type: {})'.format(self.name, self.type) + + def getCrit(self): + return self.critical + + def getMin(self): + return self.min + + def getMax(self): + return self.max + + def getInput(self): + return self.input + + def getName(self): + return self.name + + def getType(self): + return self.type + + def getHigh(self): + return self.high + + def getAlarm(self): + return self.alarm + + def __str__(self): + lines = [] + lines.append(str(self.name)) + lines.append('\t' + 'Type: ' + str(self.type)) + if self.input: + lines.append('\t' + 'Input: ' + str(self.input)) + if self.min: + lines.append('\t' + 'Min: ' + str(self.min)) + if self.max: + lines.append('\t' + 'Max: ' + str(self.max)) + if self.high: + lines.append('\t' + 'High: ' + str(self.high)) + if self.critical: + lines.append('\t' + 'Crit: ' + str(self.critical)) + lines.append('\t' + 'Alarm: ' + str(self.alarm)) + return '\n'.join(lines) + + +def parse_sensor_line(line): + sensor = Sensor() + line = line.lstrip() + [name, reading] = line.split(':') + + try: + [sensor.name, sensor.type] = name.rsplit(' ', 1) + except ValueError: + return None + + if sensor.name == 'Core': + sensor.name = name + sensor.type = 'Temperature' + elif sensor.name.find('Physical id') != -1: + sensor.name = name + sensor.type = 'Temperature' + + try: + [reading, params] = reading.lstrip().split('(') + except ValueError: + return None + + sensor.alarm = False + if line.find('ALARM') != -1: + sensor.alarm = True + + if reading.find('°C') == -1: + sensor.input = float(reading.split()[0]) + else: + sensor.input = float(reading.split('°C')[0]) + + params = params.split(',') + for param in params: + m = re.search('[0-9]+.[0-9]*', param) + if param.find('min') != -1: + sensor.min = float(m.group(0)) + elif param.find('max') != -1: + sensor.max = float(m.group(0)) + elif param.find('high') != -1: + sensor.high = float(m.group(0)) + elif param.find('crit') != -1: + sensor.critical = float(m.group(0)) + + return sensor + + +def _rads_to_rpm(rads): + return rads / (2 * math.pi) * 60 + + +def _rpm_to_rads(rpm): + return rpm * (2 * math.pi) / 60 + + +def parse_sensors_output(node: Node, output): + out = StringIO(output if isinstance(output, str) else output.decode('utf-8')) + + sensorList = [] + for line in out.readlines(): + # Check for a colon + if ':' in line and 'Adapter' not in line: + s = None + try: + s = parse_sensor_line(line) + except Exception as exc: + node.get_logger().warn( + 'Unable to parse line "%s", due to %s', line, exc + ) + if s is not None: + sensorList.append(s) + return sensorList + + +def get_sensors(): + p = subprocess.Popen( + 'sensors', stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True + ) + (o, e) = p.communicate() + if not p.returncode == 0: + return '' + if not o: + return '' + return o + + +class SensorsMonitor(object): + + def __init__(self, node: Node, hostname): + self.node = node + self.hostname = hostname + self.ignore_fans = node.declare_parameter('ignore_fans', False).value + node.get_logger().info('Ignore fanspeed warnings: %s' % self.ignore_fans) + + self.updater = DIAG.Updater(node) + self.updater.setHardwareID('none') + self.updater.add('%s Sensor Status' % self.hostname, self.monitor) + + def monitor(self, stat): + try: + stat.summary(DiagnosticStatus.OK, 'OK') + for sensor in parse_sensors_output(self.node, get_sensors()): + if sensor.getType() == 'Temperature': + if sensor.getInput() > sensor.getCrit(): + stat.mergeSummary( + DiagnosticStatus.ERROR, 'Critical Temperature' + ) + elif sensor.getInput() > sensor.getHigh(): + stat.mergeSummary(DiagnosticStatus.WARN, 'High Temperature') + stat.add( + ' '.join([sensor.getName(), sensor.getType()]), + str(sensor.getInput()), + ) + elif sensor.getType() == 'Voltage': + if sensor.getInput() < sensor.getMin(): + stat.mergeSummary(DiagnosticStatus.ERROR, 'Low Voltage') + elif sensor.getInput() > sensor.getMax(): + stat.mergeSummary(DiagnosticStatus.ERROR, 'High Voltage') + stat.add( + ' '.join([sensor.getName(), sensor.getType()]), + str(sensor.getInput()), + ) + elif sensor.getType() == 'Speed': + if not self.ignore_fans: + if sensor.getInput() < sensor.getMin(): + stat.mergeSummary(DiagnosticStatus.ERROR, 'No Fan Speed') + stat.add( + ' '.join([sensor.getName(), sensor.getType()]), + str(sensor.getInput()), + ) + except Exception: + import traceback + + self.node.get_logger().error('Unable to process lm-sensors data') + self.node.get_logger().error(traceback.format_exc()) + return stat + + +if __name__ == '__main__': + rclpy.init() + hostname = socket.gethostname() + hostname_clean = hostname.translate(hostname.maketrans('-', '_')) + node = rclpy.create_node('sensors_monitor_%s' % hostname_clean) + + monitor = SensorsMonitor(node, hostname) + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass diff --git a/diagnostic_common_diagnostics/mainpage.dox b/diagnostic_common_diagnostics/mainpage.dox index 25ee9c863..f9cd677de 100644 --- a/diagnostic_common_diagnostics/mainpage.dox +++ b/diagnostic_common_diagnostics/mainpage.dox @@ -4,6 +4,8 @@ \b diagnostic_common_diagnostics contains a few common diagnostic nodes +- cpu_monitor publishes diagnostic messages with the CPU usage of the system. +- hd_monitor publishes diagnostic messages related to the available space on a given storage device. - ntp_monitor publishes diagnostic messages for how well the NTP time sync is working. - tf_monitor used to publish diagnostic messages reporting on the health of the TF tree. It is based on tfwtf. It is not ported to ROS2. diff --git a/diagnostic_common_diagnostics/package.xml b/diagnostic_common_diagnostics/package.xml index 9d50bbe33..a698c4cb1 100644 --- a/diagnostic_common_diagnostics/package.xml +++ b/diagnostic_common_diagnostics/package.xml @@ -2,7 +2,7 @@ diagnostic_common_diagnostics - 3.1.2 + 4.3.1 diagnostic_common_diagnostics Austin Hendrix Brice Rebsamen @@ -18,21 +18,23 @@ ament_cmake ament_cmake_python - rclpy diagnostic_updater + lm-sensors python3-ntplib + python3-psutil ament_lint_auto ament_cmake_xmllint ament_cmake_lint_cmake - + ament_cmake_pytest + launch_testing_ament_cmake ament_cmake diff --git a/diagnostic_common_diagnostics/setup.py b/diagnostic_common_diagnostics/setup.py new file mode 100644 index 000000000..048e933eb --- /dev/null +++ b/diagnostic_common_diagnostics/setup.py @@ -0,0 +1,37 @@ +from setuptools import setup + +package_name = 'diagnostic_common_diagnostics' + +setup( + name=package_name, + version='4.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + [f'resource/{package_name}']), + ('share/' + package_name, ['package.xml']), + ('lib/' + package_name, [ + 'cpu_monitor.py', + 'ntp_monitor.py', + 'ram_monitor.py', + 'sensors_monitor.py', + 'hd_monitor.py', + ]), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='root', + maintainer_email='vladyslav.hrynchak@logivations.com', + description='Package for diagnostics', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'cpu_monitor = diagnostic_common_diagnostics.cpu_monitor:main', + 'ntp_monitor = diagnostic_common_diagnostics.ntp_monitor:main', + 'ram_monitor = diagnostic_common_diagnostics.ram_monitor:main', + 'sensors_monitor = diagnostic_common_diagnostics.sensors_monitor:main', + 'hd_monitor = diagnostic_common_diagnostics.hd_monitor:main', + ], + }, +) diff --git a/diagnostic_common_diagnostics/test/systemtest/test_cpu_monitor.py b/diagnostic_common_diagnostics/test/systemtest/test_cpu_monitor.py new file mode 100644 index 000000000..28430c482 --- /dev/null +++ b/diagnostic_common_diagnostics/test/systemtest/test_cpu_monitor.py @@ -0,0 +1,116 @@ +# -*- coding: utf-8 -*- +# Software License Agreement (BSD License) +# +# Copyright (c) 2023, Robert Bosch GmbH +# 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. + +import time +import unittest + +from diagnostic_common_diagnostics.cpu_monitor import CpuTask + +from diagnostic_msgs.msg import DiagnosticStatus + +from diagnostic_updater import DiagnosticArray, Updater +from diagnostic_updater import DiagnosticStatusWrapper + +import rclpy +from rclpy.node import Node + + +class TestCPUMonitor(unittest.TestCase): + + @classmethod + def setUpClass(cls): + rclpy.init(args=None) + + @classmethod + def tearDownClass(cls): + if rclpy.ok(): + rclpy.shutdown() + + def setUp(self): + # In this case is recommended for accuracy that psutil.cpu_percent() + # function be called with at least 0.1 seconds between calls. + time.sleep(0.1) + + def diagnostics_callback(self, msg): + self.message_recieved = True + self.assertEqual(len(msg.status), 1) + + def test_ok(self): + warning_percentage = 100 + task = CpuTask(warning_percentage) + stat = DiagnosticStatusWrapper() + task.run(stat) + self.assertEqual(task.name, 'CPU Information') + self.assertEqual(stat.level, DiagnosticStatus.OK) + self.assertIn(str('CPU Average'), stat.message) + + # Check for at least 1 CPU Load Average and 1 CPU Load + self.assertGreaterEqual(len(stat.values), 2) + + def test_warn(self): + warning_percentage = -1 + task = CpuTask(warning_percentage) + stat = DiagnosticStatusWrapper() + task.run(stat) + print(f'Raw readings: {task._readings}') + self.assertEqual(task.name, 'CPU Information') + self.assertEqual(stat.level, DiagnosticStatus.WARN) + self.assertIn(str('At least one CPU exceeds'), stat.message) + + # Check for at least 1 CPU Load Average and 1 CPU Load + self.assertGreaterEqual(len(stat.values), 2) + + def test_updater(self): + self.message_recieved = False + + node = Node('cpu_monitor_test') + updater = Updater(node) + updater.setHardwareID('test_id') + updater.add(CpuTask()) + + node.create_subscription( + DiagnosticArray, '/diagnostics', self.diagnostics_callback, 10) + + start_time = time.time() + timeout = 5.0 # Timeout in seconds + + while not self.message_recieved: + rclpy.spin_once(node) + time.sleep(0.1) + elapsed_time = time.time() - start_time + if elapsed_time >= timeout: + self.fail('No diagnostics received') + + +if __name__ == '__main__': + unittest.main() diff --git a/diagnostic_common_diagnostics/test/systemtest/test_hd_monitor_launchtest.py b/diagnostic_common_diagnostics/test/systemtest/test_hd_monitor_launchtest.py new file mode 100644 index 000000000..9086c5d75 --- /dev/null +++ b/diagnostic_common_diagnostics/test/systemtest/test_hd_monitor_launchtest.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# Software License Agreement (BSD License) +# +# Copyright (c) 2023, Robert Bosch GmbH +# 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. + +import unittest + +from diagnostic_msgs.msg import DiagnosticArray + +import launch + +import launch_ros + +import launch_testing + +from launch_testing_ros import WaitForTopics + +import pytest + +import rclpy + + +@pytest.mark.launch_test +def generate_test_description(): + """Launch the hd_monitor node and return a launch description.""" + return launch.LaunchDescription( + [ + launch_ros.actions.Node( + package='diagnostic_common_diagnostics', + executable='hd_monitor.py', + name='hd_monitor', + output='screen', + parameters=[{'free_percent_low': 0.20, 'free_percent_crit': 0.05}], + ), + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestHDMonitor(unittest.TestCase): + """Test if the hd_monitor node is publishing diagnostics.""" + + def __init__(self, methodName: str = 'runTest') -> None: + super().__init__(methodName) + self.received_messages = [] + + def _received_message(self, msg): + self.received_messages.append(msg) + + def _get_min_level(self): + levels = [ + int.from_bytes(status.level, 'little') + for diag in self.received_messages + for status in diag.status + ] + if len(levels) == 0: + return -1 + return min(levels) + + def test_topic_published(self): + """Test if the hd_monitor node is publishing diagnostics.""" + with WaitForTopics([('/diagnostics', DiagnosticArray)], timeout=5): + print('Topic found') + + rclpy.init() + test_node = rclpy.create_node('test_node') + test_node.create_subscription( + DiagnosticArray, '/diagnostics', self._received_message, 1 + ) + + while len(self.received_messages) < 10: + rclpy.spin_once(test_node, timeout_sec=1) + if (min_level := self._get_min_level()) == 0: + break + + test_node.destroy_node() + rclpy.shutdown() + print(f'Got {len(self.received_messages)} messages:') + for msg in self.received_messages: + print(msg) + self.assertEqual(min_level, 0) diff --git a/diagnostic_common_diagnostics/test/systemtest/test_ntp_monitor.py b/diagnostic_common_diagnostics/test/systemtest/test_ntp_monitor.py deleted file mode 100644 index 767ed2ac6..000000000 --- a/diagnostic_common_diagnostics/test/systemtest/test_ntp_monitor.py +++ /dev/null @@ -1,130 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -# Software License Agreement (BSD License) -# -# Copyright (c) 2023, Robert Bosch GmbH -# 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. - -import os -import subprocess -import unittest - -import ament_index_python - -from diagnostic_msgs.msg import DiagnosticArray - -import rclpy - -TIMEOUT_MAX_S = 5. - - -class TestNTPMonitor(unittest.TestCase): - - def __init__(self, methodName: str = 'runTest') -> None: - super().__init__(methodName) - rclpy.init() - self.n_msgs_received = 0 - - def setUp(self): - self.n_msgs_received = 0 - n = self._count_msgs(1.) - self.assertEqual(n, 0) - self.subprocess = subprocess.Popen( - [ - os.path.join( - ament_index_python.get_package_prefix( - 'diagnostic_common_diagnostics' - ), - 'lib', - 'diagnostic_common_diagnostics', - 'ntp_monitor.py' - ) - ] - ) - - def tearDown(self): - self.subprocess.kill() - - def _diagnostics_callback(self, msg): - rclpy.logging.get_logger('test_ntp_monitor').info( - f'Received diagnostics message: {msg}' - ) - search_strings = [ - 'NTP offset from', - 'NTP self-offset for' - ] - for search_string in search_strings: - if search_string in ''.join([ - s.name for s in msg.status - ]): - self.n_msgs_received += 1 - - def _count_msgs(self, timeout_s): - self.n_msgs_received = 0 - node = rclpy.create_node('test_ntp_monitor') - rclpy.logging.get_logger('test_ntp_monitor').info( - '_count_msgs' - ) - node.create_subscription( - DiagnosticArray, - 'diagnostics', - self._diagnostics_callback, - 1 - ) - TIME_D_S = .05 - waited_s = 0. - start = node.get_clock().now() - while waited_s < timeout_s and self.n_msgs_received == 0: - rclpy.spin_once(node, timeout_sec=TIME_D_S) - waited_s = (node.get_clock().now() - start).nanoseconds / 1e9 - rclpy.logging.get_logger('test_ntp_monitor').info( - f'received {self.n_msgs_received} messages after {waited_s}s' - ) - node.destroy_node() - return self.n_msgs_received - - def test_publishing(self): - self.assertEqual( - self.subprocess.poll(), - None, - 'NTP monitor subprocess died' - ) - - n = self._count_msgs(TIMEOUT_MAX_S) - - self.assertGreater( - n, - 0, - f'No messages received within {TIMEOUT_MAX_S}s' - ) - - -if __name__ == '__main__': - unittest.main() diff --git a/diagnostic_common_diagnostics/test/systemtest/test_ntp_monitor_launchtest.py b/diagnostic_common_diagnostics/test/systemtest/test_ntp_monitor_launchtest.py index d70470ed2..ede17b0f4 100644 --- a/diagnostic_common_diagnostics/test/systemtest/test_ntp_monitor_launchtest.py +++ b/diagnostic_common_diagnostics/test/systemtest/test_ntp_monitor_launchtest.py @@ -32,60 +32,79 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -import os - -import ament_index_python +import unittest +from diagnostic_msgs.msg import DiagnosticArray import launch - -import launch_pytest -from launch_pytest.tools import process as process_tools - +import launch_ros import launch_testing - +from launch_testing_ros import WaitForTopics import pytest +import rclpy -@pytest.fixture -def ntp_monitor_proc(): - # Launch a process to test - return launch.actions.ExecuteProcess( - cmd=[ - os.path.join( - ament_index_python.get_package_prefix( - 'diagnostic_common_diagnostics'), - 'lib', - 'diagnostic_common_diagnostics', - 'ntp_monitor.py' - ), - ], - ), - - -@launch_pytest.fixture -def launch_description(ntp_monitor_proc): +@pytest.mark.launch_test +def generate_test_description(): + """Launch the ntp_monitor node and return a launch description.""" return launch.LaunchDescription([ - ntp_monitor_proc, + launch_ros.actions.Node( + package='diagnostic_common_diagnostics', + executable='ntp_monitor.py', + name='ntp_monitor', + output='screen', + arguments=['--offset-tolerance', '100000', + '--error-offset-tolerance', '200000', + '--ntp_hostname', 'ntp.ubuntu.com'] + # 100s, 200s, we are not testing if your clock is correct + ), launch_testing.actions.ReadyToTest() ]) -@pytest.mark.skip(reason='This test is not working yet') -@pytest.mark.launch(fixture=launch_description) -def test_read_stdout(ntp_monitor_proc, launch_context): - """Check if 'ntp_monitor' was found in the stdout.""" - def validate_output(output): - # this function can use assertions to validate the output or return a boolean. - # pytest generates easier to understand failures when assertions are used. - assert output.splitlines() == [ - 'ntp_monitor'], 'process never printed ntp_monitor' - process_tools.assert_output_sync( - launch_context, ntp_monitor_proc, validate_output, timeout=5) +class TestNtpMonitor(unittest.TestCase): + """Test if the ntp_monitor node is publishing diagnostics.""" + + def __init__(self, methodName: str = 'runTest') -> None: + super().__init__(methodName) + self.received_messages = [] + + def _received_message(self, msg): + self.received_messages.append(msg) + + def _get_min_level(self): + levels = [ + int.from_bytes(status.level, 'little') + for diag in self.received_messages + for status in diag.status] + if len(levels) == 0: + return -1 + return min(levels) + + def test_topic_published(self): + """Test if the ntp_monitor node is publishing diagnostics.""" + with WaitForTopics( + [('/diagnostics', DiagnosticArray)], + timeout=5 + ): + print('Topic found') + + rclpy.init() + test_node = rclpy.create_node('test_node') + test_node.create_subscription( + DiagnosticArray, + '/diagnostics', + self._received_message, + 1 + ) + + while len(self.received_messages) < 10: + rclpy.spin_once(test_node, timeout_sec=1) + if (min_level := self._get_min_level()) == 0: + break - def validate_output(output): - return output == 'this will never happen' - assert not process_tools.wait_for_output_sync( - launch_context, ntp_monitor_proc, validate_output, timeout=0.1) - yield - # this is executed after launch service shutdown - assert ntp_monitor_proc.return_code == 0 + test_node.destroy_node() + rclpy.shutdown() + print(f'Got {len(self.received_messages)} messages:') + for msg in self.received_messages: + print(msg) + self.assertEqual(min_level, 0) diff --git a/diagnostic_updater/CHANGELOG.rst b/diagnostic_updater/CHANGELOG.rst index a4e692168..c51706fc9 100644 --- a/diagnostic_updater/CHANGELOG.rst +++ b/diagnostic_updater/CHANGELOG.rst @@ -2,6 +2,27 @@ Changelog for package diagnostic_updater ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.3.1 (2024-07-30) +------------------ +* Fix correctly exporting the library (`#388 `_) (`#393 `_) +* Minimize header includes by moving impl to .cpp files (`#331 `_) and Fix usage of rclcpp::ok with a non-default context (`#352 `_) (`#390 `_) +* Contributors: Christian Henkel, Ramon Wijnands, Hervé Audren + +3.2.1 (2024-06-27) +------------------ +* change(diagnosed-publisher): allow specifying node clock (`#340 `_) +* Fix usage of rclcpp::ok with a non-default context (`#352 `_) +* Contributors: Hervé Audren, Rein Appeldoorn + +3.2.0 (2024-03-22) +------------------ +* including depdency (`#322 `_) +* Debugging instability introduced by `#317 `_ (`#323 `_) +* feat: add param to use fqn in updater (`#320 `_) +* fix: method names & verbose logging (`#307 `_) +* Fix diagnostic_updater timestamps (`#299 `_) +* Contributors: Christian Henkel, Kevin Schwarzer, h-wata, outrider-jhulas + 3.1.2 (2023-03-24) ------------------ diff --git a/diagnostic_updater/CMakeLists.txt b/diagnostic_updater/CMakeLists.txt index cd723d2e6..ebca013ee 100644 --- a/diagnostic_updater/CMakeLists.txt +++ b/diagnostic_updater/CMakeLists.txt @@ -11,22 +11,25 @@ if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_C_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(diagnostic_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(rclpy REQUIRED) find_package(std_msgs REQUIRED) -add_library(${PROJECT_NAME} INTERFACE) +add_library(${PROJECT_NAME} + src/diagnostic_updater.cpp +) target_include_directories( ${PROJECT_NAME} - INTERFACE + PUBLIC $ $ ) ament_target_dependencies( ${PROJECT_NAME} - INTERFACE + PUBLIC diagnostic_msgs rclcpp ) @@ -63,12 +66,10 @@ if(BUILD_TESTING) $ $ ) + target_link_libraries(diagnostic_updater_test ${PROJECT_NAME}) ament_target_dependencies( diagnostic_updater_test - "diagnostic_msgs" - "rclcpp" "rclcpp_lifecycle" - "std_msgs" ) ament_add_gtest(diagnostic_status_wrapper_test test/diagnostic_status_wrapper_test.cpp) @@ -90,11 +91,7 @@ if(BUILD_TESTING) $ $ ) - ament_target_dependencies( - status_msg_test - "diagnostic_msgs" - "rclcpp" - ) + target_link_libraries(status_msg_test ${PROJECT_NAME}) find_package(ament_cmake_pytest REQUIRED) ament_add_pytest_test(diagnostic_updater_test.py "test/diagnostic_updater_test.py") @@ -121,6 +118,7 @@ install( ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) ament_export_dependencies(ament_cmake) ament_export_dependencies(ament_cmake_python) ament_export_dependencies(diagnostic_msgs) diff --git a/diagnostic_updater/include/diagnostic_updater/diagnostic_status_wrapper.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_status_wrapper.hpp index 904ca6e5c..8d4795669 100644 --- a/diagnostic_updater/include/diagnostic_updater/diagnostic_status_wrapper.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_status_wrapper.hpp @@ -47,7 +47,8 @@ #include "diagnostic_msgs/msg/diagnostic_status.hpp" -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" namespace diagnostic_updater { diff --git a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp index a301ada59..635ead06a 100644 --- a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp @@ -50,8 +50,12 @@ #include "rcl/time.h" -#include "rclcpp/create_timer.hpp" -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" namespace diagnostic_updater { @@ -381,43 +385,7 @@ class Updater : public DiagnosticTaskVector std::shared_ptr parameters_interface, std::shared_ptr timers_interface, std::shared_ptr topics_interface, - double period = 1.0) - : verbose_(false), - base_interface_(base_interface), - timers_interface_(timers_interface), - clock_(clock_interface->get_clock()), - period_(rclcpp::Duration::from_seconds(period)), - publisher_( - rclcpp::create_publisher( - topics_interface, "/diagnostics", 1)), - logger_(logging_interface->get_logger()), - node_name_(base_interface->get_name()), - warn_nohwid_done_(false) - { - constexpr const char * period_param_name = "diagnostic_updater.period"; - rclcpp::ParameterValue period_param; - if (parameters_interface->has_parameter(period_param_name)) { - period_param = parameters_interface->get_parameter(period_param_name).get_parameter_value(); - } else { - period_param = parameters_interface->declare_parameter( - period_param_name, rclcpp::ParameterValue(period)); - } - period = period_param.get(); - period_ = rclcpp::Duration::from_seconds(period); - - reset_timer(); - - constexpr const char * use_fqn_param_name = "diagnostic_updater.use_fqn"; - rclcpp::ParameterValue use_fqn_param; - if (parameters_interface->has_parameter(use_fqn_param_name)) { - use_fqn_param = parameters_interface->get_parameter(use_fqn_param_name).get_parameter_value(); - } else { - use_fqn_param = parameters_interface->declare_parameter( - use_fqn_param_name, rclcpp::ParameterValue(false)); - } - node_name_ = use_fqn_param.get() ? - base_interface->get_fully_qualified_name() : base_interface->get_name(); - } + double period = 1.0); /** * \brief Returns the interval between updates. @@ -459,105 +427,20 @@ class Updater : public DiagnosticTaskVector * * \param msg Status message to output. */ - void broadcast(unsigned char lvl, const std::string msg) - { - std::vector status_vec; - - const std::vector & tasks = getTasks(); - for (std::vector::const_iterator iter = - tasks.begin(); - iter != tasks.end(); iter++) - { - diagnostic_updater::DiagnosticStatusWrapper status; - - status.name = iter->getName(); - status.summary(lvl, msg); - - status_vec.push_back(status); - } + void broadcast(unsigned char lvl, const std::string msg); - publish(status_vec); - } - - void setHardwareIDf(const char * format, ...) - { - va_list va; - const int kBufferSize = 1000; - char buff[kBufferSize]; // @todo This could be done more elegantly. - va_start(va, format); - if (vsnprintf(buff, kBufferSize, format, va) >= kBufferSize) { - RCLCPP_DEBUG(logger_, "Really long string in diagnostic_updater::setHardwareIDf."); - } - hwid_ = std::string(buff); - va_end(va); - } + void setHardwareIDf(const char * format, ...); void setHardwareID(const std::string & hwid) {hwid_ = hwid;} private: - void reset_timer() - { - update_timer_ = rclcpp::create_timer( - base_interface_, - timers_interface_, - clock_, - period_, - std::bind(&Updater::update, this)); - } + void reset_timer(); /** * \brief Causes the diagnostics to update if the inter-update interval * has been exceeded. */ - void update() - { - if (rclcpp::ok()) { - bool warn_nohwid = hwid_.empty(); - - std::vector status_vec; - - std::unique_lock lock( - lock_); // Make sure no adds happen while we are processing here. - const std::vector & tasks = getTasks(); - for (std::vector::const_iterator iter = - tasks.begin(); - iter != tasks.end(); iter++) - { - diagnostic_updater::DiagnosticStatusWrapper status; - - status.name = iter->getName(); - status.level = 2; - status.message = "No message was set"; - status.hardware_id = hwid_; - - iter->run(status); - - status_vec.push_back(status); - - if (status.level) { - warn_nohwid = false; - } - - if (verbose_ && status.level) { - RCLCPP_WARN( - logger_, "Non-zero diagnostic status. Name: '%s', status %i: '%s'", - status.name.c_str(), status.level, status.message.c_str()); - } - } - - if (warn_nohwid && !warn_nohwid_done_) { - std::string error_msg = "diagnostic_updater: No HW_ID was set."; - error_msg += " This is probably a bug. Please report it."; - error_msg += " For devices that do not have a HW_ID, set this value to 'none'."; - error_msg += " This warning only occurs once all diagnostics are OK."; - error_msg += " It is okay to wait until the device is open before calling setHardwareID."; - RCLCPP_WARN(logger_, "%s", error_msg.c_str()); - warn_nohwid_done_ = true; - } - - publish(status_vec); - } - } + void update(); /** * Recheck the diagnostic_period on the parameter server. (Cached) @@ -574,41 +457,18 @@ class Updater : public DiagnosticTaskVector /** * Publishes a single diagnostic status. */ - void publish(diagnostic_msgs::msg::DiagnosticStatus & stat) - { - std::vector status_vec; - status_vec.push_back(stat); - publish(status_vec); - } + void publish(diagnostic_msgs::msg::DiagnosticStatus & stat); /** * Publishes a vector of diagnostic statuses. */ - void publish(std::vector & status_vec) - { - for (std::vector::iterator iter = - status_vec.begin(); - iter != status_vec.end(); iter++) - { - iter->name = node_name_ + std::string(": ") + iter->name; - } - diagnostic_msgs::msg::DiagnosticArray msg; - msg.status = status_vec; - msg.header.stamp = clock_->now(); - publisher_->publish(msg); - } + void publish(std::vector & status_vec); /** * Causes a placeholder DiagnosticStatus to be published as soon as a * diagnostic task is added to the Updater. */ - virtual void addedTaskCallback(DiagnosticTaskInternal & task) - { - DiagnosticStatusWrapper stat; - stat.name = task.getName(); - stat.summary(0, "Node starting up"); - publish(stat); - } + virtual void addedTaskCallback(DiagnosticTaskInternal & task); rclcpp::node_interfaces::NodeBaseInterface::SharedPtr base_interface_; rclcpp::node_interfaces::NodeTimersInterface::SharedPtr timers_interface_; diff --git a/diagnostic_updater/include/diagnostic_updater/publisher.hpp b/diagnostic_updater/include/diagnostic_updater/publisher.hpp index d23c7fd1f..f2bdcb47a 100644 --- a/diagnostic_updater/include/diagnostic_updater/publisher.hpp +++ b/diagnostic_updater/include/diagnostic_updater/publisher.hpp @@ -226,8 +226,9 @@ class DiagnosedPublisher : public TopicDiagnostic const typename PublisherT::SharedPtr & pub, diagnostic_updater::Updater & diag, const diagnostic_updater::FrequencyStatusParam & freq, - const diagnostic_updater::TimeStampStatusParam & stamp) - : TopicDiagnostic(pub->get_topic_name(), diag, freq, stamp), + const diagnostic_updater::TimeStampStatusParam & stamp, + const rclcpp::Clock::SharedPtr & clock = std::make_shared()) + : TopicDiagnostic(pub->get_topic_name(), diag, freq, stamp, clock), publisher_(pub) { static_assert(has_header::value, "Message type has to have a header."); diff --git a/diagnostic_updater/package.xml b/diagnostic_updater/package.xml index 7cf5923a4..2683e65bc 100644 --- a/diagnostic_updater/package.xml +++ b/diagnostic_updater/package.xml @@ -2,7 +2,7 @@ diagnostic_updater - 3.1.2 + 4.3.1 diagnostic_updater contains tools for easily updating diagnostics. it is commonly used in device drivers to keep track of the status of output topics, device status, etc. Austin Hendrix Brice Rebsamen @@ -32,6 +32,7 @@ ament_lint_common launch launch_testing + launch_testing_ros python3-pytest rclcpp_lifecycle diff --git a/diagnostic_updater/src/diagnostic_updater.cpp b/diagnostic_updater/src/diagnostic_updater.cpp new file mode 100644 index 000000000..bfd5ce6d9 --- /dev/null +++ b/diagnostic_updater/src/diagnostic_updater.cpp @@ -0,0 +1,198 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +#include "rclcpp/rclcpp.hpp" + +namespace diagnostic_updater +{ +Updater::Updater( + std::shared_ptr base_interface, + std::shared_ptr clock_interface, + std::shared_ptr logging_interface, + std::shared_ptr parameters_interface, + std::shared_ptr timers_interface, + std::shared_ptr topics_interface, double period) +: verbose_(false), + base_interface_(base_interface), + timers_interface_(timers_interface), + clock_(clock_interface->get_clock()), + period_(rclcpp::Duration::from_seconds(period)), + publisher_(rclcpp::create_publisher( + topics_interface, "/diagnostics", 1)), + logger_(logging_interface->get_logger()), + node_name_(base_interface->get_name()), + warn_nohwid_done_(false) +{ + constexpr const char * period_param_name = "diagnostic_updater.period"; + rclcpp::ParameterValue period_param; + if (parameters_interface->has_parameter(period_param_name)) { + period_param = parameters_interface->get_parameter(period_param_name).get_parameter_value(); + } else { + period_param = + parameters_interface->declare_parameter(period_param_name, rclcpp::ParameterValue(period)); + } + period = period_param.get(); + period_ = rclcpp::Duration::from_seconds(period); + + reset_timer(); + + constexpr const char * use_fqn_param_name = "diagnostic_updater.use_fqn"; + rclcpp::ParameterValue use_fqn_param; + if (parameters_interface->has_parameter(use_fqn_param_name)) { + use_fqn_param = parameters_interface->get_parameter(use_fqn_param_name).get_parameter_value(); + } else { + use_fqn_param = + parameters_interface->declare_parameter(use_fqn_param_name, rclcpp::ParameterValue(false)); + } + node_name_ = use_fqn_param.get() ? base_interface->get_fully_qualified_name() : + base_interface->get_name(); +} + +void Updater::broadcast(unsigned char lvl, const std::string msg) +{ + std::vector status_vec; + + const std::vector & tasks = getTasks(); + for (std::vector::const_iterator iter = tasks.begin(); + iter != tasks.end(); iter++) + { + diagnostic_updater::DiagnosticStatusWrapper status; + + status.name = iter->getName(); + status.summary(lvl, msg); + + status_vec.push_back(status); + } + + publish(status_vec); +} + +void Updater::setHardwareIDf(const char * format, ...) +{ + va_list va; + const int kBufferSize = 1000; + char buff[kBufferSize]; // @todo This could be done more elegantly. + va_start(va, format); + if (vsnprintf(buff, kBufferSize, format, va) >= kBufferSize) { + RCLCPP_DEBUG(logger_, "Really long string in diagnostic_updater::setHardwareIDf."); + } + hwid_ = std::string(buff); + va_end(va); +} + +void Updater::reset_timer() +{ + update_timer_ = rclcpp::create_timer( + base_interface_, timers_interface_, clock_, period_, std::bind(&Updater::update, this)); +} + +void Updater::update() +{ + if (rclcpp::ok(base_interface_->get_context())) { + bool warn_nohwid = hwid_.empty(); + + std::vector status_vec; + + std::unique_lock lock( + lock_); // Make sure no adds happen while we are processing here. + const std::vector & tasks = getTasks(); + for (std::vector::const_iterator iter = tasks.begin(); + iter != tasks.end(); iter++) + { + diagnostic_updater::DiagnosticStatusWrapper status; + + status.name = iter->getName(); + status.level = 2; + status.message = "No message was set"; + status.hardware_id = hwid_; + + iter->run(status); + + status_vec.push_back(status); + + if (status.level) { + warn_nohwid = false; + } + + if (verbose_ && status.level) { + RCLCPP_WARN( + logger_, "Non-zero diagnostic status. Name: '%s', status %i: '%s'", status.name.c_str(), + status.level, status.message.c_str()); + } + } + + if (warn_nohwid && !warn_nohwid_done_) { + std::string error_msg = "diagnostic_updater: No HW_ID was set."; + error_msg += " This is probably a bug. Please report it."; + error_msg += " For devices that do not have a HW_ID, set this value to 'none'."; + error_msg += " This warning only occurs once all diagnostics are OK."; + error_msg += " It is okay to wait until the device is open before calling setHardwareID."; + RCLCPP_WARN(logger_, "%s", error_msg.c_str()); + warn_nohwid_done_ = true; + } + + publish(status_vec); + } +} + +void Updater::publish(diagnostic_msgs::msg::DiagnosticStatus & stat) +{ + std::vector status_vec; + status_vec.push_back(stat); + publish(status_vec); +} + +void Updater::publish(std::vector & status_vec) +{ + for (std::vector::iterator iter = status_vec.begin(); + iter != status_vec.end(); iter++) + { + iter->name = node_name_ + std::string(": ") + iter->name; + } + diagnostic_msgs::msg::DiagnosticArray msg; + msg.status = status_vec; + msg.header.stamp = clock_->now(); + publisher_->publish(msg); +} + +void Updater::addedTaskCallback(DiagnosticTaskInternal & task) +{ + DiagnosticStatusWrapper stat; + stat.name = task.getName(); + stat.summary(0, "Node starting up"); + publish(stat); +} +} // namespace diagnostic_updater diff --git a/diagnostic_updater/src/example.cpp b/diagnostic_updater/src/example.cpp index fb03c98dc..328c2f717 100644 --- a/diagnostic_updater/src/example.cpp +++ b/diagnostic_updater/src/example.cpp @@ -34,9 +34,10 @@ #include #include - #include +#include "rclcpp/rclcpp.hpp" + using namespace std::chrono_literals; double time_to_launch; diff --git a/diagnostic_updater/test/diagnostic_updater_test.cpp b/diagnostic_updater/test/diagnostic_updater_test.cpp index 0362b2897..6fd7d1998 100644 --- a/diagnostic_updater/test/diagnostic_updater_test.cpp +++ b/diagnostic_updater/test/diagnostic_updater_test.cpp @@ -134,6 +134,42 @@ TEST(DiagnosticUpdater, testUpdaterAsNodeClassMember) { SUCCEED(); } +class SaveIfCalled : public diagnostic_updater::DiagnosticTask +{ +public: + SaveIfCalled() + : DiagnosticTask("SaveIfCalled") {} + + void run(diagnostic_updater::DiagnosticStatusWrapper & s) + { + s.summary(0, "Have been called!"); + has_been_called_ = true; + } + + bool has_been_called() const + { + return has_been_called_; + } + +private: + bool has_been_called_ = false; +}; + + +TEST(DiagnosticUpdater, testCustomContext) { + auto context = std::make_shared(); + context->init(0, nullptr, rclcpp::InitOptions()); + + auto node = + std::make_shared("CustomContextNode", rclcpp::NodeOptions().context(context)); + diagnostic_updater::Updater updater(node); + SaveIfCalled save_if_called; + updater.add(save_if_called); + updater.force_update(); + ASSERT_TRUE(save_if_called.has_been_called()); + context->shutdown("End test"); +} + TEST(DiagnosticUpdater, testDiagnosticStatusWrapperKeyValuePairs) { diagnostic_updater::DiagnosticStatusWrapper stat; diff --git a/diagnostics/CHANGELOG.rst b/diagnostics/CHANGELOG.rst index 9e45ec832..4dc637ad4 100644 --- a/diagnostics/CHANGELOG.rst +++ b/diagnostics/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package diagnostics ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.3.1 (2024-07-30) +------------------ + +3.2.1 (2024-06-27) +------------------ + +3.2.0 (2024-03-22) +------------------ + 3.1.2 (2023-03-24) ------------------ diff --git a/diagnostics/package.xml b/diagnostics/package.xml index c581103e0..158fe74a7 100644 --- a/diagnostics/package.xml +++ b/diagnostics/package.xml @@ -2,7 +2,7 @@ diagnostics - 3.1.2 + 4.3.1 diagnostics Austin Hendrix Brice Rebsamen diff --git a/self_test/CHANGELOG.rst b/self_test/CHANGELOG.rst index fe0c3e22f..aeac3fd06 100644 --- a/self_test/CHANGELOG.rst +++ b/self_test/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package self_test ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.3.1 (2024-07-30) +------------------ + +3.2.1 (2024-06-27) +------------------ +* Building in docker (`#335 `_) +* Contributors: Christian Henkel + +3.2.0 (2024-03-22) +------------------ +* Self test publishes the service under the node name, again (`#269 `_) +* Contributors: Christian Henkel + 3.1.2 (2023-03-24) ------------------ diff --git a/self_test/CMakeLists.txt b/self_test/CMakeLists.txt index 0acbaa640..46401f9ed 100644 --- a/self_test/CMakeLists.txt +++ b/self_test/CMakeLists.txt @@ -67,6 +67,10 @@ if(BUILD_TESTING) set(ament_cmake_copyright_FOUND TRUE) ament_lint_auto_find_test_dependencies() + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_uncrustify # Inconsistent between jammy and noble + ) + add_subdirectory(test) endif() diff --git a/self_test/package.xml b/self_test/package.xml index 600cc9cd4..10a894bd0 100644 --- a/self_test/package.xml +++ b/self_test/package.xml @@ -2,7 +2,7 @@ self_test - 3.1.2 + 4.3.1 self_test Austin Hendrix Brice Rebsamen