Added support for multiple BMS-es, as well as Ros diagnostics message

sensors/bms/bms/freya_bms.py

@@ -38,6 +38,8 @@ class BMS:
             returns pure BMS data string, or None if exception is thrown 
         change_usb_port(usb_port: str) -> None:
             changes the usb port for the BMS
+    
+    Note: Private members are denoted by _variable_name            
     """
 
     def __init__(self, usb_port: str = None) -> None:
@@ -49,27 +51,12 @@ def __init__(self, usb_port: str = None) -> None:
 
             Returns:
                 None
-
-            Note: Private members are denoted by _variable_name            
         """     
         if usb_port:
             self._usb_port = usb_port
             self._command = ["jbdtool", "-t", f"serial:/dev/{self._usb_port}"]
         else:
-            print("Looking for USB devices...")
-            devices = subprocess.check_output(["ls", "/dev"], text=True).split("\n")
-            usb_devices = [device for device in devices if device[:6] == "ttyUSB"]
-
-            for i, device in enumerate(usb_devices):
-                self._usb_port = device
-                self._command = ["jbdtool", "-t", f"serial:/dev/{self._usb_port}"]
-                resp = self.get_bms_data()
-                if resp != "":
-                    print(f"Found device {self._usb_port}")
-                    break
-
-                if i == len(usb_devices) - 1:
-                    raise Exception("No USB device was found. Ensure that battery pack is connected to Raspberry Pi")
+            self.usb_port = BMS.find_usb_ports()[0]
 
         self._voltage = 0
         self._current = 0
@@ -91,11 +78,42 @@ def __init__(self, usb_port: str = None) -> None:
         self._version = ""
         self._FET = ""
 
-    def get_bms_data(self) -> str | None:
+    @staticmethod
+    def find_usb_ports() -> list[str]:
+        """
+            Queries all usb ports with jbdtool to find connected bms
+
+            Parameters:
+
+            Returns:
+                All USB ports which respond to jbtool command
+        """
+        print("Looking for USB devices...")
+        bms_ports = []
+
+        devices = subprocess.check_output(["ls", "/dev"], text=True).split("\n")
+        usb_devices = [device for device in devices if device[:6] == "ttyUSB"]
+
+        for device in usb_devices:
+            usb_port = device
+            command = ["jbdtool", "-t", f"serial:/dev/{usb_port}"]
+            resp = BMS.get_bms_data(command)
+            if resp != "":
+                print(f"Found device {usb_port}")
+                bms_ports.append(usb_port)
+
+        if len(usb_devices) == 0:
+            raise Exception("No USB device was found. Ensure that battery pack is connected to Raspberry Pi")
+
+        return usb_devices
+
+    @staticmethod
+    def get_bms_data(command: str) -> str | None:
         """
             Function for getting data from the BMS
 
             Parameters: 
+                command (str): The jbdtool command to run (use self.command)
 
             Returns: 
                 if the jbdtool call works, it returns the BMS data as a string, 
@@ -125,7 +143,7 @@ def get_bms_data(self) -> str | None:
         """
 
         try: 
