From b98ea8297e1066106f034f207b51d71d40464f17 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Wed, 27 Sep 2023 11:25:25 -0400 Subject: [PATCH 01/41] W200 uROS node --- .../launch/generator.py | 20 ++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py index 0f099be..cc09e68 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py +++ b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py @@ -79,8 +79,17 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: ] ) - # J100 Add micro ros agent - self.uros_node = LaunchFile.Node( + # W200 MicroROS Agent + self.w200_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', @@ -144,13 +153,18 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: self.imu_0_filter_node, self.imu_0_filter_config, self.configure_mcu, - self.uros_node, + self.j100_uros_node, self.nmea_driver_node, self.wireless_watcher_node, ], Platform.A200: [ self.wireless_watcher_node ], + Platform.W200: [ + self.w200_uros_node, + self.configure_mcu, + self.wireless_watcher_node, + ] } def generate_sensors(self) -> None: From 376770ac3f8af429b0316f7dc47a91431c216a81 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Thu, 5 Oct 2023 12:22:06 -0400 Subject: [PATCH 02/41] Firmware and sensor diagnostics --- clearpath_diagnostics/CMakeLists.txt | 35 ++++ .../clearpath_diagnostics/__init__.py | 0 .../clearpath_diagnostics/diagnostics_updater | 165 ++++++++++++++++++ clearpath_diagnostics/config/diagnostics.yaml | 31 ++++ .../launch/diagnostics.launch.py | 85 +++++++++ clearpath_diagnostics/package.xml | 27 +++ 6 files changed, 343 insertions(+) create mode 100644 clearpath_diagnostics/CMakeLists.txt create mode 100644 clearpath_diagnostics/clearpath_diagnostics/__init__.py create mode 100755 clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater create mode 100644 clearpath_diagnostics/config/diagnostics.yaml create mode 100644 clearpath_diagnostics/launch/diagnostics.launch.py create mode 100644 clearpath_diagnostics/package.xml diff --git a/clearpath_diagnostics/CMakeLists.txt b/clearpath_diagnostics/CMakeLists.txt new file mode 100644 index 0000000..321b948 --- /dev/null +++ b/clearpath_diagnostics/CMakeLists.txt @@ -0,0 +1,35 @@ +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 + 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/diagnostics_updater b/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater new file mode 100755 index 0000000..5d9064e --- /dev/null +++ b/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater @@ -0,0 +1,165 @@ +#!/usr/bin/env python3 + +# Software License Agreement (BSD) +# +# @author Roni Kreinin +# @copyright (c) 2023, Clearpath Robotics, Inc., All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Clearpath Robotics nor the names of its contributors +# may be used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# Redistribution and use in source and binary forms, with or without +# modification, is not permitted without the express permission +# of Clearpath Robotics. + +import os + +from diagnostic_msgs.msg import DiagnosticStatus + +from diagnostic_updater import FrequencyStatusParam, HeaderlessTopicDiagnostic, Updater, DiagnosticStatusWrapper + +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data + +from sensor_msgs.msg import BatteryState, Image, Imu, LaserScan, 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 +from clearpath_config.sensors.types.lidars_2d import BaseLidar2D +from clearpath_config.sensors.types.lidars_3d import BaseLidar3D + +from clearpath_platform_msgs.msg import Status + + +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(self.config_path) + # Parse YAML into config + self.clearpath_config = ClearpathConfig(self.config) + + 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) + + # Initial values + self.firmware_version = '0.0.0' + self.newest_firmware_version = '0.0.1' + 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_commit + self.mcu_temperature = msg.mcu_temperature + self.pcb_temperature = msg.pcb_temperature + + def check_firmware_version(self, stat: DiagnosticStatusWrapper): + if self.firmware_version == '0.0.1': + stat.summary(DiagnosticStatus.OK, f'Firmware is up to date (v{self.firmware_version})') + else: + stat.summary(DiagnosticStatus.WARN, f'New firmware available. (v{self.firmware_version}) -> (v{self.newest_firmware_version})') + return stat + + def add_sensor_diagnostic(self, sensor: BaseSensor): + if self.namespace in ('', '/'): + topic = f'sensors/{sensor.name}' + else: + topic = f'{self.namespace}/sensors/{sensor.name}' + + freq_bounds = {} + message_type = None + sensor_type = sensor.get_sensor_type() + + if sensor_type == BaseCamera.SENSOR_TYPE: + freq_bounds = self.camera_freq_bounds + message_type = Image + elif sensor_type == BaseLidar2D.SENSOR_TYPE: + freq_bounds = self.lidar2d_freq_bounds + message_type = LaserScan + elif sensor_type == BaseLidar3D.SENSOR_TYPE: + freq_bounds = self.lidar3d_freq_bounds + message_type = PointCloud2 + + diagnostic = HeaderlessTopicDiagnostic( + topic, + self.updater, + FrequencyStatusParam(freq_bounds, 0.1, 10)) + + self.create_subscription( + message_type, + topic, + lambda msg: diagnostic.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..efe345e --- /dev/null +++ b/clearpath_diagnostics/config/diagnostics.yaml @@ -0,0 +1,31 @@ +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' ] diff --git a/clearpath_diagnostics/launch/diagnostics.launch.py b/clearpath_diagnostics/launch/diagnostics.launch.py new file mode 100644 index 0000000..c3e8804 --- /dev/null +++ b/clearpath_diagnostics/launch/diagnostics.launch.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python3 + +# Software License Agreement (BSD) +# +# @author Roni Kreinin +# @copyright (c) 2023, Clearpath Robotics, Inc., All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Clearpath Robotics nor the names of its contributors +# may be used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# Redistribution and use in source and binary forms, with or without +# modification, is not permitted without the express permission +# of Clearpath Robotics. + +from ament_index_python.packages import get_package_share_directory + +import launch +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + + pkg_clearpath_diagnostics = get_package_share_directory('clearpath_diagnostics') + + analyzer_params_filepath = PathJoinSubstitution( + [pkg_clearpath_diagnostics, 'config', 'diagnostics.yaml']) + + namespaced_param_file = RewrittenYaml( + source_file=analyzer_params_filepath, + root_key=LaunchConfiguration('namespace'), + param_rewrites={}, + convert_types=True) + + aggregator = Node( + package='diagnostic_aggregator', + executable='aggregator_node', + output='screen', + parameters=[namespaced_param_file], + remappings=[ + ('/diagnostics', 'diagnostics'), + ('/diagnostics_agg', 'diagnostics_agg'), + ('/diagnostics_toplevel_state', 'diagnostics_toplevel_state') + ]) + diag_publisher = Node( + package='clearpath_diagnostics', + executable='diagnostics_updater', + output='screen', + remappings=[ + ('/diagnostics', 'diagnostics'), + ('/diagnostics_agg', 'diagnostics_agg'), + ('/diagnostics_toplevel_state', 'diagnostics_toplevel_state')] + ) + return launch.LaunchDescription([ + aggregator, + diag_publisher, + launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=aggregator, + on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())], + )), + ]) \ No newline at end of file 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 @@ + + + + clearpath_diagnostics + 0.0.0 + Clearpath Robot Diagnostics scripts + rkreinin + BSD + + ament_cmake + + clearpath_config + clearpath_generator_common + clearpath_platform_msgs + diagnostic_updater + diagnostic_aggregator + diagnostic_msgs + sensor_msgs + rclpy + + ament_lint_auto + ament_lint_common + + + ament_cmake + + From 571520be0c6ada4a26aa885ddf3d8e9aa8e85326 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Tue, 10 Oct 2023 16:28:01 -0400 Subject: [PATCH 03/41] Add all sensors --- .../clearpath_diagnostics/diagnostics_updater | 143 +++++++++++++----- clearpath_diagnostics/config/diagnostics.yaml | 4 + 2 files changed, 110 insertions(+), 37 deletions(-) diff --git a/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater b/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater index 5d9064e..e593971 100755 --- a/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater +++ b/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater @@ -42,18 +42,21 @@ import rclpy from rclpy.node import Node from rclpy.qos import qos_profile_sensor_data -from sensor_msgs.msg import BatteryState, Image, Imu, LaserScan, PointCloud2 +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 -from clearpath_config.sensors.types.lidars_2d import BaseLidar2D -from clearpath_config.sensors.types.lidars_3d import BaseLidar3D +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 class ClearpathDiagnosticUpdater(Node): @@ -65,10 +68,10 @@ class ClearpathDiagnosticUpdater(Node): self.config_path = os.path.join(setup_path, 'robot.yaml') assert os.path.exists(self.config_path) - # Read YAML - self.config = read_yaml(self.config_path) + # # Read YAML + # self.config = read_yaml() # Parse YAML into config - self.clearpath_config = ClearpathConfig(self.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() @@ -116,37 +119,103 @@ class ClearpathDiagnosticUpdater(Node): stat.summary(DiagnosticStatus.WARN, f'New firmware available. (v{self.firmware_version}) -> (v{self.newest_firmware_version})') 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): - if self.namespace in ('', '/'): - topic = f'sensors/{sensor.name}' - else: - topic = f'{self.namespace}/sensors/{sensor.name}' - - freq_bounds = {} - message_type = None - sensor_type = sensor.get_sensor_type() - - if sensor_type == BaseCamera.SENSOR_TYPE: - freq_bounds = self.camera_freq_bounds - message_type = Image - elif sensor_type == BaseLidar2D.SENSOR_TYPE: - freq_bounds = self.lidar2d_freq_bounds - message_type = LaserScan - elif sensor_type == BaseLidar3D.SENSOR_TYPE: - freq_bounds = self.lidar3d_freq_bounds - message_type = PointCloud2 - - diagnostic = HeaderlessTopicDiagnostic( - topic, - self.updater, - FrequencyStatusParam(freq_bounds, 0.1, 10)) - - self.create_subscription( - message_type, - topic, - lambda msg: diagnostic.tick(), - qos_profile_sensor_data - ) + 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(): diff --git a/clearpath_diagnostics/config/diagnostics.yaml b/clearpath_diagnostics/config/diagnostics.yaml index efe345e..a227460 100644 --- a/clearpath_diagnostics/config/diagnostics.yaml +++ b/clearpath_diagnostics/config/diagnostics.yaml @@ -29,3 +29,7 @@ analyzers: type: diagnostic_aggregator/GenericAnalyzer path: IMU contains: [ 'imu' ] + gps: + type: diagnostic_aggregator/GenericAnalyzer + path: GPS + contains: [ 'gps' ] From 4b5c8090e2b131cd3c7983433d70174d882f6374 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Wed, 11 Oct 2023 15:07:00 -0400 Subject: [PATCH 04/41] Check ros-humble-clearpath-firmware package version --- .../clearpath_diagnostics/diagnostics_updater | 23 +++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater b/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater index e593971..dde43e2 100755 --- a/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater +++ b/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater @@ -57,6 +57,8 @@ 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): @@ -96,9 +98,18 @@ class ClearpathDiagnosticUpdater(Node): for sensor in self.clearpath_config.sensors.get_all_sensors(): self.add_sensor_diagnostic(sensor) - # Initial values + # 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.newest_firmware_version = '0.0.1' self.battery_percentage = 0.0 self.battery_voltage = 0.0 @@ -113,10 +124,14 @@ class ClearpathDiagnosticUpdater(Node): self.pcb_temperature = msg.pcb_temperature def check_firmware_version(self, stat: DiagnosticStatusWrapper): - if self.firmware_version == '0.0.1': + 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})') - else: + 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, From 9c205fd0b0f42565d8fd46aaf499cdaf6125db71 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Wed, 11 Oct 2023 15:41:52 -0400 Subject: [PATCH 05/41] Get namespace from robot.yaml for diagnostics launch Added diagnostics launch to generator --- clearpath_diagnostics/config/diagnostics.yaml | 71 ++++++------ .../launch/diagnostics.launch.py | 107 +++++++++++------- .../launch/generator.py | 11 +- 3 files changed, 111 insertions(+), 78 deletions(-) diff --git a/clearpath_diagnostics/config/diagnostics.yaml b/clearpath_diagnostics/config/diagnostics.yaml index a227460..0121fbe 100644 --- a/clearpath_diagnostics/config/diagnostics.yaml +++ b/clearpath_diagnostics/config/diagnostics.yaml @@ -1,35 +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' ] +/**: + 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 index c3e8804..36393ec 100644 --- a/clearpath_diagnostics/launch/diagnostics.launch.py +++ b/clearpath_diagnostics/launch/diagnostics.launch.py @@ -34,52 +34,77 @@ from ament_index_python.packages import get_package_share_directory -import launch +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 +from launch_ros.actions import Node, PushRosNamespace -from nav2_common.launch import RewrittenYaml +ARGUMENTS = [ + DeclareLaunchArgument( + "setup_path", + default_value="/etc/clearpath/", + description="Clearpath setup path", + ) +] -def generate_launch_description(): - pkg_clearpath_diagnostics = get_package_share_directory('clearpath_diagnostics') +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']) - - namespaced_param_file = RewrittenYaml( - source_file=analyzer_params_filepath, - root_key=LaunchConfiguration('namespace'), - param_rewrites={}, - convert_types=True) - - aggregator = Node( - package='diagnostic_aggregator', - executable='aggregator_node', - output='screen', - parameters=[namespaced_param_file], - remappings=[ - ('/diagnostics', 'diagnostics'), - ('/diagnostics_agg', 'diagnostics_agg'), - ('/diagnostics_toplevel_state', 'diagnostics_toplevel_state') - ]) - diag_publisher = Node( - package='clearpath_diagnostics', - executable='diagnostics_updater', - output='screen', - remappings=[ - ('/diagnostics', 'diagnostics'), - ('/diagnostics_agg', 'diagnostics_agg'), - ('/diagnostics_toplevel_state', 'diagnostics_toplevel_state')] - ) - return launch.LaunchDescription([ - aggregator, - diag_publisher, - launch.actions.RegisterEventHandler( - event_handler=launch.event_handlers.OnProcessExit( - target_action=aggregator, - on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())], - )), - ]) \ No newline at end of file + [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"), + ], + ), + ] + ) + + return [diagnostics] + + +def generate_launch_description(): + ld = LaunchDescription(ARGUMENTS) + ld.add_action(OpaqueFunction(function=launch_setup)) + return ld diff --git a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py index cc09e68..45a6b37 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 @@ -128,6 +128,10 @@ 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) + # Static transform from /odom to odom # See https://github.com/ros-controls/ros2_controllers/pull/533 self.tf_namespaced_odom_publisher = LaunchFile.get_static_tf_node( @@ -156,14 +160,17 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: self.j100_uros_node, self.nmea_driver_node, self.wireless_watcher_node, + self.diagnostics_launch ], Platform.A200: [ - self.wireless_watcher_node + self.wireless_watcher_node, + self.diagnostics_launch ], Platform.W200: [ self.w200_uros_node, self.configure_mcu, self.wireless_watcher_node, + self.diagnostics_launch ] } From f6856b7696b94dd97b6d94fb86fda841f67303e9 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Wed, 11 Oct 2023 16:07:47 -0400 Subject: [PATCH 06/41] Pass setup path --- clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater | 1 - clearpath_diagnostics/launch/diagnostics.launch.py | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater b/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater index dde43e2..583efc7 100755 --- a/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater +++ b/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater @@ -61,7 +61,6 @@ 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 diff --git a/clearpath_diagnostics/launch/diagnostics.launch.py b/clearpath_diagnostics/launch/diagnostics.launch.py index 36393ec..4395465 100644 --- a/clearpath_diagnostics/launch/diagnostics.launch.py +++ b/clearpath_diagnostics/launch/diagnostics.launch.py @@ -97,6 +97,7 @@ def launch_setup(context, *args, **kwargs): ("/diagnostics_agg", "diagnostics_agg"), ("/diagnostics_toplevel_state", "diagnostics_toplevel_state"), ], + arguments=['-s', setup_path] ), ] ) From 43d76797ea074412d4b3f8762d45cde92c8ab989 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Wed, 11 Oct 2023 17:33:48 -0400 Subject: [PATCH 07/41] Initial battery state publisher --- clearpath_diagnostics/CMakeLists.txt | 1 + .../battery_state_publisher/__init__.py | 0 .../battery_state_publisher/battery.py | 103 +++++++++++++++ .../battery_state_publisher | 118 ++++++++++++++++++ 4 files changed, 222 insertions(+) create mode 100644 clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/__init__.py create mode 100644 clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py create mode 100755 clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher diff --git a/clearpath_diagnostics/CMakeLists.txt b/clearpath_diagnostics/CMakeLists.txt index 321b948..b029b3f 100644 --- a/clearpath_diagnostics/CMakeLists.txt +++ b/clearpath_diagnostics/CMakeLists.txt @@ -13,6 +13,7 @@ ament_python_install_package(${PROJECT_NAME}) install( PROGRAMS ${PROJECT_NAME}/diagnostics_updater + ${PROJECT_NAME}/battery_state_publisher/battery_state_publisher DESTINATION lib/${PROJECT_NAME} ) diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/__init__.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py new file mode 100644 index 0000000..9e9f58c --- /dev/null +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python3 + +# Software License Agreement (BSD) +# +# @author Roni Kreinin +# @copyright (c) 2023, Clearpath Robotics, Inc., All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Clearpath Robotics nor the names of its contributors +# may be used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# Redistribution and use in source and binary forms, with or without +# modification, is not permitted without the express permission +# of Clearpath Robotics. + +from builtin_interfaces.msg import Time +from sensor_msgs.msg import BatteryState +from enum import Enum + + +class Battery: + class Type(Enum): + HE2613 = 0 + + class Calculation(Enum): + LINEAR_INTERPOLATION = 0 + BMS = 1 + + # Lookup table + LUT = { + Type.HE2613: [ + [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], + ] + } + + def __init__(self, battery_type: Type) -> None: + self.battery_type = battery_type + self._msg = BatteryState() + self._msg.header.frame_id = 'battery_link' + + match self.battery_type: + case Battery.Type.HE2613: + self.calculation = Battery.Calculation.LINEAR_INTERPOLATION + self.lut = Battery.LUT[Battery.Type.HE2613] + self._msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LION + + @property + def msg(self) -> BatteryState: + return self._msg + + def update(self, voltage: float, current: float): + self._msg.voltage = voltage + self._msg.current = current + match self.calculation: + case Battery.Calculation.LINEAR_INTERPOLATION: + self._msg.percentage = self.linear_extrapolation(self.lut, voltage) + case Battery.Calculation.BMS: + pass + + def stamp(self, stamp: Time): + self._msg.header.stamp = stamp + + def linear_extrapolation(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] diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher new file mode 100755 index 0000000..0364cc8 --- /dev/null +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher @@ -0,0 +1,118 @@ +#!/usr/bin/env python3 + +# Software License Agreement (BSD) +# +# @author Roni Kreinin +# @copyright (c) 2023, Clearpath Robotics, Inc., All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Clearpath Robotics nor the names of its contributors +# may be used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# Redistribution and use in source and binary forms, with or without +# modification, is not permitted without the express permission +# of Clearpath Robotics. + +import os + +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_publisher.battery import Battery + + +class BatteryStatePublisher(Node): + def __init__(self, setup_path='/etc/clearpath'): + super().__init__('battery_state_publisher') + self.setup_path = setup_path + + # Define paths + self.config_path = os.path.join(self.setup_path, 'robot.yaml') + assert os.path.exists(self.config_path) + + # Parse YAML into config + self.clearpath_config = ClearpathConfig(self.config_path) + self.platform = self.clearpath_config.platform.get_platform_model() + + self.battery_state_pub = self.create_publisher( + BatteryState, + 'platform/battery_status', + qos_profile_sensor_data) + + self.power_status_sub = self.create_subscription( + Power, + 'platform/mcu/status/power', + self.power_status_callback, + qos_profile_sensor_data + ) + + match self.platform: + case Platform.J100: + self.battery = Battery(Battery.Type.HE2613) + case Platform.A200: + self.battery = Battery(Battery.Type.HE2613) + case Platform.W200: + self.battery = Battery(Battery.Type.HE2613) + + def power_status_callback(self, msg: Power): + match self.platform: + case Platform.J100: + voltage = msg.measured_voltages[Power.JACKAL_MEASURED_BATTERY] + current = msg.measured_currents[Power.JACKAL_TOTAL_CURRENT] + self.battery.update(voltage, current) + case Platform.A200: + voltage = msg.measured_voltages[Power.A200_BATTERY_VOLTAGE] + current = msg.measured_currents[Power.A200_LEFT_DRIVER_CURRENT] + \ + msg.measured_currents[Power.A200_RIGHT_DRIVER_CURRENT] + \ + msg.measured_currents[Power.A200_MCU_AND_USER_PORT_CURRENT] + self.battery.update(voltage, current) + case Platform.W200: + voltage = msg.measured_voltages[Power.WARTHOG_MEASURED_BATTERY] + current = msg.measured_currents[Power.WARTHOG_TOTAL_CURRENT] + self.battery.update(voltage, current) + + self.battery.stamp(self.get_clock().now().to_msg()) + self.battery_state_pub.publish(self.battery.msg) + + +def main(): + setup_path = BaseGenerator.get_args() + rclpy.init() + + bsp = BatteryStatePublisher(setup_path) + + rclpy.spin(bsp) + + bsp.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() From 3f8b855e95253177d220517a1eeffce0b4652744 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Thu, 12 Oct 2023 14:57:24 -0400 Subject: [PATCH 08/41] rolling average --- .../battery_state_publisher/battery.py | 96 +++++++++---------- .../battery_state_publisher | 26 +---- 2 files changed, 52 insertions(+), 70 deletions(-) diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py index 9e9f58c..d370f26 100644 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py @@ -32,72 +32,70 @@ # modification, is not permitted without the express permission # of Clearpath Robotics. -from builtin_interfaces.msg import Time +from clearpath_platform_msgs.msg import Power from sensor_msgs.msg import BatteryState -from enum import Enum class Battery: - class Type(Enum): - HE2613 = 0 - - class Calculation(Enum): - LINEAR_INTERPOLATION = 0 - BMS = 1 - - # Lookup table - LUT = { - Type.HE2613: [ - [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], - ] - } - - def __init__(self, battery_type: Type) -> None: - self.battery_type = battery_type + def __init__(self, rolling_average_period=30) -> None: self._msg = BatteryState() - self._msg.header.frame_id = 'battery_link' - - match self.battery_type: - case Battery.Type.HE2613: - self.calculation = Battery.Calculation.LINEAR_INTERPOLATION - self.lut = Battery.LUT[Battery.Type.HE2613] - self._msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LION + self._msg.header.frame_id = "battery_link" + self._msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_UNKNOWN + self._readings: list[Power] = [] + self.rolling_average_period = rolling_average_period @property def msg(self) -> BatteryState: return self._msg - def update(self, voltage: float, current: float): - self._msg.voltage = voltage - self._msg.current = current - match self.calculation: - case Battery.Calculation.LINEAR_INTERPOLATION: - self._msg.percentage = self.linear_extrapolation(self.lut, voltage) - case Battery.Calculation.BMS: - pass - - def stamp(self, stamp: Time): - self._msg.header.stamp = stamp + 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) - def linear_extrapolation(self, lut: list[list], v: float) -> float: + 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 (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] + + +class HE2613(Battery): + 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], + ] + + def __init__(self) -> None: + super().__init__() + self._msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LION + + def update(self, power_msg: Power): + super().update(power_msg) + self._msg.voltage = power_msg.measured_voltages[Power.JACKAL_MEASURED_BATTERY] + self._msg.current = power_msg.measured_currents[Power.JACKAL_TOTAL_CURRENT] + + avg_voltage = 0 + for reading in self._readings: + avg_voltage += reading.measured_voltages[Power.JACKAL_MEASURED_BATTERY] + avg_voltage /= len(self._readings) + self._msg.percentage = self.linear_interpolation(self.LUT, avg_voltage) diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher index 0364cc8..927d9b7 100755 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher @@ -45,7 +45,7 @@ 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_publisher.battery import Battery +from clearpath_diagnostics.battery_state_publisher.battery import HE2613 class BatteryStatePublisher(Node): @@ -75,30 +75,14 @@ class BatteryStatePublisher(Node): match self.platform: case Platform.J100: - self.battery = Battery(Battery.Type.HE2613) + self.battery = HE2613() case Platform.A200: - self.battery = Battery(Battery.Type.HE2613) + self.battery = HE2613() case Platform.W200: - self.battery = Battery(Battery.Type.HE2613) + self.battery = HE2613() def power_status_callback(self, msg: Power): - match self.platform: - case Platform.J100: - voltage = msg.measured_voltages[Power.JACKAL_MEASURED_BATTERY] - current = msg.measured_currents[Power.JACKAL_TOTAL_CURRENT] - self.battery.update(voltage, current) - case Platform.A200: - voltage = msg.measured_voltages[Power.A200_BATTERY_VOLTAGE] - current = msg.measured_currents[Power.A200_LEFT_DRIVER_CURRENT] + \ - msg.measured_currents[Power.A200_RIGHT_DRIVER_CURRENT] + \ - msg.measured_currents[Power.A200_MCU_AND_USER_PORT_CURRENT] - self.battery.update(voltage, current) - case Platform.W200: - voltage = msg.measured_voltages[Power.WARTHOG_MEASURED_BATTERY] - current = msg.measured_currents[Power.WARTHOG_TOTAL_CURRENT] - self.battery.update(voltage, current) - - self.battery.stamp(self.get_clock().now().to_msg()) + self.battery.update(msg) self.battery_state_pub.publish(self.battery.msg) From 28e5874d167fa2ed27312ace3005bd42cae88bd8 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Fri, 13 Oct 2023 11:15:53 -0400 Subject: [PATCH 09/41] Battery types and configurations --- .../battery_state_publisher/battery.py | 215 ++++++++++++++++-- .../battery_state_publisher | 12 +- 2 files changed, 203 insertions(+), 24 deletions(-) diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py index d370f26..f116e0f 100644 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py @@ -35,14 +35,78 @@ from clearpath_platform_msgs.msg import Power from sensor_msgs.msg import BatteryState +from clearpath_config.common.types.platform import Platform + +from enum import Enum + + +# Base Battery class Battery: - def __init__(self, rolling_average_period=30) -> None: + """ Base Battery class. """ + + class Configuration(): + """Battery configuration. Represents number of battery cells.""" + + S1P1 = 'S1P1' + S2P1 = 'S2P1' + S1P3 = 'S1P3' + S1P4 = 'S1P4' + S4P1 = 'S4P1' + S4P3 = 'S4P3' + + CELL_COUNT = { + S1P1: 1, + S2P1: 2, + S1P3: 3, + S1P4: 4, + S4P1: 4, + S4P3: 12, + } + + # To be defined in child class + VALID_CONFIGURATIONS = [] + SYSTEM_CAPACITY = {} + + def __init__( + self, + platform: Platform, + configuration: Configuration, + rolling_average_period=30, + ) -> None: self._msg = BatteryState() - self._msg.header.frame_id = "battery_link" self._msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_UNKNOWN self._readings: list[Power] = [] self.rolling_average_period = rolling_average_period + self.platform = platform + self.configuration = configuration + assert self.configuration in self.VALID_CONFIGURATIONS, ( + 'Invalid Configuration\n' + + f'Configuration {self.configuration}\n' + + f'Battery: {self.__class__.__name__}\n' + + f'Platform : {self.platform}' + ) + + # 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.JACKAL_MEASURED_BATTERY + self.power_msg_current_index = Power.JACKAL_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.J100: + self.power_msg_voltage_index = Power.WARTHOG_MEASURED_BATTERY + self.power_msg_current_index = Power.WARTHOG_TOTAL_CURRENT + + # System capacity + self._msg.capacity = self._msg.design_capacity = self.SYSTEM_CAPACITY[self.configuration] @property def msg(self) -> BatteryState: @@ -56,6 +120,19 @@ def update(self, power_msg: Power): 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] + def linear_interpolation(self, lut: list[list], v: float) -> float: # Check if voltage is below minimum value if v <= lut[0][0]: @@ -64,13 +141,72 @@ def linear_interpolation(self, lut: list[list], v: float) -> float: 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] + lut[i][0] - lut[i - 1][0] + ) + lut[i - 1][1] # Return maximum value return lut[len(lut) - 1][1] -class HE2613(Battery): +# Battery Types + +class LiION(Battery): + """Base Lithium ION battery.""" + LUT = [] + + def __init__( + self, + platform: Platform, + configuration: Battery.Configuration, + rolling_average_period=30, + ) -> None: + super().__init__(platform, configuration, rolling_average_period) + self._msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LION + + def update(self, power_msg: Power): + super().update(power_msg) + + # 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 + self._msg.cell_voltage = [self._msg.voltage] * Battery.Configuration.CELL_COUNT[ + self.configuration] + + # Power supply status + if self._msg.percentage == 1.0: + self._msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_FULL + + +class SLA(Battery): + """Base Lead Acid battery.""" + def update(self, power_msg: Power): + super().update(power_msg) + # Calculate SLA SoC + pass + + +class LiFEPO4(Battery): + """Base LiFEPO4 battery.""" + def __init__(self, + platform: Platform, + configuration: Battery.Configuration, + rolling_average_period=30) -> None: + super().__init__(platform, configuration, rolling_average_period) + self._msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LIFE + + def update(self, power_msg: Power): + super().update(power_msg) + # Get BMS data + pass + + +# Batteries + +class HE2613(LiION): LUT = [ [21.00, 0.0], [21.84, 0.1], @@ -85,17 +221,64 @@ class HE2613(Battery): [29.40, 1.0], ] - def __init__(self) -> None: - super().__init__() - self._msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LION + VALID_CONFIGURATIONS = [ + Battery.Configuration.S1P1, + Battery.Configuration.S1P3, + Battery.Configuration.S1P4, + ] - def update(self, power_msg: Power): - super().update(power_msg) - self._msg.voltage = power_msg.measured_voltages[Power.JACKAL_MEASURED_BATTERY] - self._msg.current = power_msg.measured_currents[Power.JACKAL_TOTAL_CURRENT] + SYSTEM_CAPACITY = { + Battery.Configuration.S1P1: 12.8, + Battery.Configuration.S1P3: 38.4, + Battery.Configuration.S1P4: 51.2, + } - avg_voltage = 0 - for reading in self._readings: - avg_voltage += reading.measured_voltages[Power.JACKAL_MEASURED_BATTERY] - avg_voltage /= len(self._readings) - self._msg.percentage = self.linear_interpolation(self.LUT, avg_voltage) + +class ES20_12C(SLA): + VALID_CONFIGURATIONS = [ + Battery.Configuration.S2P1 + ] + + SYSTEM_CAPACITY = { + Battery.Configuration.S2P1: 20.0, + } + + +class U1_35(SLA): + VALID_CONFIGURATIONS = [ + Battery.Configuration.S4P3 + ] + + SYSTEM_CAPACITY = { + Battery.Configuration.S4P3: 105, + } + + +class NEC_ALM12V35(LiFEPO4): + VALID_CONFIGURATIONS = [ + Battery.Configuration.S4P3 + ] + + SYSTEM_CAPACITY = { + Battery.Configuration.S4P3: 105, + } + + +class VALENCE_U24_12XP(LiFEPO4): + VALID_CONFIGURATIONS = [ + Battery.Configuration.S4P1 + ] + + SYSTEM_CAPACITY = { + Battery.Configuration.S4P1: 118, + } + + +class VALENCE_U27_12XP(LiFEPO4): + VALID_CONFIGURATIONS = [ + Battery.Configuration.S4P1 + ] + + SYSTEM_CAPACITY = { + Battery.Configuration.S4P1: 144, + } diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher index 927d9b7..52280e3 100755 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher @@ -45,7 +45,7 @@ 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_publisher.battery import HE2613 +from clearpath_diagnostics.battery_state_publisher.battery import Battery, HE2613 class BatteryStatePublisher(Node): @@ -73,13 +73,9 @@ class BatteryStatePublisher(Node): qos_profile_sensor_data ) - match self.platform: - case Platform.J100: - self.battery = HE2613() - case Platform.A200: - self.battery = HE2613() - case Platform.W200: - self.battery = HE2613() + self.battery = HE2613(Platform.A200, Battery.Configuration.S1P4) + # self.battery = HE2613(Platform.A200, Battery.Configuration.S1P3) + # self.battery = HE2613(Platform.J100, Battery.Configuration.S1P1) def power_status_callback(self, msg: Power): self.battery.update(msg) From 32ec65b0132ed4ad3175f12bddc8f32693b549f4 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Fri, 13 Oct 2023 13:17:53 -0400 Subject: [PATCH 10/41] Added LUT for SLA --- .../battery_state_publisher/battery.py | 65 ++++++++++++++----- .../battery_state_publisher | 5 +- 2 files changed, 50 insertions(+), 20 deletions(-) diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py index f116e0f..94e27b3 100644 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py @@ -37,7 +37,7 @@ from clearpath_config.common.types.platform import Platform -from enum import Enum +from math import nan # Base Battery @@ -46,7 +46,7 @@ class Battery: """ Base Battery class. """ class Configuration(): - """Battery configuration. Represents number of battery cells.""" + """Battery configuration. Represents number of battery cells in series and parallel.""" S1P1 = 'S1P1' S2P1 = 'S2P1' @@ -67,6 +67,7 @@ class Configuration(): # To be defined in child class VALID_CONFIGURATIONS = [] SYSTEM_CAPACITY = {} + LUT = [] def __init__( self, @@ -76,6 +77,8 @@ def __init__( ) -> None: self._msg = BatteryState() self._msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_UNKNOWN + self._msg.present = True + self._msg.temperature = nan self._readings: list[Power] = [] self.rolling_average_period = rolling_average_period self.platform = platform @@ -132,6 +135,8 @@ def update(self, power_msg: Power): self._msg.current = 0.0 for i in self.power_msg_current_index: self._msg.current += power_msg.measured_currents[i] + # Cells + self.update_cells() def linear_interpolation(self, lut: list[list], v: float) -> float: # Check if voltage is below minimum value @@ -147,6 +152,25 @@ def linear_interpolation(self, lut: list[list], v: float) -> float: # 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 + + # Power supply status + if self._msg.percentage == 1.0: + self._msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_FULL + + def update_cells(self): + self._msg.cell_voltage = [nan] * Battery.Configuration.CELL_COUNT[ + self.configuration] + self._msg.cell_temperature = [nan] * Battery.Configuration.CELL_COUNT[ + self.configuration] + # Battery Types @@ -165,28 +189,33 @@ def __init__( def update(self, power_msg: Power): super().update(power_msg) - - # 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 - self._msg.cell_voltage = [self._msg.voltage] * Battery.Configuration.CELL_COUNT[ - self.configuration] - - # Power supply status - if self._msg.percentage == 1.0: - self._msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_FULL + self.update_from_lut() class SLA(Battery): """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 update(self, power_msg: Power): super().update(power_msg) - # Calculate SLA SoC - pass + self.update_from_lut() class LiFEPO4(Battery): diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher index 52280e3..f60664f 100755 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher @@ -45,7 +45,7 @@ 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_publisher.battery import Battery, HE2613 +from clearpath_diagnostics.battery_state_publisher.battery import Battery, HE2613, ES20_12C class BatteryStatePublisher(Node): @@ -73,9 +73,10 @@ class BatteryStatePublisher(Node): qos_profile_sensor_data ) - self.battery = HE2613(Platform.A200, Battery.Configuration.S1P4) + #self.battery = HE2613(Platform.A200, Battery.Configuration.S1P4) # self.battery = HE2613(Platform.A200, Battery.Configuration.S1P3) # self.battery = HE2613(Platform.J100, Battery.Configuration.S1P1) + self.battery = ES20_12C(Platform.A200, Battery.Configuration.S2P1) def power_status_callback(self, msg: Power): self.battery.update(msg) From 94ee7ca26d69c09437c6bdd56e570f299c56c001 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Fri, 13 Oct 2023 15:28:01 -0400 Subject: [PATCH 11/41] Properties, capacity, voltage Create pub/sub only for LiION and SLA --- .../battery_state_publisher/battery.py | 114 +++++++++--------- .../battery_state_publisher | 50 +++++--- 2 files changed, 93 insertions(+), 71 deletions(-) diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py index 94e27b3..cab6e40 100644 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py @@ -48,25 +48,17 @@ class Battery: class Configuration(): """Battery configuration. Represents number of battery cells in series and parallel.""" - S1P1 = 'S1P1' - S2P1 = 'S2P1' - S1P3 = 'S1P3' - S1P4 = 'S1P4' - S4P1 = 'S4P1' - S4P3 = 'S4P3' - - CELL_COUNT = { - S1P1: 1, - S2P1: 2, - S1P3: 3, - S1P4: 4, - S4P1: 4, - S4P3: 12, - } + S1P1 = (1, 1) + S2P1 = (2, 1) + S1P3 = (1, 3) + S1P4 = (1, 4) + S4P1 = (4, 1) + S4P3 = (4, 3) # To be defined in child class + CAPACITY = 0.0 + VOLTAGE = 0.0 VALID_CONFIGURATIONS = [] - SYSTEM_CAPACITY = {} LUT = [] def __init__( @@ -75,25 +67,25 @@ def __init__( configuration: Configuration, rolling_average_period=30, ) -> None: + self._rolling_average_period = rolling_average_period + self._platform = platform + self._configuration = configuration self._msg = BatteryState() self._msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_UNKNOWN self._msg.present = True self._msg.temperature = nan self._readings: list[Power] = [] - self.rolling_average_period = rolling_average_period - self.platform = platform - self.configuration = configuration - assert self.configuration in self.VALID_CONFIGURATIONS, ( + assert self._configuration in self.VALID_CONFIGURATIONS, ( 'Invalid Configuration\n' + - f'Configuration {self.configuration}\n' + + f'Configuration {self._configuration}\n' + f'Battery: {self.__class__.__name__}\n' + - f'Platform : {self.platform}' + f'Platform : {self._platform}' ) # 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: + match self._platform: case Platform.J100: self.power_msg_voltage_index = Power.JACKAL_MEASURED_BATTERY self.power_msg_current_index = Power.JACKAL_TOTAL_CURRENT @@ -109,18 +101,38 @@ def __init__( self.power_msg_current_index = Power.WARTHOG_TOTAL_CURRENT # System capacity - self._msg.capacity = self._msg.design_capacity = self.SYSTEM_CAPACITY[self.configuration] + 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: + if len(self._readings) > self._rolling_average_period: self._readings.pop(0) # Set battery voltage @@ -166,10 +178,8 @@ def update_from_lut(self): self._msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_FULL def update_cells(self): - self._msg.cell_voltage = [nan] * Battery.Configuration.CELL_COUNT[ - self.configuration] - self._msg.cell_temperature = [nan] * Battery.Configuration.CELL_COUNT[ - self.configuration] + self._msg.cell_voltage = [self._msg.voltage / self.series] * self.cell_count + self._msg.cell_temperature = [nan] * self.cell_count # Battery Types @@ -213,6 +223,14 @@ class SLA(Battery): [12.7, 1.00], ] + def __init__(self, + platform: Platform, + configuration: Battery.Configuration, + 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() @@ -236,6 +254,8 @@ def update(self, power_msg: Power): # Batteries class HE2613(LiION): + CAPACITY = 12.8 + VOLTAGE = 25.9 LUT = [ [21.00, 0.0], [21.84, 0.1], @@ -256,58 +276,42 @@ class HE2613(LiION): Battery.Configuration.S1P4, ] - SYSTEM_CAPACITY = { - Battery.Configuration.S1P1: 12.8, - Battery.Configuration.S1P3: 38.4, - Battery.Configuration.S1P4: 51.2, - } - class ES20_12C(SLA): + CAPACITY = 20.0 + VOLTAGE = 12.0 VALID_CONFIGURATIONS = [ Battery.Configuration.S2P1 ] - SYSTEM_CAPACITY = { - Battery.Configuration.S2P1: 20.0, - } - class U1_35(SLA): + CAPACITY = 35.0 + VOLTAGE = 12.0 VALID_CONFIGURATIONS = [ Battery.Configuration.S4P3 ] - SYSTEM_CAPACITY = { - Battery.Configuration.S4P3: 105, - } - class NEC_ALM12V35(LiFEPO4): + CAPACITY = 35.0 + VOLTAGE = 13.2 VALID_CONFIGURATIONS = [ Battery.Configuration.S4P3 ] - SYSTEM_CAPACITY = { - Battery.Configuration.S4P3: 105, - } - class VALENCE_U24_12XP(LiFEPO4): + CAPACITY = 118.0 + VOLTAGE = 12.8 VALID_CONFIGURATIONS = [ Battery.Configuration.S4P1 ] - SYSTEM_CAPACITY = { - Battery.Configuration.S4P1: 118, - } - class VALENCE_U27_12XP(LiFEPO4): + CAPACITY = 144.0 + VOLTAGE = 12.8 VALID_CONFIGURATIONS = [ Battery.Configuration.S4P1 ] - - SYSTEM_CAPACITY = { - Battery.Configuration.S4P1: 144, - } diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher index f60664f..d305377 100755 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher @@ -45,7 +45,18 @@ 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_publisher.battery import Battery, HE2613, ES20_12C +from clearpath_diagnostics.battery_state_publisher.battery import ( + Battery, + LiION, + SLA, + LiFEPO4, + HE2613, + ES20_12C, + U1_35, + NEC_ALM12V35, + VALENCE_U24_12XP, + VALENCE_U27_12XP +) class BatteryStatePublisher(Node): @@ -61,22 +72,29 @@ class BatteryStatePublisher(Node): self.clearpath_config = ClearpathConfig(self.config_path) self.platform = self.clearpath_config.platform.get_platform_model() - self.battery_state_pub = self.create_publisher( - BatteryState, - 'platform/battery_status', - qos_profile_sensor_data) - - self.power_status_sub = self.create_subscription( - Power, - 'platform/mcu/status/power', - self.power_status_callback, - qos_profile_sensor_data - ) - - #self.battery = HE2613(Platform.A200, Battery.Configuration.S1P4) - # self.battery = HE2613(Platform.A200, Battery.Configuration.S1P3) - # self.battery = HE2613(Platform.J100, Battery.Configuration.S1P1) self.battery = ES20_12C(Platform.A200, Battery.Configuration.S2P1) + #self.battery = HE2613(Platform.A200, Battery.Configuration.S1P3) + #self.battery = HE2613(Platform.A200, Battery.Configuration.S1P4) + #self.battery = HE2613(Platform.J100, Battery.Configuration.S1P1) + #self.battery = U1_35(Platform.W200, Battery.Configuration.S4P3) + #self.battery = NEC_ALM12V35(Platform.W200, Battery.Configuration.S4P3) + #self.battery = VALENCE_U24_12XP(Platform.W200, Battery.Configuration.S4P1) + #self.battery = VALENCE_U27_12XP(Platform.W200, Battery.Configuration.S4P1) + + match self.battery: + case LiION() | SLA(): + 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) + case LiFEPO4(): + pass def power_status_callback(self, msg: Power): self.battery.update(msg) From 2f6ab187fb73159ef58502c8a439d665058651b1 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Fri, 13 Oct 2023 16:14:30 -0400 Subject: [PATCH 12/41] Renamed to battery_state_estimator Added to robot generator --- clearpath_diagnostics/CMakeLists.txt | 2 +- .../__init__.py | 0 .../battery.py | 41 +---------------- .../battery_state_estimator} | 45 +++++++------------ .../launch/generator.py | 32 ++++++++----- 5 files changed, 39 insertions(+), 81 deletions(-) rename clearpath_diagnostics/clearpath_diagnostics/{battery_state_publisher => battery_state}/__init__.py (100%) rename clearpath_diagnostics/clearpath_diagnostics/{battery_state_publisher => battery_state}/battery.py (90%) rename clearpath_diagnostics/clearpath_diagnostics/{battery_state_publisher/battery_state_publisher => battery_state/battery_state_estimator} (75%) diff --git a/clearpath_diagnostics/CMakeLists.txt b/clearpath_diagnostics/CMakeLists.txt index b029b3f..8fceec0 100644 --- a/clearpath_diagnostics/CMakeLists.txt +++ b/clearpath_diagnostics/CMakeLists.txt @@ -13,7 +13,7 @@ ament_python_install_package(${PROJECT_NAME}) install( PROGRAMS ${PROJECT_NAME}/diagnostics_updater - ${PROJECT_NAME}/battery_state_publisher/battery_state_publisher + ${PROJECT_NAME}/battery_state/battery_state_estimator DESTINATION lib/${PROJECT_NAME} ) diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/__init__.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state/__init__.py similarity index 100% rename from clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/__init__.py rename to clearpath_diagnostics/clearpath_diagnostics/battery_state/__init__.py diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py similarity index 90% rename from clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py rename to clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py index cab6e40..0fa2812 100644 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery.py +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py @@ -196,6 +196,8 @@ def __init__( ) -> 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) @@ -236,21 +238,6 @@ def update(self, power_msg: Power): self.update_from_lut() -class LiFEPO4(Battery): - """Base LiFEPO4 battery.""" - def __init__(self, - platform: Platform, - configuration: Battery.Configuration, - rolling_average_period=30) -> None: - super().__init__(platform, configuration, rolling_average_period) - self._msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LIFE - - def update(self, power_msg: Power): - super().update(power_msg) - # Get BMS data - pass - - # Batteries class HE2613(LiION): @@ -291,27 +278,3 @@ class U1_35(SLA): VALID_CONFIGURATIONS = [ Battery.Configuration.S4P3 ] - - -class NEC_ALM12V35(LiFEPO4): - CAPACITY = 35.0 - VOLTAGE = 13.2 - VALID_CONFIGURATIONS = [ - Battery.Configuration.S4P3 - ] - - -class VALENCE_U24_12XP(LiFEPO4): - CAPACITY = 118.0 - VOLTAGE = 12.8 - VALID_CONFIGURATIONS = [ - Battery.Configuration.S4P1 - ] - - -class VALENCE_U27_12XP(LiFEPO4): - CAPACITY = 144.0 - VOLTAGE = 12.8 - VALID_CONFIGURATIONS = [ - Battery.Configuration.S4P1 - ] diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_estimator similarity index 75% rename from clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher rename to clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_estimator index d305377..82a5e0d 100755 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state_publisher/battery_state_publisher +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_estimator @@ -45,23 +45,17 @@ 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_publisher.battery import ( +from clearpath_diagnostics.battery_state.battery import ( Battery, - LiION, - SLA, - LiFEPO4, HE2613, ES20_12C, - U1_35, - NEC_ALM12V35, - VALENCE_U24_12XP, - VALENCE_U27_12XP + U1_35 ) -class BatteryStatePublisher(Node): +class BatteryStateEstimator(Node): def __init__(self, setup_path='/etc/clearpath'): - super().__init__('battery_state_publisher') + super().__init__('battery_state_estimator') self.setup_path = setup_path # Define paths @@ -77,24 +71,17 @@ class BatteryStatePublisher(Node): #self.battery = HE2613(Platform.A200, Battery.Configuration.S1P4) #self.battery = HE2613(Platform.J100, Battery.Configuration.S1P1) #self.battery = U1_35(Platform.W200, Battery.Configuration.S4P3) - #self.battery = NEC_ALM12V35(Platform.W200, Battery.Configuration.S4P3) - #self.battery = VALENCE_U24_12XP(Platform.W200, Battery.Configuration.S4P1) - #self.battery = VALENCE_U27_12XP(Platform.W200, Battery.Configuration.S4P1) - - match self.battery: - case LiION() | SLA(): - 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) - case LiFEPO4(): - pass + + 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) @@ -105,7 +92,7 @@ def main(): setup_path = BaseGenerator.get_args() rclpy.init() - bsp = BatteryStatePublisher(setup_path) + bsp = BatteryStateEstimator(setup_path) rclpy.spin(bsp) diff --git a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py index 45a6b37..942d39d 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py +++ b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py @@ -132,6 +132,15 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: 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] + ) + # Static transform from /odom to odom # See https://github.com/ros-controls/ros2_controllers/pull/533 self.tf_namespaced_odom_publisher = LaunchFile.get_static_tf_node( @@ -152,25 +161,24 @@ 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.platform_components = { - Platform.J100: [ + Platform.J100: common_platform_components + [ self.imu_0_filter_node, self.imu_0_filter_config, self.configure_mcu, self.j100_uros_node, - self.nmea_driver_node, - self.wireless_watcher_node, - self.diagnostics_launch - ], - Platform.A200: [ - self.wireless_watcher_node, - self.diagnostics_launch + self.nmea_driver_node ], - Platform.W200: [ + Platform.A200: common_platform_components, + Platform.W200: common_platform_components + [ self.w200_uros_node, - self.configure_mcu, - self.wireless_watcher_node, - self.diagnostics_launch + self.configure_mcu ] } From 564ed5721decb61f59219bda43cf09b2c6134204 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Mon, 16 Oct 2023 17:31:21 -0400 Subject: [PATCH 13/41] Initial battery control node --- .../battery_state/battery_state_control | 110 ++++++++++++++++++ .../battery_state/battery_state_estimator | 1 - 2 files changed, 110 insertions(+), 1 deletion(-) create mode 100755 clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control 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..5a5ecb1 --- /dev/null +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control @@ -0,0 +1,110 @@ +#!/usr/bin/env python3 + +# Software License Agreement (BSD) +# +# @author Roni Kreinin +# @copyright (c) 2023, Clearpath Robotics, Inc., All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Clearpath Robotics nor the names of its contributors +# may be used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# Redistribution and use in source and binary forms, with or without +# modification, is not permitted without the express permission +# of Clearpath Robotics. + +import os + +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data + +from clearpath_platform_msgs.msg import HMI +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 + + +class BatteryStateControl(Node): + 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: + self.hmi_battery_pub = self.create_publisher( + HMI, + 'platform/mcu/_hmi', + qos_profile_sensor_data) + + self.hmi_msg = HMI() + + def hmi_control(self, percentage: float): + if percentage >= 0.5: + self.hmi_msg.battery_color = HMI.GREEN + self.hmi_msg.battery_pattern = HMI.ON + elif percentage >= 0.2: + self.hmi_msg.battery_color = HMI.YELLOW + self.hmi_msg.battery_pattern = HMI.ON + elif percentage >= 0.1: + self.hmi_msg.battery_color = HMI.RED + self.hmi_msg.battery_pattern = HMI.ON + else: + self.hmi_msg.battery_color = HMI.RED + self.hmi_msg.battery_pattern = HMI.BLINK + self.hmi_battery_pub.publish(self.hmi_msg) + + def battery_state_callback(self, msg: BatteryState): + match self.platform: + case Platform.J100: + 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 index 82a5e0d..0c0c721 100755 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_estimator +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_estimator @@ -60,7 +60,6 @@ class BatteryStateEstimator(Node): # Define paths self.config_path = os.path.join(self.setup_path, 'robot.yaml') - assert os.path.exists(self.config_path) # Parse YAML into config self.clearpath_config = ClearpathConfig(self.config_path) From 276cbb8b7376fe2f16db8de4dcd12a2b610cc1d5 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Mon, 23 Oct 2023 14:40:28 -0400 Subject: [PATCH 14/41] Removed HMI msg, encode Uint8 instead --- clearpath_diagnostics/CMakeLists.txt | 1 + .../battery_state/battery_state_control | 46 ++++++++++++++----- 2 files changed, 36 insertions(+), 11 deletions(-) diff --git a/clearpath_diagnostics/CMakeLists.txt b/clearpath_diagnostics/CMakeLists.txt index 8fceec0..c47ea8d 100644 --- a/clearpath_diagnostics/CMakeLists.txt +++ b/clearpath_diagnostics/CMakeLists.txt @@ -14,6 +14,7 @@ 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} ) diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control index 5a5ecb1..7c51651 100755 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control @@ -38,15 +38,28 @@ import rclpy from rclpy.node import Node from rclpy.qos import qos_profile_sensor_data -from clearpath_platform_msgs.msg import HMI +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 @@ -67,25 +80,36 @@ class BatteryStateControl(Node): match self.platform: case Platform.J100: self.hmi_battery_pub = self.create_publisher( - HMI, + UInt8, 'platform/mcu/_hmi', qos_profile_sensor_data) - self.hmi_msg = HMI() + 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.hmi_msg.battery_color = HMI.GREEN - self.hmi_msg.battery_pattern = HMI.ON + self.encode_hmi_msg( + BatteryStateControl.HmiColour.GREEN, + BatteryStateControl.HmiPattern.ON) elif percentage >= 0.2: - self.hmi_msg.battery_color = HMI.YELLOW - self.hmi_msg.battery_pattern = HMI.ON + self.encode_hmi_msg( + BatteryStateControl.HmiColour.YELLOW, + BatteryStateControl.HmiPattern.ON) elif percentage >= 0.1: - self.hmi_msg.battery_color = HMI.RED - self.hmi_msg.battery_pattern = HMI.ON + self.encode_hmi_msg( + BatteryStateControl.HmiColour.RED, + BatteryStateControl.HmiPattern.ON) else: - self.hmi_msg.battery_color = HMI.RED - self.hmi_msg.battery_pattern = HMI.BLINK + 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): From 3f9de90eff3882c70e3a8defc27b13bf47b3d315 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Tue, 24 Oct 2023 09:26:13 -0400 Subject: [PATCH 15/41] Use battery model and configuration from clearpath_config --- .../battery_state/battery.py | 460 +++++++++--------- .../battery_state/battery_state_estimator | 18 +- 2 files changed, 235 insertions(+), 243 deletions(-) diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py index 0fa2812..f2acd63 100644 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py @@ -36,6 +36,7 @@ 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 @@ -43,238 +44,233 @@ # Base Battery class Battery: - """ Base Battery class. """ + class BaseBattery: + """ Base Battery class. """ - class Configuration(): """Battery configuration. Represents number of battery cells in series and parallel.""" - - S1P1 = (1, 1) - S2P1 = (2, 1) - S1P3 = (1, 3) - S1P4 = (1, 4) - S4P1 = (4, 1) - S4P3 = (4, 3) - - # To be defined in child class - CAPACITY = 0.0 - VOLTAGE = 0.0 - VALID_CONFIGURATIONS = [] - LUT = [] - - def __init__( - self, - platform: Platform, - configuration: Configuration, - rolling_average_period=30, - ) -> None: - self._rolling_average_period = rolling_average_period - self._platform = platform - self._configuration = configuration - self._msg = BatteryState() - self._msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_UNKNOWN - self._msg.present = True - self._msg.temperature = nan - self._readings: list[Power] = [] - assert self._configuration in self.VALID_CONFIGURATIONS, ( - 'Invalid Configuration\n' + - f'Configuration {self._configuration}\n' + - f'Battery: {self.__class__.__name__}\n' + - f'Platform : {self._platform}' - ) - - # 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.JACKAL_MEASURED_BATTERY - self.power_msg_current_index = Power.JACKAL_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, + CONFIGURATIONS = { + BatteryConfig.S1P1: (1, 1), + BatteryConfig.S2P1: (2, 1), + BatteryConfig.S1P3: (1, 3), + BatteryConfig.S1P4: (1, 4), + 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.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.J100: + self.power_msg_voltage_index = Power.W200_MEASURED_BATTERY + self.power_msg_current_index = Power.W200_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 ] - case Platform.J100: - self.power_msg_voltage_index = Power.WARTHOG_MEASURED_BATTERY - self.power_msg_current_index = Power.WARTHOG_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] - # 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 - - # Power supply status - if self._msg.percentage == 1.0: - self._msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_FULL - - 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(Battery): - """Base Lithium ION battery.""" - LUT = [] - - def __init__( - self, - platform: Platform, - configuration: Battery.Configuration, - 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(Battery): - """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: Battery.Configuration, - 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], - ] - - VALID_CONFIGURATIONS = [ - Battery.Configuration.S1P1, - Battery.Configuration.S1P3, - Battery.Configuration.S1P4, - ] - - -class ES20_12C(SLA): - CAPACITY = 20.0 - VOLTAGE = 12.0 - VALID_CONFIGURATIONS = [ - Battery.Configuration.S2P1 - ] - - -class U1_35(SLA): - CAPACITY = 35.0 - VOLTAGE = 12.0 - VALID_CONFIGURATIONS = [ - Battery.Configuration.S4P3 - ] + 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] + # 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 + + # Power supply status + if self._msg.percentage == 1.0: + self._msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_FULL + + 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 + + # Match battery name to class + BATTERIES = { + BatteryConfig.HE2613: HE2613, + BatteryConfig.ES20_12C: ES20_12C, + BatteryConfig.U1_35: U1_35 + } + + def __new__(cls, + battery: str, + platform: Platform, + configuration: str, + rolling_average_period=30) -> BaseBattery: + print(battery) + print(platform) + print(configuration) + return Battery.BATTERIES.setdefault(battery, Battery.BaseBattery)( + platform, configuration, rolling_average_period) diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_estimator b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_estimator index 0c0c721..464f049 100755 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_estimator +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_estimator @@ -45,12 +45,7 @@ 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, - HE2613, - ES20_12C, - U1_35 -) +from clearpath_diagnostics.battery_state.battery import Battery class BatteryStateEstimator(Node): @@ -64,12 +59,13 @@ class BatteryStateEstimator(Node): # 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 = ES20_12C(Platform.A200, Battery.Configuration.S2P1) - #self.battery = HE2613(Platform.A200, Battery.Configuration.S1P3) - #self.battery = HE2613(Platform.A200, Battery.Configuration.S1P4) - #self.battery = HE2613(Platform.J100, Battery.Configuration.S1P1) - #self.battery = U1_35(Platform.W200, Battery.Configuration.S4P3) + self.battery = Battery( + battery=self.battery_model, + configuration=self.battery_configuration, + platform=self.platform) self.battery_state_pub = self.create_publisher( BatteryState, From 737b805755cef1bbdd6671f26c23b00934dbbcb0 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Wed, 25 Oct 2023 10:49:59 -0400 Subject: [PATCH 16/41] Removed shebang --- .../clearpath_diagnostics/battery_state/battery.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py index f2acd63..8d432a3 100644 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py @@ -1,5 +1,3 @@ -#!/usr/bin/env python3 - # Software License Agreement (BSD) # # @author Roni Kreinin From e8560055996e4aeacfb05a625120d9bab3ca2d86 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Wed, 25 Oct 2023 15:40:58 -0400 Subject: [PATCH 17/41] J100 -> W200 --- .../clearpath_diagnostics/battery_state/battery.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py index 8d432a3..ccd4d4f 100644 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py @@ -89,7 +89,7 @@ def __init__( Power.A200_LEFT_DRIVER_CURRENT, Power.A200_RIGHT_DRIVER_CURRENT, ] - case Platform.J100: + case Platform.W200: self.power_msg_voltage_index = Power.W200_MEASURED_BATTERY self.power_msg_current_index = Power.W200_TOTAL_CURRENT From 200694c429ad3fe3c3cdf509b16be6b4d5e6028f Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Mon, 30 Oct 2023 10:26:19 -0400 Subject: [PATCH 18/41] Launch battery state control --- .../clearpath_generator_robot/launch/generator.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py index 942d39d..7b03e72 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py +++ b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py @@ -141,6 +141,14 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: 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] + ) + # Static transform from /odom to odom # See https://github.com/ros-controls/ros2_controllers/pull/533 self.tf_namespaced_odom_publisher = LaunchFile.get_static_tf_node( @@ -164,7 +172,8 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: common_platform_components = [ self.wireless_watcher_node, self.diagnostics_launch, - self.battery_state_estimator + self.battery_state_estimator, + self.battery_state_control ] self.platform_components = { From 8940e6bcf320e3aa8a5bf505d39347cd3b32d15f Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Tue, 31 Oct 2023 14:22:32 -0400 Subject: [PATCH 19/41] Fixed status message firmware version --- clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater b/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater index 583efc7..6060b6f 100755 --- a/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater +++ b/clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater @@ -118,7 +118,7 @@ class ClearpathDiagnosticUpdater(Node): self.updater.force_update() def status_callback(self, msg: Status): - self.firmware_version = msg.firmware_commit + self.firmware_version = msg.firmware_version self.mcu_temperature = msg.mcu_temperature self.pcb_temperature = msg.pcb_temperature From d0c6fc50382ae4af052dd4d7f70418f15394ce5b Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Wed, 8 Nov 2023 10:20:05 -0500 Subject: [PATCH 20/41] Generate lighting node --- .../clearpath_diagnostics/battery_state/battery.py | 5 ++--- .../clearpath_generator_robot/launch/generator.py | 12 +++++++++++- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py index ccd4d4f..d98dbdf 100644 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py @@ -71,6 +71,8 @@ def __init__( 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] = [] @@ -267,8 +269,5 @@ def __new__(cls, platform: Platform, configuration: str, rolling_average_period=30) -> BaseBattery: - print(battery) - print(platform) - print(configuration) return Battery.BATTERIES.setdefault(battery, Battery.BaseBattery)( platform, configuration, rolling_average_period) diff --git a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py index 7b03e72..832d2b6 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py +++ b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py @@ -149,6 +149,15 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: 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}] + ) + # Static transform from /odom to odom # See https://github.com/ros-controls/ros2_controllers/pull/533 self.tf_namespaced_odom_publisher = LaunchFile.get_static_tf_node( @@ -187,7 +196,8 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: Platform.A200: common_platform_components, Platform.W200: common_platform_components + [ self.w200_uros_node, - self.configure_mcu + self.configure_mcu, + self.lighting_node ] } From e8104005b933f83ded39220a0277502f0b24b098 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Thu, 16 Nov 2023 13:49:38 -0500 Subject: [PATCH 21/41] Generate sevcon traction node --- .../clearpath_generator_robot/launch/generator.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py index 832d2b6..f1a9f01 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py +++ b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py @@ -158,6 +158,14 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: 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 /odom to odom # See https://github.com/ros-controls/ros2_controllers/pull/533 self.tf_namespaced_odom_publisher = LaunchFile.get_static_tf_node( @@ -197,7 +205,8 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: Platform.W200: common_platform_components + [ self.w200_uros_node, self.configure_mcu, - self.lighting_node + self.lighting_node, + self.sevcon_node ] } From 9296aff182f0603fec4300bda1c9b2b9837d4212 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Thu, 16 Nov 2023 13:51:14 -0500 Subject: [PATCH 22/41] sevcon_traction dependency --- clearpath_generator_robot/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/clearpath_generator_robot/package.xml b/clearpath_generator_robot/package.xml index c71748b..ce579ed 100644 --- a/clearpath_generator_robot/package.xml +++ b/clearpath_generator_robot/package.xml @@ -16,6 +16,7 @@ clearpath_sensors imu_filter_madgwick micro_ros_agent + sevcon_traction wireless_watcher ament_lint_auto From ec2f242ddbde2a13eecbd53ea8977466cbed2b4b Mon Sep 17 00:00:00 2001 From: Tony Baltovski Date: Thu, 16 Nov 2023 17:44:48 -0500 Subject: [PATCH 23/41] [clearpath_robot] Added can-utils as dep. --- clearpath_robot/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/clearpath_robot/package.xml b/clearpath_robot/package.xml index 43421c8..9442678 100644 --- a/clearpath_robot/package.xml +++ b/clearpath_robot/package.xml @@ -14,6 +14,7 @@ ament_cmake + can-utils clearpath_generator_robot clearpath_sensors From 45a1b92668fa09e1edf2d4622c179e824c0b69d4 Mon Sep 17 00:00:00 2001 From: Tony Baltovski Date: Thu, 16 Nov 2023 17:45:13 -0500 Subject: [PATCH 24/41] [clearpath_robot] Added udev rule to automatically bring-up CANBUS PCIe card for W200. --- clearpath_robot/debian/udev | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) 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 From f37b08f7d3443bdb4e4fcf91c10cfcf49c88d308 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Tue, 21 Nov 2023 14:24:14 -0500 Subject: [PATCH 25/41] IMU 0 filter for W200 --- .../clearpath_generator_robot/launch/generator.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py index f1a9f01..3957221 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py +++ b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py @@ -203,6 +203,8 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: ], Platform.A200: common_platform_components, Platform.W200: common_platform_components + [ + self.imu_0_filter_node, + self.imu_0_filter_config, self.w200_uros_node, self.configure_mcu, self.lighting_node, From 63bee93ff172e24fe4ddd8f96c150b728c071abc Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Wed, 27 Sep 2023 13:39:13 -0400 Subject: [PATCH 26/41] Run platform and sensor services as user --- clearpath_robot/scripts/install | 16 ++++++++++++++-- .../services/clearpath-platform.service | 1 + .../services/clearpath-sensors.service | 1 + 3 files changed, 16 insertions(+), 2 deletions(-) diff --git a/clearpath_robot/scripts/install b/clearpath_robot/scripts/install index f3feb48..7552ab8 100755 --- a/clearpath_robot/scripts/install +++ b/clearpath_robot/scripts/install @@ -47,7 +47,13 @@ class PlatformProvider(robot_upstart.providers.Generic): get_package_share_directory('clearpath_robot'), 'services/clearpath-platform.service') with open(platform_service_path) as f: - platform_service_contents = f.read() + platform_service_contents: str = '' + platform_service_lines = f.readlines() + for line in platform_service_lines: + # Replace User with username from config + if 'User=' in line: + line = 'User={0}\n'.format(clearpath_config.system.username) + platform_service_contents += line return { "/lib/systemd/system/clearpath-platform.service": { "content": platform_service_contents, @@ -64,7 +70,13 @@ class SensorsProvider(robot_upstart.providers.Generic): get_package_share_directory('clearpath_robot'), 'services/clearpath-sensors.service') with open(sensors_service_path) as f: - sensors_service_contents = f.read() + sensors_service_contents: str = '' + sensors_service_lines = f.readlines() + for line in sensors_service_lines: + # Replace User with username from config + if 'User=' in line: + line = 'User={0}\n'.format(clearpath_config.system.username) + sensors_service_contents += line return { "/lib/systemd/system/clearpath-sensors.service": { "content": sensors_service_contents, diff --git a/clearpath_robot/services/clearpath-platform.service b/clearpath_robot/services/clearpath-platform.service index 100ba54..457a62b 100755 --- a/clearpath_robot/services/clearpath-platform.service +++ b/clearpath_robot/services/clearpath-platform.service @@ -4,6 +4,7 @@ PartOf=clearpath-robot.service After=clearpath-robot.service [Service] +User=administrator Type=simple ExecStart=/usr/sbin/clearpath-platform-start diff --git a/clearpath_robot/services/clearpath-sensors.service b/clearpath_robot/services/clearpath-sensors.service index 39cbb9a..2fe9afa 100755 --- a/clearpath_robot/services/clearpath-sensors.service +++ b/clearpath_robot/services/clearpath-sensors.service @@ -4,6 +4,7 @@ PartOf=clearpath-robot.service After=clearpath-robot.service [Service] +User=administrator Type=simple ExecStart=/usr/sbin/clearpath-sensors-start From 1d38c94deb617280102ea91458287345d6bd955c Mon Sep 17 00:00:00 2001 From: Hilary Luo Date: Thu, 21 Sep 2023 13:59:23 -0400 Subject: [PATCH 27/41] Initial Blackfly addition --- clearpath_sensors/config/flir_blackfly.yaml | 31 ++++++++ .../launch/flir_blackfly.launch.py | 75 +++++++++++++++++++ clearpath_sensors/package.xml | 3 + 3 files changed, 109 insertions(+) create mode 100644 clearpath_sensors/config/flir_blackfly.yaml create mode 100644 clearpath_sensors/launch/flir_blackfly.launch.py diff --git a/clearpath_sensors/config/flir_blackfly.yaml b/clearpath_sensors/config/flir_blackfly.yaml new file mode 100644 index 0000000..12153f5 --- /dev/null +++ b/clearpath_sensors/config/flir_blackfly.yaml @@ -0,0 +1,31 @@ +flir_blackfly: + ros__parameters: + debug: false + compute_brightness: false + adjust_timestamp: true + dump_node_map: false + gain_auto: Continuous + pixel_format: BayerRG8 + exposure_auto: Continuous + # These are useful for GigE cameras + # device_link_throughput_limit: 380000000 + # gev_scps_packet_size: 9000 + # ---- to reduce the sensor width and shift the crop + # image_width: 1408 + # image_height: 1080 + # offset_x: 16 + # offset_y: 0 + frame_rate_auto: Off + frame_rate: 40.0 + frame_rate_enable: true + buffer_queue_size: 1 + trigger_mode: Off + chunk_mode_active: true + chunk_selector_frame_id: FrameID + chunk_enable_frame_id: true + chunk_selector_exposure_time: ExposureTime + chunk_enable_exposure_time: true + chunk_selector_gain: Gain + chunk_enable_gain: true + chunk_selector_timestamp: Timestamp + chunk_enable_timestamp: true} \ No newline at end of file diff --git a/clearpath_sensors/launch/flir_blackfly.launch.py b/clearpath_sensors/launch/flir_blackfly.launch.py new file mode 100644 index 0000000..1e26b61 --- /dev/null +++ b/clearpath_sensors/launch/flir_blackfly.launch.py @@ -0,0 +1,75 @@ +# Software License Agreement (BSD) +# +# @author Roni Kreinin +# @copyright (c) 2023, Clearpath Robotics, Inc., All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Clearpath Robotics nor the names of its contributors +# may be used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + parameters = LaunchConfiguration('parameters') + namespace = LaunchConfiguration('namespace') + param_mapping_file = LaunchConfiguration('param_mapping_file') + + arg_parameters = DeclareLaunchArgument( + 'parameters', + default_value=PathJoinSubstitution([ + FindPackageShare('clearpath_sensors'), + 'config', + 'flir_blackfly.yaml' + ])) + + arg_param_mapping_file = DeclareLaunchArgument( + 'param_mapping_file', + default_value=PathJoinSubstitution([ + FindPackageShare('spinnaker_camera_driver'), 'config', + 'blackfly_s.yaml' + ])) + + arg_namespace = DeclareLaunchArgument( + 'namespace', + default_value='platform/sensors/camera_0') + + blackfly_camera_node = Node( + package='spinnaker_camera_driver', + namespace=namespace, + name='flir_blackfly', + executable='camera_driver_node', + parameters=[parameters, {'parameter_file': param_mapping_file,}], + output='screen', + ) + + + ld = LaunchDescription() + ld.add_action(arg_parameters) + ld.add_action(arg_param_mapping_file) + ld.add_action(arg_namespace) + ld.add_action(blackfly_camera_node) + return ld diff --git a/clearpath_sensors/package.xml b/clearpath_sensors/package.xml index eda6987..24d9b15 100644 --- a/clearpath_sensors/package.xml +++ b/clearpath_sensors/package.xml @@ -15,9 +15,12 @@ ament_cmake + flir_camera_description + flir_camera_msgs microstrain_inertial_driver nmea_navsat_driver realsense2_camera + spinnaker_camera_driver umx_driver urg_node velodyne_driver From a993674f4862f433d44decd4b6cbcb0136733ec9 Mon Sep 17 00:00:00 2001 From: Hilary Luo Date: Mon, 25 Sep 2023 23:48:02 -0400 Subject: [PATCH 28/41] add serial number to yaml --- clearpath_sensors/config/flir_blackfly.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/clearpath_sensors/config/flir_blackfly.yaml b/clearpath_sensors/config/flir_blackfly.yaml index 12153f5..ff6c2ed 100644 --- a/clearpath_sensors/config/flir_blackfly.yaml +++ b/clearpath_sensors/config/flir_blackfly.yaml @@ -1,5 +1,6 @@ flir_blackfly: ros__parameters: + serial_number: '' debug: false compute_brightness: false adjust_timestamp: true From 3589e0eea1b2463acde6557a4e31d08be2932d06 Mon Sep 17 00:00:00 2001 From: Hilary Luo Date: Thu, 28 Sep 2023 14:50:11 -0400 Subject: [PATCH 29/41] Removed errant bracket --- clearpath_sensors/config/flir_blackfly.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clearpath_sensors/config/flir_blackfly.yaml b/clearpath_sensors/config/flir_blackfly.yaml index ff6c2ed..a95fe93 100644 --- a/clearpath_sensors/config/flir_blackfly.yaml +++ b/clearpath_sensors/config/flir_blackfly.yaml @@ -29,4 +29,4 @@ flir_blackfly: chunk_selector_gain: Gain chunk_enable_gain: true chunk_selector_timestamp: Timestamp - chunk_enable_timestamp: true} \ No newline at end of file + chunk_enable_timestamp: true \ No newline at end of file From 3ccf044ec07d4e2a41d01fcf88dc503700d48430 Mon Sep 17 00:00:00 2001 From: Hilary Luo Date: Thu, 28 Sep 2023 15:13:26 -0400 Subject: [PATCH 30/41] Added debayer node --- clearpath_sensors/launch/flir_blackfly.launch.py | 8 ++++++++ clearpath_sensors/package.xml | 1 + 2 files changed, 9 insertions(+) diff --git a/clearpath_sensors/launch/flir_blackfly.launch.py b/clearpath_sensors/launch/flir_blackfly.launch.py index 1e26b61..0a787ad 100644 --- a/clearpath_sensors/launch/flir_blackfly.launch.py +++ b/clearpath_sensors/launch/flir_blackfly.launch.py @@ -66,10 +66,18 @@ def generate_launch_description(): output='screen', ) + debayer_node = Node( + package='image_proc', + namespace=namespace, + name='debayer_node', + plugin='image_proc::DebayerNode', + output='screen', + ) ld = LaunchDescription() ld.add_action(arg_parameters) ld.add_action(arg_param_mapping_file) ld.add_action(arg_namespace) ld.add_action(blackfly_camera_node) + ld.add_action(debayer_node) return ld diff --git a/clearpath_sensors/package.xml b/clearpath_sensors/package.xml index 24d9b15..0352c81 100644 --- a/clearpath_sensors/package.xml +++ b/clearpath_sensors/package.xml @@ -17,6 +17,7 @@ flir_camera_description flir_camera_msgs + image_proc microstrain_inertial_driver nmea_navsat_driver realsense2_camera From 4dc635cf46aca3d66ec9fc766a9f427ed81e9372 Mon Sep 17 00:00:00 2001 From: Hilary Luo Date: Thu, 28 Sep 2023 17:53:57 -0400 Subject: [PATCH 31/41] Correct debayer node and add remapping --- clearpath_sensors/launch/flir_blackfly.launch.py | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/clearpath_sensors/launch/flir_blackfly.launch.py b/clearpath_sensors/launch/flir_blackfly.launch.py index 0a787ad..bd5e6c7 100644 --- a/clearpath_sensors/launch/flir_blackfly.launch.py +++ b/clearpath_sensors/launch/flir_blackfly.launch.py @@ -45,7 +45,7 @@ def generate_launch_description(): 'config', 'flir_blackfly.yaml' ])) - + arg_param_mapping_file = DeclareLaunchArgument( 'param_mapping_file', default_value=PathJoinSubstitution([ @@ -64,14 +64,24 @@ def generate_launch_description(): executable='camera_driver_node', parameters=[parameters, {'parameter_file': param_mapping_file,}], output='screen', + remappings=[ + ('flir_blackfly/camera_info', 'color/camera_info'), + ('flir_blackfly/control', 'control'), + ('flir_blackfly/image_raw', 'image_raw'), + ('flir_blackfly/meta', 'meta'), + ] ) - + debayer_node = Node( package='image_proc', namespace=namespace, name='debayer_node', - plugin='image_proc::DebayerNode', + executable='image_proc', output='screen', + remappings=[ + ('image_color', 'color/image') + ('image_mono', 'mono/image') + ] ) ld = LaunchDescription() From 577bfcfd95ee6617b94037fe5a6128352647a06a Mon Sep 17 00:00:00 2001 From: Hilary Luo Date: Fri, 29 Sep 2023 13:44:37 -0400 Subject: [PATCH 32/41] Missing comma --- clearpath_sensors/launch/flir_blackfly.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/clearpath_sensors/launch/flir_blackfly.launch.py b/clearpath_sensors/launch/flir_blackfly.launch.py index bd5e6c7..670d020 100644 --- a/clearpath_sensors/launch/flir_blackfly.launch.py +++ b/clearpath_sensors/launch/flir_blackfly.launch.py @@ -79,8 +79,8 @@ def generate_launch_description(): executable='image_proc', output='screen', remappings=[ - ('image_color', 'color/image') - ('image_mono', 'mono/image') + ('image_color', 'color/image'), + ('image_mono', 'mono/image'), ] ) From 77e401e7e6b9bb4468b21e962ec4a47731c834e2 Mon Sep 17 00:00:00 2001 From: Luis Camero Date: Mon, 2 Oct 2023 14:52:29 -0400 Subject: [PATCH 33/41] Added image proc as container --- .../launch/flir_blackfly.launch.py | 76 +++++++++++++++---- 1 file changed, 60 insertions(+), 16 deletions(-) diff --git a/clearpath_sensors/launch/flir_blackfly.launch.py b/clearpath_sensors/launch/flir_blackfly.launch.py index 670d020..3447e36 100644 --- a/clearpath_sensors/launch/flir_blackfly.launch.py +++ b/clearpath_sensors/launch/flir_blackfly.launch.py @@ -27,9 +27,10 @@ # POSSIBILITY OF SUCH DAMAGE. from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.conditions import LaunchConfigurationNotEquals from launch.substitutions import LaunchConfiguration, PathJoinSubstitution - -from launch_ros.actions import Node +from launch_ros.actions import Node, ComposableNodeContainer, LoadComposableNodes +from launch_ros.descriptions import ComposableNode from launch_ros.substitutions import FindPackageShare @@ -37,6 +38,7 @@ def generate_launch_description(): parameters = LaunchConfiguration('parameters') namespace = LaunchConfiguration('namespace') param_mapping_file = LaunchConfiguration('param_mapping_file') + rectify = LaunchConfiguration('rectify') arg_parameters = DeclareLaunchArgument( 'parameters', @@ -49,14 +51,20 @@ def generate_launch_description(): arg_param_mapping_file = DeclareLaunchArgument( 'param_mapping_file', default_value=PathJoinSubstitution([ - FindPackageShare('spinnaker_camera_driver'), 'config', - 'blackfly_s.yaml' + FindPackageShare('spinnaker_camera_driver'), + 'config', + 'blackfly_s.yaml' ])) arg_namespace = DeclareLaunchArgument( 'namespace', default_value='platform/sensors/camera_0') + arg_rectify = DeclareLaunchArgument( + 'rectify', + default_value='' + ) + blackfly_camera_node = Node( package='spinnaker_camera_driver', namespace=namespace, @@ -65,29 +73,65 @@ def generate_launch_description(): parameters=[parameters, {'parameter_file': param_mapping_file,}], output='screen', remappings=[ - ('flir_blackfly/camera_info', 'color/camera_info'), + ('flir_blackfly/camera_info', 'camera_info'), ('flir_blackfly/control', 'control'), - ('flir_blackfly/image_raw', 'image_raw'), + ('flir_blackfly/image_raw', 'raw/image'), ('flir_blackfly/meta', 'meta'), ] ) - debayer_node = Node( - package='image_proc', + composable_nodes = [ + ComposableNode( + package='image_proc', + plugin='image_proc::DebayerNode', + name='debayer_node', + namespace=namespace, + remappings=[ + ('image_raw', 'raw/image'), + ('image_color', 'color/image'), + ('image_mono', 'mono/image'), + ] + ), + ComposableNode( + condition=LaunchConfigurationNotEquals('rectify', ''), + package='image_proc', + plugin='image_proc::RectifyNode', + name='rectify_mono_node', + namespace=namespace, + # Remap subscribers and publishers + remappings=[ + ('image', 'mono/image'), + ('image_rect', 'mono/image_rect') + ], + ), + ComposableNode( + condition=LaunchConfigurationNotEquals('rectify', ''), + package='image_proc', + plugin='image_proc::RectifyNode', + name='rectify_color_node', + namespace=namespace, + # Remap subscribers and publishers + remappings=[ + ('image', 'color/image'), + ('image_rect', 'color/image_rect') + ], + ) + ] + + image_processing_container = ComposableNodeContainer( + name='image_processing_node', namespace=namespace, - name='debayer_node', - executable='image_proc', - output='screen', - remappings=[ - ('image_color', 'color/image'), - ('image_mono', 'mono/image'), - ] + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=composable_nodes, + output='screen' ) ld = LaunchDescription() ld.add_action(arg_parameters) ld.add_action(arg_param_mapping_file) ld.add_action(arg_namespace) + ld.add_action(arg_rectify) ld.add_action(blackfly_camera_node) - ld.add_action(debayer_node) + ld.add_action(image_processing_container) return ld From ed02cdc99ada555fb4d6b1a26b2f6525c4b70a03 Mon Sep 17 00:00:00 2001 From: Luis Camero Date: Wed, 4 Oct 2023 10:53:13 -0400 Subject: [PATCH 34/41] Removed 'platform' from default namespace --- clearpath_sensors/launch/flir_blackfly.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clearpath_sensors/launch/flir_blackfly.launch.py b/clearpath_sensors/launch/flir_blackfly.launch.py index 3447e36..23701fd 100644 --- a/clearpath_sensors/launch/flir_blackfly.launch.py +++ b/clearpath_sensors/launch/flir_blackfly.launch.py @@ -58,7 +58,7 @@ def generate_launch_description(): arg_namespace = DeclareLaunchArgument( 'namespace', - default_value='platform/sensors/camera_0') + default_value='sensors/camera_0') arg_rectify = DeclareLaunchArgument( 'rectify', From 27863a8a99d614488c1e6e1122185ba73817883b Mon Sep 17 00:00:00 2001 From: Tony Baltovski Date: Wed, 4 Oct 2023 13:13:05 -0400 Subject: [PATCH 35/41] Changes. --- clearpath_generator_robot/CHANGELOG.rst | 3 +++ clearpath_robot/CHANGELOG.rst | 5 +++++ clearpath_sensors/CHANGELOG.rst | 12 ++++++++++++ 3 files changed, 20 insertions(+) diff --git a/clearpath_generator_robot/CHANGELOG.rst b/clearpath_generator_robot/CHANGELOG.rst index e140a6f..58e2b22 100644 --- a/clearpath_generator_robot/CHANGELOG.rst +++ b/clearpath_generator_robot/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package clearpath_generator_robot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.1.2 (2023-09-27) ------------------ diff --git a/clearpath_robot/CHANGELOG.rst b/clearpath_robot/CHANGELOG.rst index 7197645..ed1f0ef 100644 --- a/clearpath_robot/CHANGELOG.rst +++ b/clearpath_robot/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package clearpath_robot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Run platform and sensor services as user +* Contributors: Roni Kreinin + 0.1.2 (2023-09-27) ------------------ diff --git a/clearpath_sensors/CHANGELOG.rst b/clearpath_sensors/CHANGELOG.rst index 8401a84..f6be69a 100644 --- a/clearpath_sensors/CHANGELOG.rst +++ b/clearpath_sensors/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package clearpath_sensors ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Removed 'platform' from default namespace +* Added image proc as container +* Missing comma +* Correct debayer node and add remapping +* Added debayer node +* Removed errant bracket +* add serial number to yaml +* Initial Blackfly addition +* Contributors: Hilary Luo, Luis Camero + 0.1.2 (2023-09-27) ------------------ * Renamed convert to transform From 48d3edbfb98efa95974f0667fa89a900fce34bfa Mon Sep 17 00:00:00 2001 From: Tony Baltovski Date: Wed, 4 Oct 2023 13:13:39 -0400 Subject: [PATCH 36/41] 0.1.3 --- clearpath_generator_robot/CHANGELOG.rst | 4 ++-- clearpath_generator_robot/package.xml | 2 +- clearpath_robot/CHANGELOG.rst | 4 ++-- clearpath_robot/package.xml | 2 +- clearpath_sensors/CHANGELOG.rst | 4 ++-- clearpath_sensors/package.xml | 2 +- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/clearpath_generator_robot/CHANGELOG.rst b/clearpath_generator_robot/CHANGELOG.rst index 58e2b22..afc73a0 100644 --- a/clearpath_generator_robot/CHANGELOG.rst +++ b/clearpath_generator_robot/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package clearpath_generator_robot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.3 (2023-10-04) +------------------ 0.1.2 (2023-09-27) ------------------ diff --git a/clearpath_generator_robot/package.xml b/clearpath_generator_robot/package.xml index ce579ed..c0de5b7 100644 --- a/clearpath_generator_robot/package.xml +++ b/clearpath_generator_robot/package.xml @@ -2,7 +2,7 @@ clearpath_generator_robot - 0.1.2 + 0.1.3 Clearpath Robot Generator rkreinin BSD diff --git a/clearpath_robot/CHANGELOG.rst b/clearpath_robot/CHANGELOG.rst index ed1f0ef..47efec1 100644 --- a/clearpath_robot/CHANGELOG.rst +++ b/clearpath_robot/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package clearpath_robot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.3 (2023-10-04) +------------------ * Run platform and sensor services as user * Contributors: Roni Kreinin diff --git a/clearpath_robot/package.xml b/clearpath_robot/package.xml index 9442678..91b6305 100644 --- a/clearpath_robot/package.xml +++ b/clearpath_robot/package.xml @@ -2,7 +2,7 @@ clearpath_robot - 0.1.2 + 0.1.3 Clearpath Robot Metapackage Luis Camero Roni Kreinin diff --git a/clearpath_sensors/CHANGELOG.rst b/clearpath_sensors/CHANGELOG.rst index f6be69a..7067b2f 100644 --- a/clearpath_sensors/CHANGELOG.rst +++ b/clearpath_sensors/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package clearpath_sensors ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.1.3 (2023-10-04) +------------------ * Removed 'platform' from default namespace * Added image proc as container * Missing comma diff --git a/clearpath_sensors/package.xml b/clearpath_sensors/package.xml index 0352c81..75fd1df 100644 --- a/clearpath_sensors/package.xml +++ b/clearpath_sensors/package.xml @@ -2,7 +2,7 @@ clearpath_sensors - 0.1.2 + 0.1.3 Clearpath sensor default launch files and configurations Luis Camero From 31c7119fc57bda00eb4b146bad6cb015bb220c18 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Tue, 5 Dec 2023 13:32:37 -0500 Subject: [PATCH 37/41] Added D100 and D150 to generator and battery node --- .../battery_state/battery.py | 33 ++++++++++++++++- .../launch/generator.py | 36 ++++++++++++++++--- 2 files changed, 64 insertions(+), 5 deletions(-) diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py index d98dbdf..dc15b6a 100644 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py @@ -94,6 +94,13 @@ def __init__( 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 @@ -257,11 +264,35 @@ 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.U1_35: U1_35, + BatteryConfig.TLV1222: TLV1222, + BatteryConfig.RB20: RB20 } def __new__(cls, diff --git a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py index 3957221..1e946c2 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py +++ b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py @@ -79,8 +79,8 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: ] ) - # W200 MicroROS Agent - self.w200_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', @@ -205,11 +205,39 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: Platform.W200: common_platform_components + [ self.imu_0_filter_node, self.imu_0_filter_config, - self.w200_uros_node, + 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, + ], } def generate_sensors(self) -> None: From 6bb9c519d5c86b709cd52f8faa0f7f2ed4089409 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Tue, 5 Dec 2023 13:34:18 -0500 Subject: [PATCH 38/41] Added dingo to battery state control --- .../clearpath_diagnostics/battery_state/battery_state_control | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control index 7c51651..a4c0b33 100755 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control @@ -78,7 +78,7 @@ class BatteryStateControl(Node): qos_profile_sensor_data) match self.platform: - case Platform.J100: + case Platform.J100 | Platform.DD100 | Platform.DO100 | Platform.DD150 | Platform.DO150: self.hmi_battery_pub = self.create_publisher( UInt8, 'platform/mcu/_hmi', @@ -114,7 +114,7 @@ class BatteryStateControl(Node): def battery_state_callback(self, msg: BatteryState): match self.platform: - case Platform.J100: + case Platform.J100 | Platform.DD100 | Platform.DO100 | Platform.DD150 | Platform.DO150: self.hmi_control(msg.percentage) From 58948d5ac03851324a082b0d178fc535dae1b432 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Tue, 5 Dec 2023 15:24:38 -0500 Subject: [PATCH 39/41] Set battery charging status --- .../clearpath_diagnostics/battery_state/battery.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py index dc15b6a..5d8e7c1 100644 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py @@ -149,6 +149,11 @@ def update(self, power_msg: Power): 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() @@ -175,10 +180,6 @@ def update_from_lut(self): self._msg.percentage = self.linear_interpolation(self.LUT, avg_voltage) self._msg.charge = self._msg.capacity * self._msg.percentage - # Power supply status - if self._msg.percentage == 1.0: - self._msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_FULL - def update_cells(self): self._msg.cell_voltage = [self._msg.voltage / self.series] * self.cell_count self._msg.cell_temperature = [nan] * self.cell_count From 0e14a0419f4aed0ef8d7de16cf9ef4e5ff97b069 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Tue, 5 Dec 2023 16:07:22 -0500 Subject: [PATCH 40/41] Added S1P2 battery configuration --- .../clearpath_diagnostics/battery_state/battery.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py index 5d8e7c1..2a8d9d9 100644 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py @@ -48,9 +48,10 @@ class BaseBattery: """Battery configuration. Represents number of battery cells in series and parallel.""" CONFIGURATIONS = { BatteryConfig.S1P1: (1, 1), - BatteryConfig.S2P1: (2, 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), } From 2ef406f30c074e578db6ea799f5c2714bce1c15d Mon Sep 17 00:00:00 2001 From: Tony Baltovski Date: Wed, 13 Dec 2023 10:59:42 -0500 Subject: [PATCH 41/41] [clearpath_generator_robot] Disabled depend for now. --- clearpath_generator_robot/package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/clearpath_generator_robot/package.xml b/clearpath_generator_robot/package.xml index c0de5b7..01a1006 100644 --- a/clearpath_generator_robot/package.xml +++ b/clearpath_generator_robot/package.xml @@ -16,7 +16,8 @@ clearpath_sensors imu_filter_madgwick micro_ros_agent - sevcon_traction + + wireless_watcher ament_lint_auto