diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 0c619177..41c30858 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -39,7 +39,6 @@ jobs: with: target-ros2-distro: jazzy package-name: | - clearpath_diagnostics clearpath_generator_robot clearpath_robot clearpath_sensors @@ -56,7 +55,6 @@ jobs: with: target-ros2-distro: jazzy package-name: | - clearpath_diagnostics clearpath_generator_robot clearpath_robot clearpath_sensors @@ -92,7 +90,6 @@ jobs: with: target-ros2-distro: jazzy package-name: | - clearpath_diagnostics clearpath_generator_robot clearpath_robot clearpath_sensors @@ -125,7 +122,6 @@ jobs: with: target-ros2-distro: jazzy package-name: | - clearpath_diagnostics clearpath_generator_robot clearpath_robot clearpath_sensors diff --git a/clearpath_diagnostics/CHANGELOG.rst b/clearpath_diagnostics/CHANGELOG.rst deleted file mode 100644 index 58535cc3..00000000 --- a/clearpath_diagnostics/CHANGELOG.rst +++ /dev/null @@ -1,229 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package clearpath_diagnostics -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.4.0 (2025-04-30) ------------------- - -2.3.3 (2025-04-17) ------------------- - -2.3.2 (2025-04-16) ------------------- - -2.3.1 (2025-04-14) ------------------- -* Remove duplicate and incorrect current / voltage labels (`#197 `_) - The terms "measured voltage" or "measured current" are prepended to these labels where they are used. -* Contributors: Hilary Luo - -2.3.0 (2025-04-11) ------------------- -* Lint: Diagnostic move log to new line (`#191 `_) -* Fix/estop diagnostics (`#184 `_) -* Feature/clear stale diagnostics (`#183 `_) -* Feature/diagnostic categories (`#175 `_) -* Feature/inventus diagnostics (`#170 `_) - -* Contributors: Hilary Luo, Tony Baltovski, Luis Camero - -2.2.4 (2025-04-07) ------------------- - -2.2.3 (2025-03-20) ------------------- -* [clearpath_diagnostics] Updated for changes to MCU status message. -* Contributors: Tony Baltovski - -2.2.2 (2025-03-17) ------------------- - -2.2.1 (2025-03-12) ------------------- - -2.2.0 (2025-03-11) ------------------- -* Feature/generated agg yaml (`#158 `_) - * Clear out sensor categories that will be generated - * Improve logging and error handling - * Correction to dependencies -* Fix BMS frequency diagnostic (`#157 `_) -* Added Lynx motor driver diagnostics (`#149 `_) - * Removed trailing spaces - * Added Lynx motor diagnostics -* Feature/lighting diagnostics (`#144 `_) - * Add lighting diagnostics - * Remap lighting diagnostic topic - * Set diagnostic updater hardware id to platform since serial isn't locally available - * Improve clarity of diagnostic summary text -* Feature/battery diagnostics (`#142 `_) - * Add missing DiagnosticStatus dependency - * Monitor BatteryState, Power and StopStatus messages - * Declare methods before member variables - * Move the template function to the cpp since it is private - * Only display error summaries if a message has been received -* Contributors: Hilary Luo - -2.1.2 (2025-02-28) ------------------- - -2.1.1 (2025-02-06) ------------------- - -2.1.0 (2025-01-31) ------------------- -* 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 -* Contributors: Hilary Luo - -2.0.4 (2025-01-22) ------------------- - -2.0.3 (2025-01-17) ------------------- -* [clearpath_diagnostics] Fixed version. -* Contributors: Tony Baltovski - -2.0.2 (2025-01-17) ------------------- - -2.0.1 (2025-01-17) ------------------- - -2.0.0 (2025-01-17) ------------------- -* Fix hard-coded humble packages (`#117 `_) -* Contributors: Chris Iverach-Brereton, Luis Camero, Tony Baltovski - -1.1.0 (2025-01-15) ------------------- - -1.0.1 (2024-11-28) ------------------- - -1.0.0 (2024-11-26) ------------------- -* Added minimum version. -* Remove battery_state from CMakeLists -* Move battery_state to clearpath_hardware_interfaces -* Contributors: Luis Camero, Tony Baltovski - -0.3.2 (2024-10-04) ------------------- - -0.3.1 (2024-09-23) ------------------- - -0.3.0 (2024-09-19) ------------------- -* R100 Initial Battry -* Contributors: Luis Camero, luis-camero - -0.2.15 (2024-08-12) -------------------- - -0.2.14 (2024-08-08) -------------------- - -0.2.13 (2024-07-30) -------------------- - -0.2.12 (2024-07-22) -------------------- -* Use PathJoinSubstitution for setup_path -* Contributors: Luis Camero - -0.2.11 (2024-05-28) -------------------- - -0.2.10 (2024-05-16) -------------------- - -0.2.9 (2024-05-16) ------------------- - -0.2.8 (2024-05-14) ------------------- -* Even more lint errors -* More linting changes -* Fixed linting errors -* Contributors: Luis Camero - -0.2.7 (2024-04-10) ------------------- - -0.2.6 (2024-04-08) ------------------- - -0.2.5 (2024-03-07) ------------------- - -0.2.4 (2024-01-19) ------------------- - -0.2.3 (2024-01-18) ------------------- - -0.2.2 (2024-01-10) ------------------- -* Get topic without namespace to address duplicate namespacing -* Contributors: Hilary Luo - -0.2.1 (2023-12-18) ------------------- - -0.2.0 (2023-12-13) ------------------- -* Added S1P2 battery configuration -* Set battery charging status -* Added dingo to battery state control -* Added D100 and D150 to generator and battery node -* Generate lighting node -* Fixed status message firmware version -* J100 -> W200 -* Removed shebang -* Use battery model and configuration from clearpath_config -* Removed HMI msg, encode Uint8 instead -* Initial battery control node -* Renamed to battery_state_estimator - Added to robot generator -* Properties, capacity, voltage - Create pub/sub only for LiION and SLA -* Added LUT for SLA -* Battery types and configurations -* rolling average -* Initial battery state publisher -* Pass setup path -* Get namespace from robot.yaml for diagnostics launch - Added diagnostics launch to generator -* Check ros-humble-clearpath-firmware package version -* Add all sensors -* Firmware and sensor diagnostics -* Contributors: Roni Kreinin - -0.1.3 (2023-10-04) ------------------- - -0.1.2 (2023-09-27) ------------------- - -0.1.1 (2023-09-11) ------------------- - -0.1.0 (2023-08-31) ------------------- - -0.0.3 (2023-08-15) ------------------- - -0.0.2 (2023-07-25) ------------------- - -0.0.1 (2023-07-20) ------------------- diff --git a/clearpath_diagnostics/CMakeLists.txt b/clearpath_diagnostics/CMakeLists.txt deleted file mode 100644 index 1e86df90..00000000 --- a/clearpath_diagnostics/CMakeLists.txt +++ /dev/null @@ -1,53 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(clearpath_diagnostics) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(clearpath_platform_msgs REQUIRED) -find_package(diagnostic_updater REQUIRED) -find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) - -set(DEPENDENCIES - ament_cmake - clearpath_platform_msgs - diagnostic_updater - rclcpp - sensor_msgs -) - -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} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/clearpath_diagnostics/config/diagnostic_aggregator.yaml b/clearpath_diagnostics/config/diagnostic_aggregator.yaml deleted file mode 100644 index b9cd49d1..00000000 --- a/clearpath_diagnostics/config/diagnostic_aggregator.yaml +++ /dev/null @@ -1,54 +0,0 @@ -# Note that this file is the default configuration but some elements are appended or overridden -# in clearpath_generator_common -diagnostic_aggregator: - ros__parameters: - path: Clearpath Diagnostics - platform: - type: diagnostic_aggregator/AnalyzerGroup - path: Platform - analyzers: - power: - type: diagnostic_aggregator/GenericAnalyzer - path: Power - expected: [ - 'clearpath_diagnostic_updater: Battery Management System', - 'clearpath_diagnostic_updater: Power Status' - ] - contains: [ 'Battery', 'Power' ] - stop_status: - type: diagnostic_aggregator/GenericAnalyzer - path: E-stop Status - expected: [ 'clearpath_diagnostic_updater: E-stop Status' ] - contains: [ 'E-stop' ] - lighting: - type: diagnostic_aggregator/GenericAnalyzer - path: Lighting - contains: [ 'Light' ] - drive: - type: diagnostic_aggregator/GenericAnalyzer - path: Drive System - expected: [ - 'controller_manager: Controller Manager Activity', - 'controller_manager: Controllers Activity', - 'controller_manager: Hardware Components Activity', - ] - contains: [ - 'lynx', - 'puma', - 'sevcon', - 'controller_manager', - 'twist_mux', - 'joy_node' - ] - odometry: - type: diagnostic_aggregator/GenericAnalyzer - path: Odometry - contains: [ 'odometry', 'ekf_node' ] - networking: - type: diagnostic_aggregator/GenericAnalyzer - path: Networking - contains: [ 'Wi-Fi' ] - # sensors: - # type: diagnostic_aggregator/AnalyzerGroup - # path: Sensors - # analyzers: # Must be populated with the sensor categories diff --git a/clearpath_diagnostics/config/diagnostic_updater.yaml b/clearpath_diagnostics/config/diagnostic_updater.yaml deleted file mode 100644 index 7f12e0c5..00000000 --- a/clearpath_diagnostics/config/diagnostic_updater.yaml +++ /dev/null @@ -1,8 +0,0 @@ -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: # Must be populated with the topics to be monitored \ No newline at end of file diff --git a/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_labels.hpp b/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_labels.hpp deleted file mode 100644 index ecd16849..00000000 --- a/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_labels.hpp +++ /dev/null @@ -1,177 +0,0 @@ -/** -Software License Agreement (BSD) - -\file clearpath_diagnostic_labels.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_LABELS_HPP -#define CLEARPATH_DIAGNOSTIC_LABELS_HPP - -#include -#include - -#include "sensor_msgs/msg/battery_state.hpp" - -using BatteryState = sensor_msgs::msg::BatteryState; - -namespace clearpath_diagnostic_labels -{ - -class DiagnosticLabels { - -public: - //-------------------------------------------------------- - // Robot Platforms - //-------------------------------------------------------- - - // Dingo D V1 - inline static const std::string DD100 = "dd100"; - // Dingo O V1 - inline static const std::string DO100 = "do100"; - // Dingo D V1.5 - inline static const std::string DD150 = "dd150"; - // Dingo D V1.5 - inline static const std::string DO150 = "do150"; - // Jackal V1 - inline static const std::string J100 = "j100"; - // Husky V2 - inline static const std::string A200 = "a200"; - // Husky V3 - inline static const std::string A300 = "a300"; - // Ridgeback V1 - inline static const std::string R100 = "r100"; - // Warthog V2 - inline static const std::string W200 = "w200"; - // Genric Robot - inline static const std::string GENERIC = "generic"; - - //-------------------------------------------------------- - // Labels for clearpath_platform_msgs::msg::Power - //-------------------------------------------------------- - - // int8 statuses - inline static const std::map POWER_STATUS = { - {-1, "Not Applicable"}, - {0, "False"}, - {1, "True"} - }; - - // Common Core Measured Voltage Labels - inline static const std::vector CC01_MEASURED_VOLTAGES = { - "Battery", - "User Battery", - "User 24V", - "User 12V", - "System 12V", - "Expansion", - "Breakout 24V Aux", - "Breakout 12V Aux", - "Breakout User 12VA", - "Breakout Lynx1", - "Breakout Lynx2", - "Breakout Lynx3", - "Breakout Lynx4" - }; - - // Common Core Measured Currents Labels - inline static const std::vector CC01_MEASURED_CURRENTS = { - "Battery", - "User Battery", - "Aux", - "System 12V", - "24V", - "12V", - "Breakout User 24V", - "Breakout User 12VA", - "Breakout User 12VB", - "Breakout Lynx1", - "Breakout Lynx2", - "Breakout Lynx3", - "Breakout Lynx4" - }; - - inline static const std::map> MEASURED_VOLTAGES = { - // Order and count of labels must match the Power message definition - {DD100, {"Battery", "5V Rail", "12V Rail"}}, - {DO100, {"Battery", "5V Rail", "12V Rail"}}, - {DD150, {"Battery", "5V Rail", "12V Rail"}}, - {DO150, {"Battery", "5V Rail", "12V Rail"}}, - {J100, {"Battery", "5V Rail", "12V Rail"}}, - {A200, {"Battery", "Left Driver Voltage", "Right Driver Voltage"}}, - {A300, CC01_MEASURED_VOLTAGES}, - {R100, {"Battery", "5V Rail", "12V Rail", "Inverter", "Front Axle", "Rear Axle", "Light"}}, - {W200, {"Battery", "12V Rail", "24V Rail", "48V Rail"}}, - }; - - inline static const std::map> MEASURED_CURRENTS = { - // Order and count of labels must match the Power message definition - {DD100, {"Total", "Computer"}}, - {DO100, {"Total", "Computer"}}, - {DD150, {"Total", "Computer"}}, - {DO150, {"Total", "Computer"}}, - {J100, {"Total", "Computer", "Drive", "User"}}, - {A200, {"MCU and User Port", "Left Driver", "Right Driver", "Currents Size"}}, - {A300, CC01_MEASURED_CURRENTS}, - {R100, {"Total"}}, - {W200, {"Total", "Computer", "12V", "24V"}}, - }; - - //-------------------------------------------------------- - // Labels for sensor_msgs::msg::BatteryState - //-------------------------------------------------------- - - inline static const std::map POWER_SUPPLY_HEALTH = { - {BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN, "Unknown"}, - {BatteryState::POWER_SUPPLY_HEALTH_GOOD, "Good"}, - {BatteryState::POWER_SUPPLY_HEALTH_OVERHEAT, "Overheat"}, - {BatteryState::POWER_SUPPLY_HEALTH_DEAD, "Dead"}, - {BatteryState::POWER_SUPPLY_HEALTH_OVERVOLTAGE, "Overvoltage"}, - {BatteryState::POWER_SUPPLY_HEALTH_UNSPEC_FAILURE, "Unspecified Failure"}, - {BatteryState::POWER_SUPPLY_HEALTH_COLD, "Cold"}, - {BatteryState::POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE, "Watchdog Timer Expired"}, - {BatteryState::POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE, "Safety Timer Expired"}, - }; - - inline static const std::map POWER_SUPPLY_STATUS = { - {BatteryState::POWER_SUPPLY_STATUS_UNKNOWN, "Unknown"}, - {BatteryState::POWER_SUPPLY_STATUS_CHARGING, "Charging"}, - {BatteryState::POWER_SUPPLY_STATUS_DISCHARGING, "Discharging"}, - {BatteryState::POWER_SUPPLY_STATUS_NOT_CHARGING, "Not Charging"}, - {BatteryState::POWER_SUPPLY_STATUS_FULL, "Full"}, - }; - - inline static const std::map POWER_SUPPLY_TECHNOLOGY = { - {BatteryState::POWER_SUPPLY_TECHNOLOGY_UNKNOWN, "Unknown"}, - {BatteryState::POWER_SUPPLY_TECHNOLOGY_NIMH, "NIMH"}, - {BatteryState::POWER_SUPPLY_TECHNOLOGY_LION, "LION"}, - {BatteryState::POWER_SUPPLY_TECHNOLOGY_LIPO, "LIPO"}, - {BatteryState::POWER_SUPPLY_TECHNOLOGY_LIFE, "LIFE"}, - {BatteryState::POWER_SUPPLY_TECHNOLOGY_NICD, "NICD"}, - {BatteryState::POWER_SUPPLY_TECHNOLOGY_LIMN, "LIMN"}, - {BatteryState::POWER_SUPPLY_TECHNOLOGY_TERNARY, "Ternary"}, - {BatteryState::POWER_SUPPLY_TECHNOLOGY_VRLA, "VRLA"}, - }; - -}; -} - -#endif // CLEARPATH_DIAGNOSTIC_LABELS_HPP diff --git a/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp b/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp deleted file mode 100644 index 3912c084..00000000 --- a/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp +++ /dev/null @@ -1,138 +0,0 @@ -/** -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 "diagnostic_updater/update_functions.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 "std_msgs/msg/bool.hpp" - -#include "clearpath_diagnostics/clearpath_diagnostic_labels.hpp" -#include "clearpath_platform_msgs/msg/power.hpp" -#include "clearpath_platform_msgs/msg/status.hpp" -#include "clearpath_platform_msgs/msg/stop_status.hpp" - -namespace clearpath -{ - -class ClearpathDiagnosticUpdater : public rclcpp::Node -{ -public: - ClearpathDiagnosticUpdater(); - -private: - using BatteryState = sensor_msgs::msg::BatteryState; - using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; - using DiagnosticStatusWrapper = diagnostic_updater::DiagnosticStatusWrapper; - using FrequencyStatus = diagnostic_updater::FrequencyStatus; - using FrequencyStatusParam = diagnostic_updater::FrequencyStatusParam; - - // Callbacks - void mcu_status_callback(const clearpath_platform_msgs::msg::Status & msg); - void mcu_power_callback(const clearpath_platform_msgs::msg::Power & msg); - void bms_state_callback(const BatteryState & msg); - void stop_status_callback(const clearpath_platform_msgs::msg::StopStatus & msg); - void estop_callback(const std_msgs::msg::Bool & msg); - - // Diagnostic Tasks - void firmware_diagnostic(DiagnosticStatusWrapper & stat); - void mcu_status_diagnostic(DiagnosticStatusWrapper & stat); - void mcu_power_diagnostic(DiagnosticStatusWrapper & stat); - void bms_state_diagnostic(DiagnosticStatusWrapper & stat); - void stop_status_diagnostic(DiagnosticStatusWrapper & stat); - - // Get parameters from config - std::string get_string_param(std::string param_name, bool mandatory = false); - double get_double_param(std::string param_name, bool mandatory = false); - - void setup_topic_rate_diagnostics(); - template void add_rate_diagnostic(const std::string topic_name, const double rate); - - // Parameters from config - std::string serial_number_; - std::string platform_model_; - std::string namespace_; - 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::map> topic_map_; - - // Topic names and rates - std::string mcu_status_topic_; - std::string mcu_power_topic_; - std::string bms_state_topic_; - std::string stop_status_topic_; - std::string estop_topic_; - double mcu_status_rate_; - double mcu_power_rate_; - double bms_state_rate_; - double stop_status_rate_; - - // Message Data - std::string mcu_firmware_version_; - clearpath_platform_msgs::msg::Status mcu_status_msg_; - clearpath_platform_msgs::msg::Power mcu_power_msg_; - BatteryState bms_state_msg_; - clearpath_platform_msgs::msg::StopStatus stop_status_msg_; - std_msgs::msg::Bool estop_msg_; - - // Frequency statuses - std::shared_ptr mcu_status_freq_status_; - std::shared_ptr mcu_power_freq_status_; - std::shared_ptr bms_state_freq_status_; - std::shared_ptr stop_status_freq_status_; - - // Subscriptions - rclcpp::Subscription::SharedPtr sub_mcu_status_; - rclcpp::Subscription::SharedPtr sub_mcu_power_; - rclcpp::Subscription::SharedPtr sub_bms_state_; - rclcpp::Subscription::SharedPtr sub_stop_status_; - rclcpp::Subscription::SharedPtr sub_estop_; - - // Diagnostic Updater - diagnostic_updater::Updater updater_; - - // Lists to ensure all variables relating to the rate monitoring persist until spin - std::list rates_; - std::list> topic_diagnostics_; - std::list> subscriptions_; - -}; - -} - -#endif // CLEARPATH_DIAGNOSTIC_UPDATER_HPP diff --git a/clearpath_diagnostics/launch/diagnostics.launch.py b/clearpath_diagnostics/launch/diagnostics.launch.py deleted file mode 100644 index 3d074e06..00000000 --- a/clearpath_diagnostics/launch/diagnostics.launch.py +++ /dev/null @@ -1,102 +0,0 @@ -# Software License Agreement (BSD) -# -# @author Roni Kreinin -# @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: -# * 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. - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution - -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -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( - [ - # Aggregator - Node( - package='diagnostic_aggregator', - executable='aggregator_node', - name='diagnostic_aggregator', - namespace=namespace, - output='screen', - parameters=[aggregator_parameters], - remappings=[ - ('/diagnostics', 'diagnostics'), - ('/diagnostics_agg', 'diagnostics_agg'), - ('/diagnostics_toplevel_state', 'diagnostics_toplevel_state'), - ], - ), - # Updater - Node( - package='clearpath_diagnostics', - 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'), - ], - ), - ] - ) - - 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 deleted file mode 100644 index d3aaae8f..00000000 --- a/clearpath_diagnostics/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - clearpath_diagnostics - 2.4.0 - Clearpath Robot Diagnostics Monitor - - Hilary Luo - BSD - - Roni Kreinin - Hilary Luo - - ament_cmake - - clearpath_platform_msgs - diagnostic_updater - rclcpp - sensor_msgs - - diagnostic_aggregator - ros2launch - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp b/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp deleted file mode 100755 index 382af258..00000000 --- a/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp +++ /dev/null @@ -1,597 +0,0 @@ -/** -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" - -using namespace clearpath_diagnostic_labels; - -namespace clearpath -{ - -/** - * @brief Construct a new ClearpathDiagnosticUpdater node - */ -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 -{ - // Get mandatory parameters from the config - serial_number_ = get_string_param("serial_number", true); - platform_model_ = get_string_param("platform_model", true); - ros_distro_ = get_string_param("ros_distro", true); - latest_apt_firmware_version_ = get_string_param("latest_apt_firmware_version", true); - installed_apt_firmware_version_ = get_string_param("installed_apt_firmware_version", true); - RCLCPP_INFO(this->get_logger(), "Diagnostics starting for a %s platform with serial number %s", - platform_model_.c_str(), serial_number_.c_str()); - - // Get optional parameters from the config - mcu_status_topic_ = get_string_param("mcu_status_topic"); - mcu_status_topic_ = (mcu_status_topic_ == UNKNOWN) ? "platform/mcu/status" : mcu_status_topic_; - mcu_power_topic_ = get_string_param("mcu_power_topic"); - mcu_power_topic_ = (mcu_power_topic_ == UNKNOWN) ? "platform/mcu/status/power" : mcu_power_topic_; - bms_state_topic_ = get_string_param("bms_state_topic"); - bms_state_topic_ = (bms_state_topic_ == UNKNOWN) ? "platform/bms/state" : bms_state_topic_; - stop_status_topic_ = get_string_param("stop_status_topic"); - stop_status_topic_ = - (stop_status_topic_ == UNKNOWN) ? "platform/mcu/status/stop" : stop_status_topic_; - estop_topic_ = get_string_param("estop_topic"); - estop_topic_ = (estop_topic_ == UNKNOWN) ? "platform/emergency_stop" : estop_topic_; - mcu_status_rate_ = get_double_param("mcu_status_rate"); - mcu_status_rate_ = (std::isnan(mcu_status_rate_)) ? 1.0 : mcu_status_rate_; - mcu_power_rate_ = get_double_param("mcu_power_rate"); - mcu_power_rate_ = (std::isnan(mcu_power_rate_)) ? 10.0 : mcu_power_rate_; - bms_state_rate_ = get_double_param("bms_state_rate"); - bms_state_rate_ = (std::isnan(bms_state_rate_)) ? 1.0 : bms_state_rate_; - stop_status_rate_ = get_double_param("stop_status_rate"); - stop_status_rate_ = (std::isnan(stop_status_rate_)) ? 1.0 : stop_status_rate_; - - // Initialize variables that are populated in callbacks - mcu_firmware_version_ = UNKNOWN; - - // Set Hardware ID as serial number in diagnostics - updater_.setHardwareID(serial_number_); - - // MCU status and firmware version if there is an MCU - 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") { - // Subscribe to MCU Status topics - sub_mcu_status_ = - this->create_subscription( - mcu_status_topic_, - rclcpp::SensorDataQoS(), - std::bind(&ClearpathDiagnosticUpdater::mcu_status_callback, this, std::placeholders::_1)); - - // Create MCU Frequency Status tracking objects - mcu_status_freq_status_ = std::make_shared( - FrequencyStatusParam(&mcu_status_rate_, &mcu_status_rate_, 0.1, 10)); - - // Add diagnostic tasks for MCU data - updater_.add("MCU Status", this, &ClearpathDiagnosticUpdater::mcu_status_diagnostic); - updater_.add("MCU Firmware Version", this, &ClearpathDiagnosticUpdater::firmware_diagnostic); - RCLCPP_INFO(this->get_logger(), "MCU diagnostics started."); - } - - // Diagnostics Applicable to all robot platforms irrelevant of which MCU or battery - // Create subscriptions - sub_mcu_power_ = - this->create_subscription( - mcu_power_topic_, - rclcpp::SensorDataQoS(), - std::bind(&ClearpathDiagnosticUpdater::mcu_power_callback, this, std::placeholders::_1)); - sub_bms_state_ = - this->create_subscription( - bms_state_topic_, - rclcpp::SensorDataQoS(), - std::bind(&ClearpathDiagnosticUpdater::bms_state_callback, this, std::placeholders::_1)); - sub_stop_status_ = - this->create_subscription( - stop_status_topic_, - rclcpp::SensorDataQoS(), - std::bind(&ClearpathDiagnosticUpdater::stop_status_callback, this, std::placeholders::_1)); - sub_estop_ = - this->create_subscription( - estop_topic_, - rclcpp::SensorDataQoS(), - std::bind(&ClearpathDiagnosticUpdater::estop_callback, this, std::placeholders::_1)); - - // Create Frequency Status tracking objects - mcu_power_freq_status_ = std::make_shared( - FrequencyStatusParam(&mcu_power_rate_, &mcu_power_rate_, 0.1, 5)); - bms_state_freq_status_ = std::make_shared( - FrequencyStatusParam(&bms_state_rate_, &bms_state_rate_, 0.15, 10)); - stop_status_freq_status_ = std::make_shared( - FrequencyStatusParam(&stop_status_rate_, &stop_status_rate_, 0.1, 10)); - - // Add diagnostic tasks - updater_.add("Power Status", this, &ClearpathDiagnosticUpdater::mcu_power_diagnostic); - updater_.add("Battery Management System", this, - &ClearpathDiagnosticUpdater::bms_state_diagnostic); - updater_.add("E-stop Status", this, &ClearpathDiagnosticUpdater::stop_status_diagnostic); - - setup_topic_rate_diagnostics(); -} - -/** - * @brief Get string parameter from config yaml and optionally log an error if parameter is missing - */ -std::string ClearpathDiagnosticUpdater::get_string_param(std::string param_name, bool mandatory) -{ - try { - return this->get_parameter(param_name).as_string(); - } catch (const std::exception & e) { - if (mandatory) { - RCLCPP_ERROR(this->get_logger(), "Could not retrieve mandatory parameter %s: %s", - param_name.c_str(), e.what()); - } - return UNKNOWN; - } -} - -/** - * @brief Get double parameter from config yaml and optionally log an error if parameter is missing - */ -double ClearpathDiagnosticUpdater::get_double_param(std::string param_name, bool mandatory) -{ - try { - return this->get_parameter(param_name).as_double(); - } catch (const std::exception & e) { - if (mandatory) { - RCLCPP_ERROR(this->get_logger(), "Could not retrieve mandatory parameter %s: %s", - param_name.c_str(), e.what()); - } - return NAN; - } -} - -/** - * @brief Report the firmware version information to diagnostics - */ -void ClearpathDiagnosticUpdater::firmware_diagnostic(DiagnosticStatusWrapper & stat) -{ - if (latest_apt_firmware_version_ == "not_found") { - stat.summaryf(DiagnosticStatus::ERROR, - "ros-%s-clearpath-firmware package not found", - ros_distro_.c_str()); - } else if (latest_apt_firmware_version_ == UNKNOWN) { - stat.summaryf(DiagnosticStatus::ERROR, - "ros-%s-clearpath-firmware package version not provided in config", - ros_distro_.c_str()); - } else if (mcu_firmware_version_ == UNKNOWN) { - stat.summary(DiagnosticStatus::ERROR, - "No firmware version received from MCU"); - } else if (mcu_firmware_version_ == latest_apt_firmware_version_) { - stat.summaryf(DiagnosticStatus::OK, - "Firmware is up to date (%s)", - mcu_firmware_version_.c_str()); - } else if (mcu_firmware_version_ < latest_apt_firmware_version_) { - stat.summaryf(DiagnosticStatus::WARN, - "New firmware available: (%s) -> (%s)", - mcu_firmware_version_.c_str(), - latest_apt_firmware_version_.c_str()); - } else { - stat.summaryf(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_); -} - -/** - * @brief Save data from MCU Status messages - */ -void ClearpathDiagnosticUpdater::mcu_status_callback( - const clearpath_platform_msgs::msg::Status & msg) -{ - mcu_firmware_version_ = msg.firmware_version; - mcu_status_msg_ = msg; - mcu_status_freq_status_->tick(); -} - -/** - * @brief Report MCU Status message information to diagnostics - */ -void ClearpathDiagnosticUpdater::mcu_status_diagnostic(DiagnosticStatusWrapper & stat) -{ - mcu_status_freq_status_->run(stat); - - if (stat.level != diagnostic_updater::DiagnosticStatusWrapper::ERROR) { - // if status messages are being received then add the message details - stat.add("Firmware Version", mcu_firmware_version_); - stat.add("Platform Model", mcu_status_msg_.hardware_id); - stat.add("MCU Uptime (sec)", mcu_status_msg_.mcu_uptime.sec); - stat.add("Connection Uptime (sec)", mcu_status_msg_.connection_uptime.sec); - } -} - -/** - * @brief Save data from MCU Power messages - */ -void ClearpathDiagnosticUpdater::mcu_power_callback(const clearpath_platform_msgs::msg::Power & msg) -{ - mcu_power_msg_ = msg; - mcu_power_freq_status_->tick(); -} - -/** - * @brief Report MCU Power message information to diagnostics - */ -void ClearpathDiagnosticUpdater::mcu_power_diagnostic(DiagnosticStatusWrapper & stat) -{ - mcu_power_freq_status_->run(stat); - - if (stat.level != diagnostic_updater::DiagnosticStatusWrapper::ERROR) { - // if messages are being received then add the message details - try { - // check if each datapoint is applicable before displaying it - if (mcu_power_msg_.shore_power_connected >= 0) { - stat.add("Shore Power Connected", - DiagnosticLabels::POWER_STATUS.at(mcu_power_msg_.shore_power_connected)); - } - if (mcu_power_msg_.battery_connected >= 0) { - stat.add("Battery Connected", - DiagnosticLabels::POWER_STATUS.at(mcu_power_msg_.battery_connected)); - } - if (mcu_power_msg_.power_12v_user_nominal >= 0) { - stat.add("Power 12V User Nominal", - DiagnosticLabels::POWER_STATUS.at(mcu_power_msg_.power_12v_user_nominal)); - } - if (mcu_power_msg_.charger_connected >= 0) { - stat.add("Charger Connected", - DiagnosticLabels::POWER_STATUS.at(mcu_power_msg_.charger_connected)); - } - if (mcu_power_msg_.charging_complete >= 0) { - stat.add("Charging Complete", - DiagnosticLabels::POWER_STATUS.at(mcu_power_msg_.charging_complete)); - } - } catch(const std::out_of_range & e) { - RCLCPP_ERROR(this->get_logger(), - "Unknown MCU Power message status value with no string description: %s", - e.what()); - } - - try { - for (unsigned i = 0; i < mcu_power_msg_.measured_voltages.size(); i++) { - std::string name = "Measured Voltage: " + - DiagnosticLabels::MEASURED_VOLTAGES.at(platform_model_)[i] + " (V)"; - stat.add(name, mcu_power_msg_.measured_voltages[i]); - } - for (unsigned i = 0; i < mcu_power_msg_.measured_currents.size(); i++) { - std::string name = "Measured Current: " + - DiagnosticLabels::MEASURED_CURRENTS.at(platform_model_)[i] + " (A)"; - stat.add(name, mcu_power_msg_.measured_currents[i]); - } - } catch(const std::out_of_range & e) { - RCLCPP_ERROR(this->get_logger(), - "No measured voltage or current labels for the given platform: %s", e.what()); - } - } -} - -/** - * @brief Save data from BMS State / BatteryState messages - */ -void ClearpathDiagnosticUpdater::bms_state_callback(const BatteryState & msg) -{ - bms_state_msg_ = msg; - bms_state_freq_status_->tick(); -} - -/** - * @brief Report BMS State / BatteryState message information to diagnostics - */ -void ClearpathDiagnosticUpdater::bms_state_diagnostic(DiagnosticStatusWrapper & stat) -{ - std::string power_supply_status = "undefined"; - std::string power_supply_health = "undefined"; - std::string power_supply_technology = "undefined"; - try { - power_supply_status = - DiagnosticLabels::POWER_SUPPLY_STATUS.at(bms_state_msg_.power_supply_status); - power_supply_health = - DiagnosticLabels::POWER_SUPPLY_HEALTH.at(bms_state_msg_.power_supply_health); - power_supply_technology = - DiagnosticLabels::POWER_SUPPLY_TECHNOLOGY.at(bms_state_msg_.power_supply_technology); - } catch(const std::out_of_range & e) { - RCLCPP_ERROR(this->get_logger(), - "Unknown Battery State enum with no string description: %s", e.what()); - } - - bms_state_freq_status_->run(stat); - - if (stat.level != diagnostic_updater::DiagnosticStatusWrapper::ERROR) { - // if messages are being received then add the message details - stat.add("Power Supply Status", power_supply_status); - stat.add("Power Supply Health", power_supply_health); - stat.add("Power Supply Technology", power_supply_technology); - - stat.add("Voltage (V)", bms_state_msg_.voltage); - stat.add("Temperature (C)", bms_state_msg_.temperature); - stat.add("Current (A)", bms_state_msg_.current); - stat.add("Charge (Ah)", bms_state_msg_.charge); - stat.add("Capacity (Ah)", bms_state_msg_.capacity); - stat.add("Charge Percentage", bms_state_msg_.percentage * 100); - - std::string voltages = ""; - for (auto v : bms_state_msg_.cell_voltage) { - voltages.append(std::to_string(v)); - voltages.append("; "); - } - stat.add("Cell Voltages (V)", voltages); - - std::string temperatures = ""; - for (auto t : bms_state_msg_.cell_temperature) { - temperatures.append(std::to_string(t)); - temperatures.append("; "); - } - stat.add("Cell Temperature (C)", temperatures); - - // Diagnostic summaries based on charging activity / level - if (bms_state_msg_.header.stamp.sec != 0) { - if (bms_state_msg_.power_supply_status == BatteryState::POWER_SUPPLY_STATUS_CHARGING) { - stat.mergeSummaryf(DiagnosticStatus::OK, - "Battery Charging (%.1f%%)", - bms_state_msg_.percentage * 100); - } else if (bms_state_msg_.percentage >= 0.2) { - stat.mergeSummaryf(DiagnosticStatus::OK, - "Battery level is %.1f%%", - bms_state_msg_.percentage * 100); - } else if (bms_state_msg_.percentage >= 0.1) { - stat.mergeSummaryf(DiagnosticStatus::WARN, - "Low Battery (%.1f%%)", - bms_state_msg_.percentage * 100); - } else { - stat.mergeSummaryf(DiagnosticStatus::WARN, - "Critically Low Battery (%.1f%%)", - bms_state_msg_.percentage * 100); - } - - // Error diagnostic summaries - if (bms_state_msg_.power_supply_health != BatteryState::POWER_SUPPLY_HEALTH_GOOD) { - stat.mergeSummaryf(DiagnosticStatus::ERROR, - "Power Supply Health: %s", power_supply_health.c_str()); - } else if (bms_state_msg_.power_supply_status == BatteryState::POWER_SUPPLY_STATUS_UNKNOWN) { - stat.mergeSummary(DiagnosticStatus::ERROR, "Power Supply Status Unknown"); - } - } - } -} - -/** - * @brief Save data from Stop Status messages - */ -void ClearpathDiagnosticUpdater::stop_status_callback( - const clearpath_platform_msgs::msg::StopStatus & msg) -{ - stop_status_msg_ = msg; - stop_status_freq_status_->tick(); -} - - -/** - * @brief Save data from E-stop messages - */ -void ClearpathDiagnosticUpdater::estop_callback( - const std_msgs::msg::Bool & msg) -{ - estop_msg_ = msg; -} - -/** - * @brief Report E-stop / Stop Status message information to diagnostics - */ -void ClearpathDiagnosticUpdater::stop_status_diagnostic(DiagnosticStatusWrapper & stat) -{ - stop_status_freq_status_->run(stat); - - if (stat.level != diagnostic_updater::DiagnosticStatusWrapper::ERROR) { - // if status messages are being received then add the message details - stat.add("E-stop Triggered", - (estop_msg_.data ? "True" : "False")); - stat.add("E-stop loop is powered", - (stop_status_msg_.stop_power_status ? "True" : "False")); - stat.add("External E-stop has been plugged in", - (stop_status_msg_.external_stop_present ? "True" : "False")); - stat.add("Stop loop needs reset", - (stop_status_msg_.needs_reset ? "True" : "False")); - - if (stop_status_msg_.header.stamp.sec != 0) { - if (!stop_status_msg_.stop_power_status) { - stat.mergeSummary(DiagnosticStatus::ERROR, "E-stop loop power error"); - } else if (estop_msg_.data) { - stat.mergeSummary(DiagnosticStatus::WARN, "E-stopped"); - } else if (stop_status_msg_.needs_reset) { - stat.mergeSummary(DiagnosticStatus::WARN, "E-stop needs reset"); - } - } - } -} - -/** - * @brief Process topics provided in the config yaml and add publish frequency monitoring for each - */ -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 - try { - if (!this->get_parameters("topics", topic_map_raw)) { - RCLCPP_WARN(this->get_logger(), "No topics found to monitor."); - return; - } - } catch (const std::exception & e) { - 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()); - } -} - -/** - * @brief Template to add rate diagnostics for a given topic and rate - */ -template void ClearpathDiagnosticUpdater::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_, - 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)); -} -} - -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_robot/package.xml b/clearpath_robot/package.xml index 0d4ca70c..92f1f349 100644 --- a/clearpath_robot/package.xml +++ b/clearpath_robot/package.xml @@ -18,7 +18,6 @@ socat - clearpath_diagnostics clearpath_generator_robot clearpath_hardware_interfaces clearpath_sensors