-            response = subprocess.run(self._command,
+            response = subprocess.run(command,
                                   stdout=subprocess.PIPE,
                                   stderr=subprocess.PIPE,
                                   check=True)
@@ -174,7 +192,6 @@ def parse_bms_data(self, bms_data: str) -> None:
         self._version = data[17][1]
         self._FET = data[18][1]
 
-
     def change_usb_port(self, usb_port: str) -> None:
         """
             Changes the usb port.
@@ -190,6 +207,10 @@ def change_usb_port(self, usb_port: str) -> None:
         self._command = ["jbdtool", "-t", f"serial:/dev/{usb_port}"]
 
 #region getters
+    @property
+    def command(self):
+        return self._command
+
     @property 
     def voltage(self):
         return self._voltage

sensors/bms/bms/freya_bms_node.py

@@ -1,9 +1,12 @@
 import rclpy
 from rclpy.node import Node
 from sensor_msgs.msg import BatteryState
+from diagnostic_msgs.msg import DiagnosticStatus, KeyValue
 from bms.freya_bms import BMS
 
-# TODO: also publish ros2 Diagnostics message with BMS data
+
+#TODO: run the node on startup
+
 
 class FreyaBMSNode(Node):
     """
@@ -14,7 +17,7 @@ class FreyaBMSNode(Node):
         publish_bms_data() -> None
             publises BMS data from BMS system to ros2 node.
     """
-    def __init__(self, usb_port: str=None) -> None:
+    def __init__(self, usb_ports: list[str]=None) -> None:
         """
             Parameters:
                 usb_port(str): The USB port to look for BMS data from. If no port is
@@ -23,37 +26,74 @@ def __init__(self, usb_port: str=None) -> None:
             Returns: 
                 None
         """
-        super().__init__('freya_bms')
-        self._publisher = self.create_publisher(BatteryState, '/internal/status/bms', 10)
-        self._timer = self.create_timer(2, self.publish_bms_data)
-
-        if usb_port:
-            self._bms_system = BMS(usb_port=usb_port)
+        super().__init__(f'freya_bms')
+
+        if usb_ports:
+            self._bms_systems = [BMS(usb_port=port) for port in usb_ports]
         else:
-            self._bms_system = BMS()
+            ports = BMS.find_usb_ports()
+            self._bms_systems = [BMS(usb_port=port) for port in ports]
+
+        self._batterystate_publishers = [self.create_publisher(BatteryState, f'/internal/status/bms{i}', 10) for i in range(len(self._bms_systems))]
+
+        self._diagnostics_publishers = [self.create_publisher(DiagnosticStatus, f'/internal/status/bms_diagnostic{i}', 10) for i in range(len(self._bms_systems))]
+
+        self._timer = self.create_timer(2, self.publish_bms_data)
+
 
     def publish_bms_data(self) -> None:
         """
-            Publishes BMS data to ros2 node.
+            Publishes BMS data to ros2 topics.
         """
-        battery_msg = BatteryState()
-
-        self._bms_system.parse_bms_data(self._bms_system.get_bms_data())
 
-        battery_msg.voltage = self._bms_system.voltage
-        battery_msg.current = self._bms_system.current
-        battery_msg.cell_temperature = self._bms_system.temps
-        battery_msg.percentage = self._bms_system.percent_capacity
+        for i, bms_system in enumerate(self._bms_systems):
+            battery_msg = BatteryState()
+            diagnostics_msg = DiagnosticStatus()
+
+            bms_system.parse_bms_data(bms_system.get_bms_data(bms_system.command))
+
+            battery_msg.voltage = bms_system.voltage
+            battery_msg.current = bms_system.current
+            battery_msg.cell_temperature = bms_system.temps
+            battery_msg.percentage = bms_system.percent_capacity
+
+            self._batterystate_publishers[i].publish(battery_msg)
+
+            diagnostics_msg.name = f"Freya battery status {i}"
 
-        self._publisher.publish(battery_msg)
+            if bms_system.percent_capacity < 1:
+                diagnostics_msg.level = DiagnosticStatus.ERROR
+            elif bms_system.percent_capacity < 20:
+                diagnostics_msg.level = DiagnosticStatus.WARN
+            else:
+                diagnostics_msg.level = DiagnosticStatus.OK
+
+            diagnostics_msg.values.extend([
+                create_key_value_pair("voltage", bms_system.voltage), 
+                create_key_value_pair("current", bms_system.current), 
+                create_key_value_pair("temps", bms_system.temps), 
+                create_key_value_pair("percentage", bms_system.percent_capacity)])
+
+            diagnostics_msg.message = "level indicates whether the battery level is \
+            below 0 (ERROR), below 20 (WARN) or above 20 (OK)"
+
+            self._diagnostics_publishers[i].publish(diagnostics_msg)
+
+def create_key_value_pair(key: str, value) -> KeyValue:
+    kv = KeyValue()
+    kv.key = str(key)
+    kv.value = str(value)
+
+    return kv
 
 def main(args=None):
     rclpy.init(args=args)
 
-    freya_bms_node = FreyaBMSNode()
-    rclpy.spin(freya_bms_node)
+    node = FreyaBMSNode()
+
+    rclpy.spin(node)
 
-    freya_bms_node.destroy_node()
+    node.destroy_node()    
     rclpy.shutdown()
 
 if __name__ == "__main__":

sensors/bms/install/bms/share/bms/launch/bms_launch.py

@@ -1,13 +1,41 @@
-from launch import LaunchDescription
 from launch_ros.actions import Node
 
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction
+# from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PythonExpression
+
 def generate_launch_description():
+    # usb_port_1 = LaunchConfiguration("usb_port_1")
+    # usb_port_2 = LaunchConfiguration("usb_port_2")
+
+
     return LaunchDescription([
+        DeclareLaunchArgument(
+            "usb_port_1",
+            default_value="",
+            description="USB port of first battery package"
+        ),
+        DeclareLaunchArgument(
+            "usb_port_2",
+            default_value="",
+            description="USB port of second battery package"
+        ),
+        Node(
+            package="bms",
+            executable="bms_publisher",
+            name="freya_bms1",
+            parameters=[
+                {"usb_port": LaunchConfiguration("usb_port_1")}
+            ]
+        ),
         Node(
             package="bms",
-            namespace="/internal/status",
-            executable="talker",
-            name="bms1"
+            executable="bms_publisher",
+            name="freya_bms",
+            parameters=[
+                {"usb_port": LaunchConfiguration("usb_port_2")}
+            ]
         )
         # Node(
         #     package="bms",

sensors/bms/log/build_2023-10-22_16-17-57/bms/command.log

@@ -0,0 +1,2 @@
+Invoking command in '/home/vortex/david_test_ws/vortex-asv/sensors/bms': PS1=${debian_chroot:+($debian_chroot)}\[\033[01;32m\]\u@\h\[\033[00m\]:\[\033[01;34m\]\w\[\033[00m\]\[\033[35m\]$(parse_git_branch)\[\033[00m\]\$ PYTHONPATH=/home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/prefix_override:/home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 setup.py egg_info --egg-base build/bms build --build-base /home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/build install --record /home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/install.log --single-version-externally-managed
+Invoked command in '/home/vortex/david_test_ws/vortex-asv/sensors/bms' returned '0': PS1=${debian_chroot:+($debian_chroot)}\[\033[01;32m\]\u@\h\[\033[00m\]:\[\033[01;34m\]\w\[\033[00m\]\[\033[35m\]$(parse_git_branch)\[\033[00m\]\$ PYTHONPATH=/home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/prefix_override:/home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 setup.py egg_info --egg-base build/bms build --build-base /home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/build install --record /home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/install.log --single-version-externally-managed

sensors/bms/log/build_2023-10-22_16-17-57/bms/stderr.log

@@ -0,0 +1,2 @@
+/usr/lib/python3/dist-packages/setuptools/command/install.py:34: SetuptoolsDeprecationWarning: setup.py install is deprecated. Use build and pip and other standards-based tools.
+  warnings.warn(

sensors/bms/log/build_2023-10-22_16-17-57/bms/stdout.log

@@ -0,0 +1,23 @@
+running egg_info
+writing build/bms/bms.egg-info/PKG-INFO
+writing dependency_links to build/bms/bms.egg-info/dependency_links.txt
+writing entry points to build/bms/bms.egg-info/entry_points.txt
+writing requirements to build/bms/bms.egg-info/requires.txt
+writing top-level names to build/bms/bms.egg-info/top_level.txt
+reading manifest file 'build/bms/bms.egg-info/SOURCES.txt'
+writing manifest file 'build/bms/bms.egg-info/SOURCES.txt'
+running build
+running build_py
+copying bms/freya_bms_node.py -> /home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/build/lib/bms
+running install
+running install_lib
+copying /home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/build/lib/bms/freya_bms_node.py -> /home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/lib/python3.10/site-packages/bms
+byte-compiling /home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/lib/python3.10/site-packages/bms/freya_bms_node.py to freya_bms_node.cpython-310.pyc
+running install_data
+copying launch/bms_launch.py -> /home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/share/bms/launch
+running install_egg_info
+removing '/home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/lib/python3.10/site-packages/bms-0.0.0-py3.10.egg-info' (and everything under it)
+Copying build/bms/bms.egg-info to /home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/lib/python3.10/site-packages/bms-0.0.0-py3.10.egg-info
+running install_scripts
+Installing bms_publihser script to /home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/lib/bms
+writing list of installed files to '/home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/install.log'

sensors/bms/log/build_2023-10-22_16-17-57/bms/stdout_stderr.log

@@ -0,0 +1,25 @@
+running egg_info
+writing build/bms/bms.egg-info/PKG-INFO
+writing dependency_links to build/bms/bms.egg-info/dependency_links.txt
+writing entry points to build/bms/bms.egg-info/entry_points.txt
+writing requirements to build/bms/bms.egg-info/requires.txt
+writing top-level names to build/bms/bms.egg-info/top_level.txt
+reading manifest file 'build/bms/bms.egg-info/SOURCES.txt'
+writing manifest file 'build/bms/bms.egg-info/SOURCES.txt'
+running build
+running build_py
+copying bms/freya_bms_node.py -> /home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/build/lib/bms
+running install
+/usr/lib/python3/dist-packages/setuptools/command/install.py:34: SetuptoolsDeprecationWarning: setup.py install is deprecated. Use build and pip and other standards-based tools.
+  warnings.warn(
+running install_lib
+copying /home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/build/lib/bms/freya_bms_node.py -> /home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/lib/python3.10/site-packages/bms
+byte-compiling /home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/lib/python3.10/site-packages/bms/freya_bms_node.py to freya_bms_node.cpython-310.pyc
+running install_data
+copying launch/bms_launch.py -> /home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/share/bms/launch
+running install_egg_info
+removing '/home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/lib/python3.10/site-packages/bms-0.0.0-py3.10.egg-info' (and everything under it)
+Copying build/bms/bms.egg-info to /home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/lib/python3.10/site-packages/bms-0.0.0-py3.10.egg-info
+running install_scripts
+Installing bms_publihser script to /home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/lib/bms
+writing list of installed files to '/home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/install.log'

sensors/bms/log/build_2023-10-22_16-17-57/bms/streams.log

@@ -0,0 +1,27 @@
+[2.977s] Invoking command in '/home/vortex/david_test_ws/vortex-asv/sensors/bms': PS1=${debian_chroot:+($debian_chroot)}\[\033[01;32m\]\u@\h\[\033[00m\]:\[\033[01;34m\]\w\[\033[00m\]\[\033[35m\]$(parse_git_branch)\[\033[00m\]\$ PYTHONPATH=/home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/prefix_override:/home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 setup.py egg_info --egg-base build/bms build --build-base /home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/build install --record /home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/install.log --single-version-externally-managed
+[3.880s] running egg_info
+[3.882s] writing build/bms/bms.egg-info/PKG-INFO
+[3.883s] writing dependency_links to build/bms/bms.egg-info/dependency_links.txt
+[3.884s] writing entry points to build/bms/bms.egg-info/entry_points.txt
+[3.885s] writing requirements to build/bms/bms.egg-info/requires.txt
+[3.885s] writing top-level names to build/bms/bms.egg-info/top_level.txt
+[3.893s] reading manifest file 'build/bms/bms.egg-info/SOURCES.txt'
+[3.898s] writing manifest file 'build/bms/bms.egg-info/SOURCES.txt'
+[3.898s] running build
+[3.899s] running build_py
+[3.900s] copying bms/freya_bms_node.py -> /home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/build/lib/bms
+[3.902s] running install
+[3.904s] /usr/lib/python3/dist-packages/setuptools/command/install.py:34: SetuptoolsDeprecationWarning: setup.py install is deprecated. Use build and pip and other standards-based tools.
+[3.904s]   warnings.warn(
+[3.905s] running install_lib
+[3.909s] copying /home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/build/lib/bms/freya_bms_node.py -> /home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/lib/python3.10/site-packages/bms
+[3.913s] byte-compiling /home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/lib/python3.10/site-packages/bms/freya_bms_node.py to freya_bms_node.cpython-310.pyc
+[3.916s] running install_data
+[3.918s] copying launch/bms_launch.py -> /home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/share/bms/launch
+[3.919s] running install_egg_info
+[3.925s] removing '/home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/lib/python3.10/site-packages/bms-0.0.0-py3.10.egg-info' (and everything under it)
+[3.927s] Copying build/bms/bms.egg-info to /home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/lib/python3.10/site-packages/bms-0.0.0-py3.10.egg-info
+[3.937s] running install_scripts
+[4.016s] Installing bms_publihser script to /home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/lib/bms
+[4.018s] writing list of installed files to '/home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/install.log'
+[4.148s] Invoked command in '/home/vortex/david_test_ws/vortex-asv/sensors/bms' returned '0': PS1=${debian_chroot:+($debian_chroot)}\[\033[01;32m\]\u@\h\[\033[00m\]:\[\033[01;34m\]\w\[\033[00m\]\[\033[35m\]$(parse_git_branch)\[\033[00m\]\$ PYTHONPATH=/home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/prefix_override:/home/vortex/david_test_ws/vortex-asv/sensors/bms/install/bms/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 setup.py egg_info --egg-base build/bms build --build-base /home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/build install --record /home/vortex/david_test_ws/vortex-asv/sensors/bms/build/bms/install.log --single-version-externally-managed