diff --git a/clearpath_diagnostics/CMakeLists.txt b/clearpath_diagnostics/CMakeLists.txt new file mode 100644 index 0000000..c47ea8d --- /dev/null +++ b/clearpath_diagnostics/CMakeLists.txt @@ -0,0 +1,37 @@ +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(ament_cmake_python REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install( + PROGRAMS ${PROJECT_NAME}/diagnostics_updater + ${PROJECT_NAME}/battery_state/battery_state_estimator + ${PROJECT_NAME}/battery_state/battery_state_control + 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/clearpath_diagnostics/__init__.py b/clearpath_diagnostics/clearpath_diagnostics/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/__init__.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py new file mode 100644 index 0000000..2a8d9d9 --- /dev/null +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py @@ -0,0 +1,306 @@ +# Software License Agreement (BSD) +# +# @author Roni Kreinin <rkreinin@clearpathrobotics.com> +# @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. + +from clearpath_platform_msgs.msg import Power +from sensor_msgs.msg import BatteryState + +from clearpath_config.common.types.platform import Platform +from clearpath_config.platform.battery import BatteryConfig + +from math import nan + + +# Base Battery + +class Battery: + class BaseBattery: + """ Base Battery class. """ + + """Battery configuration. Represents number of battery cells in series and parallel.""" + CONFIGURATIONS = { + BatteryConfig.S1P1: (1, 1), + BatteryConfig.S1P2: (1, 2), + BatteryConfig.S1P3: (1, 3), + BatteryConfig.S1P4: (1, 4), + BatteryConfig.S2P1: (2, 1), + BatteryConfig.S4P1: (4, 1), + BatteryConfig.S4P3: (4, 3), + } + + # To be defined in child class + CAPACITY = 0.0 + VOLTAGE = 0.0 + LUT = [] + + def __init__( + self, + platform: Platform, + configuration: str, + rolling_average_period=30, + ) -> None: + self._rolling_average_period = rolling_average_period + self._platform = platform + self._configuration = self.CONFIGURATIONS[configuration] + self._msg = BatteryState() + self._msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_UNKNOWN + self._msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD + self._msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING + self._msg.present = True + self._msg.temperature = nan + self._readings: list[Power] = [] + + # Which power message indices to use + self.power_msg_voltage_index: int = 0 + self.power_msg_current_index: int | list[int] = 0 + match self._platform: + case Platform.J100: + self.power_msg_voltage_index = Power.J100_MEASURED_BATTERY + self.power_msg_current_index = Power.J100_TOTAL_CURRENT + case Platform.A200: + self.power_msg_voltage_index = Power.A200_BATTERY_VOLTAGE + self.power_msg_current_index = [ + Power.A200_MCU_AND_USER_PORT_CURRENT, + Power.A200_LEFT_DRIVER_CURRENT, + Power.A200_RIGHT_DRIVER_CURRENT, + ] + case Platform.W200: + self.power_msg_voltage_index = Power.W200_MEASURED_BATTERY + self.power_msg_current_index = Power.W200_TOTAL_CURRENT + case Platform.DD100 | Platform.DO100: + self.power_msg_voltage_index = Power.D100_MEASURED_BATTERY + self.power_msg_current_index = Power.D100_TOTAL_CURRENT + case Platform.DD150 | Platform.DO150: + self.power_msg_voltage_index = Power.D150_MEASURED_BATTERY + self.power_msg_current_index = Power.D150_TOTAL_CURRENT + + + # System capacity + self._msg.capacity = self._msg.design_capacity = self.system_capacity + + @property + def msg(self) -> BatteryState: + return self._msg + + @property + def series(self) -> int: + return self._configuration[0] + + @property + def parallel(self) -> int: + return self._configuration[1] + + @property + def cell_count(self) -> int: + return self.series * self.parallel + + @property + def system_capacity(self) -> float: + return self.CAPACITY * self.parallel + + @property + def system_voltage(self) -> float: + return self.VOLTAGE * self.series + + def update(self, power_msg: Power): + # Add new reading to rolling average + self._readings.append(power_msg) + self._msg.header = power_msg.header + # Remove oldest reading + if len(self._readings) > self._rolling_average_period: + self._readings.pop(0) + + # Set battery voltage + self._msg.voltage = power_msg.measured_voltages[self.power_msg_voltage_index] + + # Set battery current + if isinstance(self.power_msg_current_index, int): + self._msg.current = power_msg.measured_currents[ + self.power_msg_current_index + ] + elif isinstance(self.power_msg_current_index, list): + self._msg.current = 0.0 + for i in self.power_msg_current_index: + self._msg.current += power_msg.measured_currents[i] + # Set charging status + if power_msg.charger_connected == 1: + self._msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING + else: + self._msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING + # Cells + self.update_cells() + + def linear_interpolation(self, lut: list[list], v: float) -> float: + # Check if voltage is below minimum value + if v <= lut[0][0]: + return lut[0][1] + + for i in range(0, len(lut)): + if v < lut[i][0]: + return (v - lut[i - 1][0]) * (lut[i][1] - lut[i - 1][1]) / ( + lut[i][0] - lut[i - 1][0] + ) + lut[i - 1][1] + + # Return maximum value + return lut[len(lut) - 1][1] + + def update_from_lut(self): + # Calculate state of charge + avg_voltage = 0 + for reading in self._readings: + avg_voltage += reading.measured_voltages[self.power_msg_voltage_index] + avg_voltage /= len(self._readings) + self._msg.percentage = self.linear_interpolation(self.LUT, avg_voltage) + self._msg.charge = self._msg.capacity * self._msg.percentage + + def update_cells(self): + self._msg.cell_voltage = [self._msg.voltage / self.series] * self.cell_count + self._msg.cell_temperature = [nan] * self.cell_count + + # Battery Types + + class LiION(BaseBattery): + """Base Lithium ION battery.""" + LUT = [] + + def __init__( + self, + platform: Platform, + configuration: str, + rolling_average_period=30, + ) -> None: + super().__init__(platform, configuration, rolling_average_period) + self._msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LION + # Multiply LUT voltage by number of batteries in series + self.LUT = [(i * self.series, j) for (i, j) in self.LUT] + + def update(self, power_msg: Power): + super().update(power_msg) + self.update_from_lut() + + class SLA(BaseBattery): + """Base Lead Acid battery.""" + + # Rough estimate of 12V Lead-Acid SoC from voltage, + # scaled such that 11.6V is considered discharged. + # From https://iopscience.iop.org/article/10.1088/1742-6596/1367/1/012077/pdf + LUT = [ + [11.6, 0.0], + [11.7, 0.1], + [11.9, 0.2], + [12.0, 0.3], + [12.2, 0.4], + [12.3, 0.5], + [12.4, 0.6], + [12.5, 0.7], + [12.55, 0.8], + [12.6, 0.90], + [12.65, 0.95], + [12.7, 1.00], + ] + + def __init__(self, + platform: Platform, + configuration: str, + rolling_average_period=30) -> None: + super().__init__(platform, configuration, rolling_average_period) + # Multiply LUT voltage by number of batteries in series + self.LUT = [(i * self.series, j) for (i, j) in self.LUT] + + def update(self, power_msg: Power): + super().update(power_msg) + self.update_from_lut() + + # Batteries + + class HE2613(LiION): + CAPACITY = 12.8 + VOLTAGE = 25.9 + LUT = [ + [21.00, 0.0], + [21.84, 0.1], + [22.68, 0.2], + [23.52, 0.3], + [24.36, 0.4], + [25.20, 0.5], + [26.04, 0.6], + [26.88, 0.7], + [27.72, 0.8], + [28.56, 0.9], + [29.40, 1.0], + ] + + class ES20_12C(SLA): + CAPACITY = 20.0 + VOLTAGE = 12.0 + + class U1_35(SLA): + CAPACITY = 35.0 + VOLTAGE = 12.0 + + class TLV1222(SLA): + CAPACITY = 22.0 + VOLTAGE = 12.0 + + class RB20(LiION): + CAPACITY = 20.0 + VOLTAGE = 12.8 + LUT = [ + [10.5, 0.0], + [12.0, 0.05], + [13.0, 0.1], + [13.25, 0.2], + [13.3, 0.3], + [13.4, 0.4], + [13.45, 0.5], + [13.5, 0.6], + [13.6, 0.7], + [13.7, 0.8], + [13.8, 0.9], + [14.5, 1.0], + ] + + # Match battery name to class + BATTERIES = { + BatteryConfig.HE2613: HE2613, + BatteryConfig.ES20_12C: ES20_12C, + BatteryConfig.U1_35: U1_35, + BatteryConfig.TLV1222: TLV1222, + BatteryConfig.RB20: RB20 + } + + def __new__(cls, + battery: str, + platform: Platform, + configuration: str, + rolling_average_period=30) -> BaseBattery: + return Battery.BATTERIES.setdefault(battery, Battery.BaseBattery)( + platform, configuration, rolling_average_period) diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control new file mode 100755 index 0000000..a4c0b33 --- /dev/null +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control @@ -0,0 +1,134 @@ +#!/usr/bin/env python3 + +# Software License Agreement (BSD) +# +# @author Roni Kreinin <rkreinin@clearpathrobotics.com> +# @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 + +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data + +from std_msgs.msg import UInt8 + +from sensor_msgs.msg import BatteryState + +from clearpath_config.clearpath_config import ClearpathConfig +from clearpath_config.common.types.platform import Platform +from clearpath_generator_common.common import BaseGenerator + +from enum import Enum + + +class BatteryStateControl(Node): + class HmiColour(Enum): + GREEN = 0 + YELLOW = 1 + RED = 2 + + class HmiPattern(Enum): + OFF = 0 + ON = 1 + BLINK = 2 + + def __init__(self, setup_path='/etc/clearpath'): + super().__init__('battery_state_control') + self.setup_path = setup_path + + # Define paths + self.config_path = os.path.join(self.setup_path, 'robot.yaml') + + # Parse YAML into config + self.clearpath_config = ClearpathConfig(self.config_path) + self.platform = self.clearpath_config.platform.get_platform_model() + + self.battery_state_sub = self.create_subscription( + BatteryState, + 'platform/bms/state', + self.battery_state_callback, + qos_profile_sensor_data) + + match self.platform: + case Platform.J100 | Platform.DD100 | Platform.DO100 | Platform.DD150 | Platform.DO150: + self.hmi_battery_pub = self.create_publisher( + UInt8, + 'platform/mcu/_hmi', + qos_profile_sensor_data) + + self.hmi_msg = UInt8() + + # UInt8 represents HMI state + # Bits 7-4: Unused + # Bits 3-2: Battery LED pattern + # Bits 1-0: Battery LED colour + def encode_hmi_msg(self, colour: HmiColour, pattern: HmiPattern) -> UInt8: + self.hmi_msg.data = (pattern.value << 2) | (colour.value) + + def hmi_control(self, percentage: float): + if percentage >= 0.5: + self.encode_hmi_msg( + BatteryStateControl.HmiColour.GREEN, + BatteryStateControl.HmiPattern.ON) + elif percentage >= 0.2: + self.encode_hmi_msg( + BatteryStateControl.HmiColour.YELLOW, + BatteryStateControl.HmiPattern.ON) + elif percentage >= 0.1: + self.encode_hmi_msg( + BatteryStateControl.HmiColour.RED, + BatteryStateControl.HmiPattern.ON) + else: + self.encode_hmi_msg( + BatteryStateControl.HmiColour.RED, + BatteryStateControl.HmiPattern.BLINK) + self.hmi_battery_pub.publish(self.hmi_msg) + + def battery_state_callback(self, msg: BatteryState): + match self.platform: + case Platform.J100 | Platform.DD100 | Platform.DO100 | Platform.DD150 | Platform.DO150: + self.hmi_control(msg.percentage) + + +def main(): + setup_path = BaseGenerator.get_args() + rclpy.init() + + bsc = BatteryStateControl(setup_path) + + rclpy.spin(bsc) + + bsc.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_estimator b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_estimator new file mode 100755 index 0000000..464f049 --- /dev/null +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_estimator @@ -0,0 +1,99 @@ +#!/usr/bin/env python3 + +# Software License Agreement (BSD) +# +# @author Roni Kreinin <rkreinin@clearpathrobotics.com> +# @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 + +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data + +from clearpath_platform_msgs.msg import Power +from sensor_msgs.msg import BatteryState + +from clearpath_config.clearpath_config import ClearpathConfig +from clearpath_config.common.types.platform import Platform +from clearpath_generator_common.common import BaseGenerator + +from clearpath_diagnostics.battery_state.battery import Battery + + +class BatteryStateEstimator(Node): + def __init__(self, setup_path='/etc/clearpath'): + super().__init__('battery_state_estimator') + self.setup_path = setup_path + + # Define paths + self.config_path = os.path.join(self.setup_path, 'robot.yaml') + + # Parse YAML into config + self.clearpath_config = ClearpathConfig(self.config_path) + self.platform = self.clearpath_config.platform.get_platform_model() + self.battery_model = self.clearpath_config.platform.battery.model + self.battery_configuration = self.clearpath_config.platform.battery.configuration + + self.battery = Battery( + battery=self.battery_model, + configuration=self.battery_configuration, + platform=self.platform) + + self.battery_state_pub = self.create_publisher( + BatteryState, + 'platform/bms/state', + qos_profile_sensor_data) + + self.power_status_sub = self.create_subscription( + Power, + 'platform/mcu/status/power', + self.power_status_callback, + qos_profile_sensor_data) + + def power_status_callback(self, msg: Power): + self.battery.update(msg) + self.battery_state_pub.publish(self.battery.msg) + + +def main(): + setup_path = BaseGenerator.get_args() + rclpy.init() + + bsp = BatteryStateEstimator(setup_path) + + rclpy.spin(bsp) + + bsp.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater b/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater new file mode 100755 index 0000000..6060b6f --- /dev/null +++ b/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater @@ -0,0 +1,248 @@ +#!/usr/bin/env python3 + +# Software License Agreement (BSD) +# +# @author Roni Kreinin <rkreinin@clearpathrobotics.com> +# @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_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['ros-humble-clearpath-firmware'] + versions = pkg.versions + newest: Version = versions[0] + self.newest_firmware_version = newest.version.split('-')[0] + except KeyError: + self.get_logger().warn('ros-humble-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, 'ros-humble-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, 'ros-humble-clearpath-firmware package is outdated.') + return stat + + def get_headerless_topic_diagnostic(self, + sensor: BaseSensor, + topic: str) -> HeaderlessTopicDiagnostic: + sensor_topic = sensor.get_topic(topic) + 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), + self.get_headerless_topic_diagnostic( + sensor, IntelRealsense.TOPICS.COLOR_IMAGE) + )) + if sensor.depth_enabled: + diagnostics.append(( + Image, + sensor.get_topic(IntelRealsense.TOPICS.DEPTH_IMAGE), + self.get_headerless_topic_diagnostic( + sensor, IntelRealsense.TOPICS.DEPTH_IMAGE) + )) + if sensor.pointcloud_enabled: + diagnostics.append(( + PointCloud2, + sensor.get_topic(IntelRealsense.TOPICS.POINTCLOUD), + self.get_headerless_topic_diagnostic( + sensor, IntelRealsense.TOPICS.POINTCLOUD) + )) + + case BaseCamera(): + diagnostics.append(( + Image, + sensor.get_topic(BaseCamera.TOPICS.COLOR_IMAGE), + self.get_headerless_topic_diagnostic( + sensor, BaseCamera.TOPICS.COLOR_IMAGE) + )) + + case BaseLidar2D(): + diagnostics.append(( + LaserScan, + sensor.get_topic(BaseLidar2D.TOPICS.SCAN), + self.get_headerless_topic_diagnostic( + sensor, BaseLidar2D.TOPICS.SCAN) + )) + + case BaseLidar3D(): + diagnostics.append(( + LaserScan, + sensor.get_topic(BaseLidar3D.TOPICS.SCAN), + self.get_headerless_topic_diagnostic( + sensor, BaseLidar3D.TOPICS.SCAN) + )) + diagnostics.append(( + PointCloud2, + sensor.get_topic(BaseLidar3D.TOPICS.POINTS), + self.get_headerless_topic_diagnostic( + sensor, BaseLidar3D.TOPICS.POINTS) + )) + + case BaseIMU(): + diagnostics.append(( + Imu, + sensor.get_topic(BaseIMU.TOPICS.DATA), + self.get_headerless_topic_diagnostic( + sensor, BaseIMU.TOPICS.DATA) + )) + diagnostics.append(( + MagneticField, + sensor.get_topic(BaseIMU.TOPICS.MAG), + self.get_headerless_topic_diagnostic( + sensor, BaseIMU.TOPICS.MAG) + )) + + case BaseGPS(): + diagnostics.append(( + NavSatFix, + sensor.get_topic(BaseGPS.TOPICS.FIX), + 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/diagnostics.yaml b/clearpath_diagnostics/config/diagnostics.yaml new file mode 100644 index 0000000..0121fbe --- /dev/null +++ b/clearpath_diagnostics/config/diagnostics.yaml @@ -0,0 +1,36 @@ +/**: + 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/launch/diagnostics.launch.py b/clearpath_diagnostics/launch/diagnostics.launch.py new file mode 100644 index 0000000..4395465 --- /dev/null +++ b/clearpath_diagnostics/launch/diagnostics.launch.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 + +# Software License Agreement (BSD) +# +# @author Roni Kreinin <rkreinin@clearpathrobotics.com> +# @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. + +from ament_index_python.packages import get_package_share_directory + +from clearpath_config.common.utils.yaml import read_yaml +from clearpath_config.clearpath_config import ClearpathConfig + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + OpaqueFunction, +) +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") + + setup_path = LaunchConfiguration("setup_path") + + analyzer_params_filepath = PathJoinSubstitution( + [pkg_clearpath_diagnostics, "config", "diagnostics.yaml"] + ) + + # Read robot YAML + config = read_yaml(setup_path.perform(context) + "robot.yaml") + # Parse robot YAML into config + clearpath_config = ClearpathConfig(config) + + namespace = clearpath_config.system.namespace + diagnostics = GroupAction( + [ + PushRosNamespace(namespace), + # Aggregator + Node( + package="diagnostic_aggregator", + executable="aggregator_node", + output="screen", + parameters=[analyzer_params_filepath], + remappings=[ + ("/diagnostics", "diagnostics"), + ("/diagnostics_agg", "diagnostics_agg"), + ("/diagnostics_toplevel_state", "diagnostics_toplevel_state"), + ], + ), + # Updater + Node( + package="clearpath_diagnostics", + executable="diagnostics_updater", + output="screen", + 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)) + return ld diff --git a/clearpath_diagnostics/package.xml b/clearpath_diagnostics/package.xml new file mode 100644 index 0000000..e89a63a --- /dev/null +++ b/clearpath_diagnostics/package.xml @@ -0,0 +1,27 @@ +<?xml version="1.0"?> +<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> +<package format="3"> + <name>clearpath_diagnostics</name> + <version>0.0.0</version> + <description>Clearpath Robot Diagnostics scripts</description> + <maintainer email="rkreinin@clearpathrobotics.com">rkreinin</maintainer> + <license>BSD</license> + + <buildtool_depend>ament_cmake</buildtool_depend> + + <exec_depend>clearpath_config</exec_depend> + <exec_depend>clearpath_generator_common</exec_depend> + <exec_depend>clearpath_platform_msgs</exec_depend> + <exec_depend>diagnostic_updater</exec_depend> + <exec_depend>diagnostic_aggregator</exec_depend> + <exec_depend>diagnostic_msgs</exec_depend> + <exec_depend>sensor_msgs</exec_depend> + <exec_depend>rclpy</exec_depend> + + <test_depend>ament_lint_auto</test_depend> + <test_depend>ament_lint_common</test_depend> + + <export> + <build_type>ament_cmake</build_type> + </export> +</package> diff --git a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py index 0f099be..1e946c2 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py +++ b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py @@ -32,7 +32,7 @@ # modification, is not permitted without the express permission # of Clearpath Robotics. -from clearpath_generator_common.common import LaunchFile +from clearpath_generator_common.common import LaunchFile, Package from clearpath_generator_common.launch.writer import LaunchWriter from clearpath_generator_common.launch.generator import LaunchGenerator from clearpath_generator_robot.launch.sensors import SensorLaunch @@ -79,8 +79,17 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: ] ) - # J100 Add micro ros agent - self.uros_node = LaunchFile.Node( + # Ethernet MicroROS Agent + self.eth_uros_node = LaunchFile.Node( + name='micro_ros_agent', + package='micro_ros_agent', + executable='micro_ros_agent', + namespace=self.namespace, + arguments=['udp4', '--port', '11411'], + ) + + # J100 MicroROS Agent + self.j100_uros_node = LaunchFile.Node( name='micro_ros_agent', package='micro_ros_agent', executable='micro_ros_agent', @@ -119,6 +128,44 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: ], ) + # Diagnostics + clearpath_diagnostics_package = Package('clearpath_diagnostics') + self.diagnostics_launch = LaunchFile('diagnostics', package=clearpath_diagnostics_package) + + # Battery state + self.battery_state_estimator = LaunchFile.Node( + package='clearpath_diagnostics', + executable='battery_state_estimator', + name='battery_state_estimator', + namespace=self.namespace, + arguments=['-s', setup_path] + ) + + self.battery_state_control = LaunchFile.Node( + package='clearpath_diagnostics', + executable='battery_state_control', + name='battery_state_control', + namespace=self.namespace, + arguments=['-s', setup_path] + ) + + # Lighting + self.lighting_node = LaunchFile.Node( + package='clearpath_platform', + executable='lighting_node', + name='lighting_node', + namespace=self.namespace, + parameters=[{'platform': self.platform_model}] + ) + + # Sevcon + self.sevcon_node = LaunchFile.Node( + package='sevcon_traction', + executable='sevcon_traction_node', + name='sevcon_traction_node', + namespace=self.namespace, + ) + # Static transform from <namespace>/odom to odom # See https://github.com/ros-controls/ros2_controllers/pull/533 self.tf_namespaced_odom_publisher = LaunchFile.get_static_tf_node( @@ -139,17 +186,57 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: ) # Components required for each platform + common_platform_components = [ + self.wireless_watcher_node, + self.diagnostics_launch, + self.battery_state_estimator, + self.battery_state_control + ] + self.platform_components = { - Platform.J100: [ + Platform.J100: common_platform_components + [ self.imu_0_filter_node, self.imu_0_filter_config, self.configure_mcu, - self.uros_node, - self.nmea_driver_node, - self.wireless_watcher_node, + self.j100_uros_node, + self.nmea_driver_node ], - Platform.A200: [ - self.wireless_watcher_node + Platform.A200: common_platform_components, + Platform.W200: common_platform_components + [ + self.imu_0_filter_node, + self.imu_0_filter_config, + self.eth_uros_node, + self.configure_mcu, + self.lighting_node, + self.sevcon_node + ], + Platform.DD100: common_platform_components + [ + self.imu_0_filter_node, + self.imu_0_filter_config, + self.eth_uros_node, + self.configure_mcu, + self.lighting_node, + ], + Platform.DO100: common_platform_components + [ + self.imu_0_filter_node, + self.imu_0_filter_config, + self.eth_uros_node, + self.configure_mcu, + self.lighting_node, + ], + Platform.DD150: common_platform_components + [ + self.imu_0_filter_node, + self.imu_0_filter_config, + self.eth_uros_node, + self.configure_mcu, + self.lighting_node, + ], + Platform.DO150: common_platform_components + [ + self.imu_0_filter_node, + self.imu_0_filter_config, + self.eth_uros_node, + self.configure_mcu, + self.lighting_node, ], } diff --git a/clearpath_generator_robot/package.xml b/clearpath_generator_robot/package.xml index 41f3335..01a1006 100644 --- a/clearpath_generator_robot/package.xml +++ b/clearpath_generator_robot/package.xml @@ -16,6 +16,8 @@ <exec_depend>clearpath_sensors</exec_depend> <exec_depend>imu_filter_madgwick</exec_depend> <exec_depend>micro_ros_agent</exec_depend> + <!-- Disabled for now.--> + <!-- <exec_depend>sevcon_traction</exec_depend> --> <exec_depend>wireless_watcher</exec_depend> <test_depend>ament_lint_auto</test_depend> diff --git a/clearpath_robot/debian/udev b/clearpath_robot/debian/udev index 66184ce..cd67145 100644 --- a/clearpath_robot/debian/udev +++ b/clearpath_robot/debian/udev @@ -13,4 +13,8 @@ SUBSYSTEM=="tty", KERNEL=="ttyUSB[0-9]*", ATTRS{idProduct}=="6001", ATTRS{idVend # Udev rule for the Logitech controllers KERNEL=="js*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c21f", SYMLINK+="input/f710" KERNEL=="js*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c219", SYMLINK+="input/f710" -KERNEL=="js*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c21d", SYMLINK+="input/f310" \ No newline at end of file +KERNEL=="js*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="c21d", SYMLINK+="input/f310" + +# CAN rule for PCI CANBUS card +SUBSYSTEM=="net", KERNEL == "can0", RUN+="/bin/sh -c 'ip link set can0 type can bitrate 1000000; ip link set can0 up'" +SUBSYSTEM=="net", KERNEL == "can1", RUN+="/bin/sh -c 'ip link set can1 type can bitrate 500000; ip link set can1 up'" \ No newline at end of file diff --git a/clearpath_robot/package.xml b/clearpath_robot/package.xml index d6f856a..91b6305 100644 --- a/clearpath_robot/package.xml +++ b/clearpath_robot/package.xml @@ -14,6 +14,7 @@ <buildtool_depend>ament_cmake</buildtool_depend> + <exec_depend>can-utils</exec_depend> <!-- Metapackage dependencies--> <exec_depend>clearpath_generator_robot</exec_depend> <exec_depend>clearpath_sensors</exec_depend>