From 1394b885e6a3cb23aa273fd464d07d866078ea12 Mon Sep 17 00:00:00 2001 From: Hilary Luo <103377417+hilary-luo@users.noreply.github.com> Date: Tue, 28 Jan 2025 14:58:03 -0500 Subject: [PATCH] Feature/diagnostics (#135) * Initial port of diagnostics to C++ * Remap axis camera topics to match API * Monitor MCU Status message frequency * Added firmware version check * Group MCU diagnostics together * Improve messaging around firmware versions * Disable MCU diagnostics for A200 --- clearpath_diagnostics/CMakeLists.txt | 28 +- .../clearpath_diagnostics/__init__.py | 0 .../clearpath_diagnostics/diagnostics_updater | 249 ---------------- .../config/diagnostic_aggregator.yaml | 35 +++ .../config/diagnostic_updater.yaml | 8 + clearpath_diagnostics/config/diagnostics.yaml | 36 --- .../clearpath_diagnostic_updater.hpp | 114 ++++++++ .../launch/diagnostics.launch.py | 101 +++---- clearpath_diagnostics/package.xml | 21 +- .../src/clearpath_diagnostic_updater.cpp | 266 ++++++++++++++++++ .../launch/generator.py | 25 +- .../launch/axis_camera.launch.py | 3 +- 12 files changed, 527 insertions(+), 359 deletions(-) delete mode 100644 clearpath_diagnostics/clearpath_diagnostics/__init__.py delete mode 100755 clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater create mode 100644 clearpath_diagnostics/config/diagnostic_aggregator.yaml create mode 100644 clearpath_diagnostics/config/diagnostic_updater.yaml delete mode 100644 clearpath_diagnostics/config/diagnostics.yaml create mode 100644 clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp create mode 100755 clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp diff --git a/clearpath_diagnostics/CMakeLists.txt b/clearpath_diagnostics/CMakeLists.txt index 3f60ac8..1e86df9 100644 --- a/clearpath_diagnostics/CMakeLists.txt +++ b/clearpath_diagnostics/CMakeLists.txt @@ -7,14 +7,32 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -find_package(ament_cmake_python REQUIRED) +find_package(clearpath_platform_msgs REQUIRED) +find_package(diagnostic_updater REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) -ament_python_install_package(${PROJECT_NAME}) +set(DEPENDENCIES + ament_cmake + clearpath_platform_msgs + diagnostic_updater + rclcpp + sensor_msgs +) -install(PROGRAMS - ${PROJECT_NAME}/diagnostics_updater - DESTINATION lib/${PROJECT_NAME} +add_executable(clearpath_diagnostic_updater + src/clearpath_diagnostic_updater.cpp ) +target_include_directories(clearpath_diagnostic_updater PUBLIC + $ + $) +target_compile_features(clearpath_diagnostic_updater PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +ament_target_dependencies(clearpath_diagnostic_updater ${DEPENDENCIES}) +target_link_libraries(clearpath_diagnostic_updater) + +install(TARGETS clearpath_diagnostic_updater + DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME} diff --git a/clearpath_diagnostics/clearpath_diagnostics/__init__.py b/clearpath_diagnostics/clearpath_diagnostics/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater b/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater deleted file mode 100755 index a684619..0000000 --- a/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater +++ /dev/null @@ -1,249 +0,0 @@ -#!/usr/bin/env python3 - -# Software License Agreement (BSD) -# -# @author Roni Kreinin -# @copyright (c) 2023, Clearpath Robotics, 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 Clearpath Robotics 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 HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -# Redistribution and use in source and binary forms, with or without -# modification, is not permitted without the express permission -# of Clearpath Robotics. - -import os - -from diagnostic_msgs.msg import DiagnosticStatus - -from diagnostic_updater import FrequencyStatusParam, HeaderlessTopicDiagnostic, Updater, DiagnosticStatusWrapper - -import rclpy -from rclpy.node import Node -from rclpy.qos import qos_profile_sensor_data - -from sensor_msgs.msg import BatteryState, Image, Imu, LaserScan, MagneticField, NavSatFix, PointCloud2 - -from clearpath_generator_common.common import BaseGenerator -from clearpath_generator_common.ros import ROS_DISTRO -from clearpath_config.common.utils.yaml import read_yaml -from clearpath_config.clearpath_config import ClearpathConfig -from clearpath_config.sensors.types.sensor import BaseSensor -from clearpath_config.sensors.types.cameras import BaseCamera, IntelRealsense, FlirBlackfly -from clearpath_config.sensors.types.imu import BaseIMU, CHRoboticsUM6, Microstrain, RedshiftUM7 -from clearpath_config.sensors.types.lidars_2d import BaseLidar2D, HokuyoUST, SickLMS1XX -from clearpath_config.sensors.types.lidars_3d import BaseLidar3D, VelodyneLidar -from clearpath_config.sensors.types.gps import BaseGPS, SwiftNavDuro, NMEA - -from clearpath_platform_msgs.msg import Status - -from typing import List -from apt import Cache, Version - - -class ClearpathDiagnosticUpdater(Node): - def __init__(self, setup_path: str = '/etc/clearpath/'): - super().__init__('clearpath_diagnostics_updater') - self.setup_path = setup_path - - # Define paths - self.config_path = os.path.join(setup_path, 'robot.yaml') - assert os.path.exists(self.config_path) - - # # Read YAML - # self.config = read_yaml() - # Parse YAML into config - self.clearpath_config = ClearpathConfig(self.config_path) - - self.serial_number = self.clearpath_config.serial_number - self.platform_model = self.clearpath_config.platform.get_platform_model() - self.namespace = self.clearpath_config.system.namespace - - # Create updater - self.updater = Updater(self) - self.updater.setHardwareID(self.serial_number) - - # Subscribe to topics - self.status_sub = self.create_subscription( - Status, - 'platform/mcu/status', - self.status_callback, - qos_profile_sensor_data - ) - - self.camera_freq_bounds = {'min': 5, 'max': 10} - self.lidar2d_freq_bounds = {'min': 5, 'max': 10} - self.lidar3d_freq_bounds = {'min': 5, 'max': 10} - - for sensor in self.clearpath_config.sensors.get_all_sensors(): - self.add_sensor_diagnostic(sensor) - - # Check current firmware version - cache = Cache() - try: - pkg = cache[f'ros-{ROS_DISTRO}-clearpath-firmware'] - versions = pkg.versions - newest: Version = versions[0] - self.newest_firmware_version = newest.version.split('-')[0] - except KeyError: - self.get_logger().warn(f'ros-{ROS_DISTRO}-clearpath-firmware package not found') - self.newest_firmware_version = 'Not found' - - self.firmware_version = '0.0.0' - self.battery_percentage = 0.0 - self.battery_voltage = 0.0 - - # Add Diagnostic status - self.updater.add('Firmware Version', self.check_firmware_version) - - self.updater.force_update() - - def status_callback(self, msg: Status): - self.firmware_version = msg.firmware_version - self.mcu_temperature = msg.mcu_temperature - self.pcb_temperature = msg.pcb_temperature - - def check_firmware_version(self, stat: DiagnosticStatusWrapper): - if self.newest_firmware_version == 'Not found': - stat.summary(DiagnosticStatus.ERROR, f'ros-{ROS_DISTRO}-clearpath-firmware package not found') - elif self.firmware_version == self.newest_firmware_version: - stat.summary(DiagnosticStatus.OK, f'Firmware is up to date (v{self.firmware_version})') - elif self.firmware_version < self.newest_firmware_version: - stat.summary(DiagnosticStatus.WARN, f'New firmware available. (v{self.firmware_version}) -> (v{self.newest_firmware_version})') - else: - stat.summary(DiagnosticStatus.WARN, f'ros-{ROS_DISTRO}-clearpath-firmware package is outdated.') - return stat - - def get_headerless_topic_diagnostic(self, - sensor: BaseSensor, - topic: str) -> HeaderlessTopicDiagnostic: - sensor_topic = sensor.get_topic(topic, local=True) - rate = sensor.get_topic_rate(topic) - freq_bounds = FrequencyStatusParam({'min': rate, 'max': rate}) - return HeaderlessTopicDiagnostic(sensor_topic, self.updater, freq_bounds) - - def add_sensor_diagnostic(self, sensor: BaseSensor): - diagnostics: List[tuple] = [] - - if not sensor.launch_enabled: - return - - match sensor: - case IntelRealsense(): - if sensor.color_enabled: - diagnostics.append(( - Image, - sensor.get_topic(IntelRealsense.TOPICS.COLOR_IMAGE, local=True), - self.get_headerless_topic_diagnostic( - sensor, IntelRealsense.TOPICS.COLOR_IMAGE) - )) - if sensor.depth_enabled: - diagnostics.append(( - Image, - sensor.get_topic(IntelRealsense.TOPICS.DEPTH_IMAGE, local=True), - self.get_headerless_topic_diagnostic( - sensor, IntelRealsense.TOPICS.DEPTH_IMAGE) - )) - if sensor.pointcloud_enabled: - diagnostics.append(( - PointCloud2, - sensor.get_topic(IntelRealsense.TOPICS.POINTCLOUD, local=True), - self.get_headerless_topic_diagnostic( - sensor, IntelRealsense.TOPICS.POINTCLOUD) - )) - - case BaseCamera(): - diagnostics.append(( - Image, - sensor.get_topic(BaseCamera.TOPICS.COLOR_IMAGE, local=True), - self.get_headerless_topic_diagnostic( - sensor, BaseCamera.TOPICS.COLOR_IMAGE) - )) - - case BaseLidar2D(): - diagnostics.append(( - LaserScan, - sensor.get_topic(BaseLidar2D.TOPICS.SCAN, local=True), - self.get_headerless_topic_diagnostic( - sensor, BaseLidar2D.TOPICS.SCAN) - )) - - case BaseLidar3D(): - diagnostics.append(( - LaserScan, - sensor.get_topic(BaseLidar3D.TOPICS.SCAN, local=True), - self.get_headerless_topic_diagnostic( - sensor, BaseLidar3D.TOPICS.SCAN) - )) - diagnostics.append(( - PointCloud2, - sensor.get_topic(BaseLidar3D.TOPICS.POINTS, local=True), - self.get_headerless_topic_diagnostic( - sensor, BaseLidar3D.TOPICS.POINTS) - )) - - case BaseIMU(): - diagnostics.append(( - Imu, - sensor.get_topic(BaseIMU.TOPICS.DATA, local=True), - self.get_headerless_topic_diagnostic( - sensor, BaseIMU.TOPICS.DATA) - )) - diagnostics.append(( - MagneticField, - sensor.get_topic(BaseIMU.TOPICS.MAG, local=True), - self.get_headerless_topic_diagnostic( - sensor, BaseIMU.TOPICS.MAG) - )) - - case BaseGPS(): - diagnostics.append(( - NavSatFix, - sensor.get_topic(BaseGPS.TOPICS.FIX, local=True), - self.get_headerless_topic_diagnostic( - sensor, BaseGPS.TOPICS.FIX) - )) - - for d in diagnostics: - self.create_subscription( - d[0], - d[1], - lambda msg: d[2].tick(), - qos_profile_sensor_data - ) - - -def main(): - setup_path = BaseGenerator.get_args() - - rclpy.init() - - node = ClearpathDiagnosticUpdater(setup_path) - rclpy.spin(node) - - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/clearpath_diagnostics/config/diagnostic_aggregator.yaml b/clearpath_diagnostics/config/diagnostic_aggregator.yaml new file mode 100644 index 0000000..e52c385 --- /dev/null +++ b/clearpath_diagnostics/config/diagnostic_aggregator.yaml @@ -0,0 +1,35 @@ +diagnostic_aggregator: + ros__parameters: + path: Clearpath Diagnostics + platform: + type: diagnostic_aggregator/AnalyzerGroup + path: Platform + analyzers: + mcu: + type: diagnostic_aggregator/GenericAnalyzer + path: MCU + contains: [ 'MCU' ] + sensors: + type: diagnostic_aggregator/AnalyzerGroup + path: Sensors + analyzers: + cameras: + type: diagnostic_aggregator/GenericAnalyzer + path: Cameras + contains: [ 'camera' ] + lidar2d: + type: diagnostic_aggregator/GenericAnalyzer + path: Lidar2D + contains: [ 'lidar2d' ] + lidar3d: + type: diagnostic_aggregator/GenericAnalyzer + path: Lidar3D + contains: [ 'lidar3d' ] + imu: + type: diagnostic_aggregator/GenericAnalyzer + path: IMU + contains: [ 'imu' ] + gps: + type: diagnostic_aggregator/GenericAnalyzer + path: GPS + contains: [ 'gps' ] diff --git a/clearpath_diagnostics/config/diagnostic_updater.yaml b/clearpath_diagnostics/config/diagnostic_updater.yaml new file mode 100644 index 0000000..1c01659 --- /dev/null +++ b/clearpath_diagnostics/config/diagnostic_updater.yaml @@ -0,0 +1,8 @@ +clearpath_diagnostic_updater: + ros__parameters: + serial_number: unknown + platform_model: unknown + ros_distro: unknown + latest_apt_firmware_version: unknown + installed_apt_firmware_version: unknown + topics: {} \ No newline at end of file diff --git a/clearpath_diagnostics/config/diagnostics.yaml b/clearpath_diagnostics/config/diagnostics.yaml deleted file mode 100644 index 0121fbe..0000000 --- a/clearpath_diagnostics/config/diagnostics.yaml +++ /dev/null @@ -1,36 +0,0 @@ -/**: - analyzers: - ros__parameters: - path: Clearpath Diagnostics - platform: - type: diagnostic_aggregator/AnalyzerGroup - path: Platform - analyzers: - firmware: - type: diagnostic_aggregator/GenericAnalyzer - path: Firmware - contains: [ 'Firmware' ] - sensors: - type: diagnostic_aggregator/AnalyzerGroup - path: Sensors - analyzers: - cameras: - type: diagnostic_aggregator/GenericAnalyzer - path: Cameras - contains: [ 'camera' ] - lidar2d: - type: diagnostic_aggregator/GenericAnalyzer - path: Lidar2D - contains: [ 'lidar2d' ] - lidar3d: - type: diagnostic_aggregator/GenericAnalyzer - path: Lidar3D - contains: [ 'lidar3d' ] - imu: - type: diagnostic_aggregator/GenericAnalyzer - path: IMU - contains: [ 'imu' ] - gps: - type: diagnostic_aggregator/GenericAnalyzer - path: GPS - contains: [ 'gps' ] diff --git a/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp b/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp new file mode 100644 index 0000000..d516b94 --- /dev/null +++ b/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp @@ -0,0 +1,114 @@ +/** +Software License Agreement (BSD) + +\file clearpath_diagnostic_updater.hpp +\authors Hilary Luo +\copyright Copyright (c) 2025, Clearpath Robotics, 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 Clearpath Robotics 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 WAR- +RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- +DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef CLEARPATH_DIAGNOSTIC_UPDATER_HPP +#define CLEARPATH_DIAGNOSTIC_UPDATER_HPP + +#include +#include + +#include "diagnostic_updater/diagnostic_updater.hpp" +#include "diagnostic_updater/publisher.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/compressed_image.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/magnetic_field.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" + +#include "clearpath_platform_msgs/msg/status.hpp" + +namespace clearpath +{ + +class ClearpathDiagnosticUpdater : public rclcpp::Node +{ +public: + ClearpathDiagnosticUpdater(); + +private: + std::string serial_number_; + std::string platform_model_; + std::string namespace_; + + // MCU Status Info + std::string ros_distro_; // Specifically the ros distro used for the firmware apt package check + std::string latest_apt_firmware_version_; + std::string installed_apt_firmware_version_; + std::string mcu_firmware_version_; + std::string mcu_platform_model_; + double mcu_status_rate_; + int mcu_temperature_; + int pcb_temperature_; + long connection_uptime_; + long mcu_uptime_; + std::shared_ptr mcu_freq_status_; + rclcpp::Subscription::SharedPtr sub_mcu_status_; + + diagnostic_updater::Updater updater_; + std::map> topic_map_; + + // Lists to ensure all variables relating to the rate monitoring persist until spin + std::list rates_; + std::list> topic_diagnostics_; + std::list> subscriptions_; + + void mcu_callback(const clearpath_platform_msgs::msg::Status & msg); + void check_firmware_version(diagnostic_updater::DiagnosticStatusWrapper & stat); + void mcu_status_diagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat); + + std::string get_mandatory_param(std::string param_name); + void setup_topic_rate_diagnostics(); + + template void add_rate_diagnostic(const std::string topic_name, const double rate) + { + // Store the rate so that it can be accessed via a pointer and is not deleted + rates_.push_back(rate); + + // Create the diagnostic task object that handles calculating and publishing rate statistics + auto topic_diagnostic = + std::make_shared( + topic_name, + updater_, + diagnostic_updater::FrequencyStatusParam(&rates_.back(), &rates_.back(), 0.1, 5)); + + // Store the diagnostic task object so that it can be accessed via a pointer and is not deleted + topic_diagnostics_.push_back(topic_diagnostic); + + auto sub = this->create_subscription( + topic_name, + rclcpp::SensorDataQoS(), + [this, topic_diagnostic] + ([[maybe_unused]] const MsgType & msg) { + topic_diagnostic->tick(); + }); + subscriptions_.push_back(std::static_pointer_cast(sub)); + } +}; + +} + +#endif // CLEARPATH_DIAGNOSTIC_UPDATER_HPP diff --git a/clearpath_diagnostics/launch/diagnostics.launch.py b/clearpath_diagnostics/launch/diagnostics.launch.py index 8726ac4..3d074e0 100644 --- a/clearpath_diagnostics/launch/diagnostics.launch.py +++ b/clearpath_diagnostics/launch/diagnostics.launch.py @@ -1,9 +1,8 @@ -#!/usr/bin/env python3 - # Software License Agreement (BSD) # # @author Roni Kreinin -# @copyright (c) 2023, Clearpath Robotics, Inc., All rights reserved. +# @author Hilary Luo +# @copyright (c) 2025, Clearpath Robotics, 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: @@ -28,63 +27,50 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -# Redistribution and use in source and binary forms, with or without -# modification, is not permitted without the express permission -# of Clearpath Robotics. - -from ament_index_python.packages import get_package_share_directory - -from clearpath_config.clearpath_config import ClearpathConfig -from clearpath_config.common.utils.yaml import read_yaml - from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - GroupAction, - OpaqueFunction, -) +from launch.actions import DeclareLaunchArgument, GroupAction from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node, PushRosNamespace - - -ARGUMENTS = [ - DeclareLaunchArgument( - 'setup_path', - default_value='/etc/clearpath/', - description='Clearpath setup path', - ) -] - - -def launch_setup(context, *args, **kwargs): - pkg_clearpath_diagnostics = get_package_share_directory('clearpath_diagnostics') +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare - setup_path = LaunchConfiguration('setup_path') - robot_yaml = PathJoinSubstitution( - [setup_path, 'robot.yaml'] - ) - - analyzer_params_filepath = PathJoinSubstitution( - [pkg_clearpath_diagnostics, 'config', 'diagnostics.yaml'] - ) - - # Read robot YAML - config = read_yaml(robot_yaml.perform(context)) - # Parse robot YAML into config - clearpath_config = ClearpathConfig(config) - - namespace = clearpath_config.system.namespace - diagnostics = GroupAction( +def generate_launch_description(): + namespace = LaunchConfiguration('namespace') + aggregator_parameters = LaunchConfiguration('aggregator_parameters') + updater_parameters = LaunchConfiguration('updater_parameters') + + arg_namespace = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Robot namespace, applied to diagnostic nodes and topics') + + arg_aggregator_params = DeclareLaunchArgument( + 'aggregator_parameters', + default_value=PathJoinSubstitution([ + FindPackageShare('clearpath_diagnostics'), + 'config', + 'diagnostic_aggregator.yaml' + ])) + + arg_updater_params = DeclareLaunchArgument( + 'updater_parameters', + default_value=PathJoinSubstitution([ + FindPackageShare('clearpath_diagnostics'), + 'config', + 'diagnostic_updater.yaml' + ])) + + diagnostics_action = GroupAction( [ - PushRosNamespace(namespace), # Aggregator Node( package='diagnostic_aggregator', executable='aggregator_node', + name='diagnostic_aggregator', + namespace=namespace, output='screen', - parameters=[analyzer_params_filepath], + parameters=[aggregator_parameters], remappings=[ ('/diagnostics', 'diagnostics'), ('/diagnostics_agg', 'diagnostics_agg'), @@ -94,22 +80,23 @@ def launch_setup(context, *args, **kwargs): # Updater Node( package='clearpath_diagnostics', - executable='diagnostics_updater', + executable='clearpath_diagnostic_updater', + name='clearpath_diagnostic_updater', + namespace=namespace, output='screen', + parameters=[updater_parameters], remappings=[ ('/diagnostics', 'diagnostics'), ('/diagnostics_agg', 'diagnostics_agg'), ('/diagnostics_toplevel_state', 'diagnostics_toplevel_state'), ], - arguments=['-s', setup_path] ), ] ) - return [diagnostics] - - -def generate_launch_description(): - ld = LaunchDescription(ARGUMENTS) - ld.add_action(OpaqueFunction(function=launch_setup)) + ld = LaunchDescription() + ld.add_action(arg_namespace) + ld.add_action(arg_aggregator_params) + ld.add_action(arg_updater_params) + ld.add_action(diagnostics_action) return ld diff --git a/clearpath_diagnostics/package.xml b/clearpath_diagnostics/package.xml index c7ab57b..3c7b001 100644 --- a/clearpath_diagnostics/package.xml +++ b/clearpath_diagnostics/package.xml @@ -3,20 +3,23 @@ clearpath_diagnostics 1.1.0 - Clearpath Robot Diagnostics scripts - rkreinin + Clearpath Robot Diagnostics Monitor + + Hilary Luo BSD + Roni Kreinin + Hilary Luo + ament_cmake - clearpath_config - clearpath_generator_common - clearpath_platform_msgs - diagnostic_updater + clearpath_platform_msgs + diagnostic_updater + rclcpp + sensor_msgs + diagnostic_aggregator - diagnostic_msgs - sensor_msgs - rclpy + ros2launch ament_lint_auto ament_lint_common diff --git a/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp b/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp new file mode 100755 index 0000000..5cee651 --- /dev/null +++ b/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp @@ -0,0 +1,266 @@ +/** +Software License Agreement (BSD) + +\file clearpath_diagnostic_updater.cpp +\authors Hilary Luo +\copyright Copyright (c) 2025, Clearpath Robotics, 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 Clearpath Robotics 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 WAR- +RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- +DIRECT, 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 "clearpath_diagnostics/clearpath_diagnostic_updater.hpp" + +#define UNKNOWN "unknown" + +namespace clearpath +{ + +ClearpathDiagnosticUpdater::ClearpathDiagnosticUpdater() +: Node( + "clearpath_diagnostic_updater", + rclcpp::NodeOptions().allow_undeclared_parameters(true). + automatically_declare_parameters_from_overrides(true)), + updater_(this) // Create the diagnostic updater object +{ + serial_number_ = this->get_mandatory_param("serial_number"); + platform_model_ = this->get_mandatory_param("platform_model"); + ros_distro_ = this->get_mandatory_param("ros_distro"); + latest_apt_firmware_version_ = this->get_mandatory_param("latest_apt_firmware_version"); + installed_apt_firmware_version_ = this->get_mandatory_param("installed_apt_firmware_version"); + + // Initialize variables that are populated in callbacks + mcu_firmware_version_ = UNKNOWN; + mcu_platform_model_ = UNKNOWN; + + // Set Hardware ID as serial number in diagnostics + updater_.setHardwareID(serial_number_); + if (latest_apt_firmware_version_ == "not_applicable") { + RCLCPP_INFO(this->get_logger(), "No MCU indicated, MCU diagnostics disabled."); + } else if (latest_apt_firmware_version_ != "simulated") { + // Publish MCU Status information as diagnostics + updater_.add("MCU Status", this, &ClearpathDiagnosticUpdater::mcu_status_diagnostic); + updater_.add("MCU Firmware Version", this, &ClearpathDiagnosticUpdater::check_firmware_version); + RCLCPP_INFO(this->get_logger(), "MCU diagnostics started."); + } + + mcu_status_rate_ = 1.0; + mcu_freq_status_ = std::make_shared( + diagnostic_updater::FrequencyStatusParam(&mcu_status_rate_, &mcu_status_rate_, 0.1, 5)); + + // subscribe to MCU status + sub_mcu_status_ = + this->create_subscription( + "platform/mcu/status", + rclcpp::SensorDataQoS(), + std::bind(&ClearpathDiagnosticUpdater::mcu_callback, this, std::placeholders::_1)); + + setup_topic_rate_diagnostics(); +} + +// get parameter from yaml and log an error if the parameter is not present +std::string ClearpathDiagnosticUpdater::get_mandatory_param(std::string param_name) +{ + try { + return this->get_parameter(param_name).as_string(); + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "Could not retrieve parameter %s: %s", + param_name.c_str(), e.what()); + return UNKNOWN; + } +} + +void ClearpathDiagnosticUpdater::check_firmware_version( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + if (latest_apt_firmware_version_ == "not_found") { + stat.summaryf(diagnostic_msgs::msg::DiagnosticStatus::ERROR, + "ros-%s-clearpath-firmware package not found", + ros_distro_.c_str()); + } else if (latest_apt_firmware_version_ == UNKNOWN) { + stat.summaryf(diagnostic_msgs::msg::DiagnosticStatus::ERROR, + "ros-%s-clearpath-firmware package version not provided in config", + ros_distro_.c_str()); + } else if (mcu_firmware_version_ == UNKNOWN) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, + "No firmware version received from MCU"); + } else if (mcu_firmware_version_ == latest_apt_firmware_version_) { + stat.summaryf(diagnostic_msgs::msg::DiagnosticStatus::OK, + "Firmware is up to date (%s)", + mcu_firmware_version_.c_str()); + } else if (mcu_firmware_version_ < latest_apt_firmware_version_) { + stat.summaryf(diagnostic_msgs::msg::DiagnosticStatus::WARN, + "New firmware available: (%s) -> (%s)", + mcu_firmware_version_.c_str(), + latest_apt_firmware_version_.c_str()); + } else { + stat.summaryf(diagnostic_msgs::msg::DiagnosticStatus::WARN, + "ros-%s-clearpath-firmware package is outdated", + ros_distro_.c_str()); + } + stat.add("Latest Firmware Version Package", latest_apt_firmware_version_); + stat.add("Firmware Version Installed on Computer", installed_apt_firmware_version_); + stat.add("Firmware Version on MCU", mcu_firmware_version_); +} + +// save data from MCU Status messages +void ClearpathDiagnosticUpdater::mcu_callback(const clearpath_platform_msgs::msg::Status & msg) +{ + mcu_firmware_version_ = msg.firmware_version; + mcu_platform_model_ = msg.hardware_id; + mcu_uptime_ = msg.mcu_uptime.sec; + connection_uptime_ = msg.connection_uptime.sec; + mcu_temperature_ = msg.mcu_temperature; + pcb_temperature_ = msg.pcb_temperature; + mcu_freq_status_->tick(); +} + +void ClearpathDiagnosticUpdater::mcu_status_diagnostic( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + stat.add("Firmware Version", mcu_firmware_version_); + stat.add("Platform Model", mcu_platform_model_); + stat.add("MCU Uptime", mcu_uptime_); + stat.add("Connection Uptime", connection_uptime_); + stat.add("MCU Temperature", mcu_temperature_); + stat.add("PCB Temperature", pcb_temperature_); + + mcu_freq_status_->run(stat); +} + +void ClearpathDiagnosticUpdater::setup_topic_rate_diagnostics() +{ + std::map topic_map_raw; + + // Get all parameters under the "topics" key in the yaml and store it in map format + if (!this->get_parameters("topics", topic_map_raw)) { + RCLCPP_WARN(this->get_logger(), "No topics found to monitor."); + return; + } + RCLCPP_INFO(this->get_logger(), "Retrieved %zu topic parameters.", topic_map_raw.size()); + + // Parse the raw topic map into a map of parameters per topic, stored in another map with + // the topic as the key + for (const auto & entry : topic_map_raw) { + + // Identify the topic name and parameter name in the entry key + auto key = entry.first; // Contains topic_name.param_name + auto pos = key.find("."); + auto topic_name = key.substr(0, pos); + auto param_name = key.substr(pos + 1); + + // RCLCPP_INFO(this->get_logger(), + // "Diagnostics config: Topic is %s, parameter %s = %s.", + // topic_name.c_str(), param_name.c_str(), entry.second.value_to_string().c_str()); + + // Add the parameter to the topic specific map + if (auto it = topic_map_.find(topic_name); it != topic_map_.end()) { + // Topic already exists as a key + it->second[param_name] = entry.second; + } else { + // Topic needs to be added as a new key + std::map map; + map[param_name] = entry.second; + topic_map_[topic_name] = map; + } + } + + // For each topic, create a subscription that monitors the publishing frequency + for (const auto & topic : topic_map_) { + auto topic_name = topic.first; + RCLCPP_INFO(this->get_logger(), "Diagnostics config: Topic is %s", topic_name.c_str()); + auto params_map = topic.second; + + for(const auto & param : params_map) { + RCLCPP_INFO(this->get_logger(), " Param: %s = %s", + param.first.c_str(), param.second.value_to_string().c_str()); + } + + // Get the message type + std::string type; + try { + type = params_map.at("type").as_string(); + } catch (const std::out_of_range & e) { + RCLCPP_ERROR(this->get_logger(), "No type provided for %s. This topic will not be monitored", + topic_name.c_str()); + continue; + } catch(const rclcpp::ParameterTypeException & e) { + RCLCPP_ERROR(this->get_logger(), + "Type provided for %s was not a valid string. Value is %s. \ + This topic will not be monitored", + topic_name.c_str(), params_map.at("type").value_to_string().c_str()); + continue; + } + + // Get the expected publishing rate + double rate = 0.0; + try { + rate = params_map.at("rate").as_double(); + } catch (const std::out_of_range & e) { + RCLCPP_ERROR(this->get_logger(), "No rate provided for %s. This topic will not be monitored", + topic_name.c_str()); + continue; + } catch(const rclcpp::ParameterTypeException & e) { + RCLCPP_ERROR(this->get_logger(), + "Rate provided for %s was not a valid double. Value is %s. \ + This topic will not be monitored", + topic_name.c_str(), params_map.at("rate").value_to_string().c_str()); + continue; + } + + /* + * The section below does not use a generic subscription because the generic subscription was + * observed to have significantly hgiher CPU usage seemingly related to too short of callbacks + * and allocating/releasing the memory too quickly with Fast DDS. Standard subscriptions + * perform more reliably. + */ + + // Create a subscription using the topic name and message type info from the yaml + if (type == "sensor_msgs/msg/CompressedImage") { + add_rate_diagnostic(topic_name, rate); + } else if (type == "sensor_msgs/msg/Image") { + add_rate_diagnostic(topic_name, rate); + } else if (type == "sensor_msgs/msg/Imu") { + add_rate_diagnostic(topic_name, rate); + } else if (type == "sensor_msgs/msg/LaserScan") { + add_rate_diagnostic(topic_name, rate); + } else if (type == "sensor_msgs/msg/MagneticField") { + add_rate_diagnostic(topic_name, rate); + } else if (type == "sensor_msgs/msg/NavSatFix") { + add_rate_diagnostic(topic_name, rate); + } else if (type == "sensor_msgs/msg/PointCloud2") { + add_rate_diagnostic(topic_name, rate); + } else { + RCLCPP_ERROR(this->get_logger(), + "Type \"%s\" of topic \"%s\" is not supported", + type.c_str(), + topic_name.c_str()); + continue; + } + + RCLCPP_INFO(this->get_logger(), "Created subscription for %s", topic_name.c_str()); + } +} +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py index 580fc44..ef6c2c2 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py +++ b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py @@ -132,9 +132,28 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: ], ) - # Diagnostics + # Diagnostics launch args + self.diag_updater_params = LaunchFile.LaunchArg( + 'diagnostic_updater_params', + default_value=os.path.join(self.platform_params_path, 'diagnostic_updater.yaml'), + ) + self.diag_aggregator_params = LaunchFile.LaunchArg( + 'diagnostic_aggregator_params', + default_value=os.path.join(self.platform_params_path, 'diagnostic_aggregator.yaml'), + ) + + self.diagnostic_args = [ + ('namespace', self.namespace), + ('updater_parameters', LaunchFile.Variable('diagnostic_updater_params')), + ('aggregator_parameters', LaunchFile.Variable('diagnostic_aggregator_params')), + ] + + # Diagnostics launch clearpath_diagnostics_package = Package('clearpath_diagnostics') - self.diagnostics_launch = LaunchFile('diagnostics', package=clearpath_diagnostics_package) + self.diagnostics_launch = LaunchFile( + 'diagnostics', + package=clearpath_diagnostics_package, + args=self.diagnostic_args) # Battery state self.battery_state_estimator = LaunchFile.Node( @@ -296,6 +315,8 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: # Components required for each platform common_platform_components = [ self.wireless_watcher_node, + self.diag_updater_params, + self.diag_aggregator_params, self.diagnostics_launch, self.battery_state_control, ] diff --git a/clearpath_sensors/launch/axis_camera.launch.py b/clearpath_sensors/launch/axis_camera.launch.py index 8a9782b..7e0d7f1 100644 --- a/clearpath_sensors/launch/axis_camera.launch.py +++ b/clearpath_sensors/launch/axis_camera.launch.py @@ -64,7 +64,8 @@ def generate_launch_description(): parameters=[parameters], output='screen', remappings=[ - ('image_raw/compressed', 'image/compressed'), + ('image_raw/compressed', 'color/image/compressed'), + ('camera_info', 'color/camera_info'), ('joint_states', PathJoinSubstitution(['/', robot_namespace, 'platform', 'joint_states'])), ]