fixed bugs and created startup script

vortexntnu · Oct 29, 2023 · 077e2a8 · 077e2a8
1 parent 429f758
commit 077e2a8
Show file tree

Hide file tree

Showing 362 changed files with 84 additions and 13,165 deletions.
diff --git a/.clang-format b/.clang-format
diff --git a/.dockerignore b/.dockerignore
diff --git a/.github/workflows/clang-formatter.yml b/.github/workflows/clang-formatter.yml
diff --git a/.github/workflows/docker-publish.yml b/.github/workflows/docker-publish.yml
diff --git a/.github/workflows/pep8-formatter.yml b/.github/workflows/pep8-formatter.yml
diff --git a/.gitignore b/.gitignore
diff --git a/LICENSE b/LICENSE
diff --git a/README.md b/README.md
diff --git a/asv_setup/CMakeLists.txt b/asv_setup/CMakeLists.txt
diff --git a/asv_setup/README.md b/asv_setup/README.md
diff --git a/asv_setup/config/environments/trondheim_freshwater.yaml b/asv_setup/config/environments/trondheim_freshwater.yaml
diff --git a/asv_setup/config/environments/trondheim_saltwater.yaml b/asv_setup/config/environments/trondheim_saltwater.yaml
diff --git a/asv_setup/launch/pc.launch.yaml b/asv_setup/launch/pc.launch.yaml
diff --git a/asv_setup/package.xml b/asv_setup/package.xml
diff --git a/dependencies.repos b/dependencies.repos
diff --git a/mission/joystick_interface/CMakeLists.txt b/mission/joystick_interface/CMakeLists.txt
diff --git a/mission/joystick_interface/README.md b/mission/joystick_interface/README.md
diff --git a/mission/joystick_interface/config/params.yaml b/mission/joystick_interface/config/params.yaml
diff --git a/mission/joystick_interface/joystick_interface/__init__.py b/mission/joystick_interface/joystick_interface/__init__.py
diff --git a/mission/joystick_interface/joystick_interface/test/__init__.py b/mission/joystick_interface/joystick_interface/test/__init__.py
diff --git a/mission/joystick_interface/joystick_interface/test/test_joystick_interface.py b/mission/joystick_interface/joystick_interface/test/test_joystick_interface.py
diff --git a/mission/joystick_interface/launch/joystick_interface.launch.yaml b/mission/joystick_interface/launch/joystick_interface.launch.yaml
diff --git a/mission/joystick_interface/package.xml b/mission/joystick_interface/package.xml
diff --git a/sensors/bms/bms/__init__.py b/sensors/bms/bms/__init__.py
diff --git a/sensors/bms/bms/freya_bms.py b/sensors/bms/bms/freya_bms.py
@@ -1,4 +1,7 @@
 import subprocess
+import rclpy.logging
+
+#TODO: use ros2 logging instead of python print
 
 class BMS:
     """ Class containing Freya's BMS system. 
@@ -58,6 +61,8 @@ def __init__(self, usb_port: str = None) -> None:
         else:
             self.usb_port = BMS.find_usb_ports()[0]
 
+        self._logger = logger
+
         self._voltage = 0
         self._current = 0
         self._design_capacity = 0
@@ -105,7 +110,7 @@ def find_usb_ports() -> list[str]:
         if len(usb_devices) == 0:
             raise Exception("No USB device was found. Ensure that battery pack is connected to Raspberry Pi")
 
-        return usb_devices
+        return bms_ports
 
     @staticmethod
     def get_bms_data(command: str) -> str | None:
@@ -144,31 +149,32 @@ def get_bms_data(command: str) -> str | None:
 
         try: 
             response = subprocess.run(command,
-                                  stdout=subprocess.PIPE,
-                                  stderr=subprocess.PIPE,
-                                  check=True)
+                                    stdout=subprocess.PIPE,
+                                    stderr=subprocess.PIPE,
+                                    check=True)
 
             return response.stdout.decode()
         except subprocess.CalledProcessError as e:
             print("An error occured when getting BMS data")
             print(f"Error: {e.stderr.decode()}")
-            print("Please check that USBs are connected to Raspberry Pi, and that the _usb_port variable is set correctly")
+            # print("Please check that USBs are connected to Raspberry Pi, and that the _usb_port variable is set correctly")
 
             return None
 
-    def parse_bms_data(self, bms_data: str) -> None:
+    def parse_bms_data(self, bms_data: str) -> bool:
         """
             Parses BMS data and updates class members accordingly
 
             Parameters:
                 bms_data (str): string containing result of the jbdtool command
 
-            Returns: None
+            Returns: 
+                Returns a bool indicating whether data was found or not
         """
 
         if bms_data == "":
             print("Warning: No data was found.")
-            return
+            return False
 
         data = [entry.split() for entry in bms_data.split("\n")][:-1]       # [:-1] is only there because there is a empty list at the end for some reason
 
@@ -192,6 +198,8 @@ def parse_bms_data(self, bms_data: str) -> None:
         self._version = data[17][1]
         self._FET = data[18][1]
 
+        return True
+
     def change_usb_port(self, usb_port: str) -> None:
         """
             Changes the usb port.

diff --git a/sensors/bms/bms/freya_bms_node.py b/sensors/bms/bms/freya_bms_node.py
@@ -1,13 +1,10 @@
 import rclpy
 from rclpy.node import Node
+import rclpy.logging
 from sensor_msgs.msg import BatteryState
-from diagnostic_msgs.msg import DiagnosticStatus, KeyValue
+from diagnostic_msgs.msg import DiagnosticStatus, KeyValue, DiagnosticArray
 from bms.freya_bms import BMS
 
-
-#TODO: run the node on startup
-
-
 class FreyaBMSNode(Node):
     """
         Publishes Freya's BMS data to ROS.
@@ -27,7 +24,8 @@ def __init__(self, usb_ports: list[str]=None) -> None:
                 None
         """
         super().__init__(f'freya_bms')
-
+        rclpy.logging.initialize()
+
         if usb_ports:
             self._bms_systems = [BMS(usb_port=port) for port in usb_ports]
         else:
@@ -36,7 +34,7 @@ def __init__(self, usb_ports: list[str]=None) -> None:
 
         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._diagnostics_publisher = self.create_publisher(DiagnosticArray, '/diagnostics', 10)
 
         self._timer = self.create_timer(2, self.publish_bms_data)
 
@@ -46,38 +44,45 @@ def publish_bms_data(self) -> None:
             Publishes BMS data to ros2 topics.
         """
 
+        diagnostic_array = DiagnosticArray()
+
         for i, bms_system in enumerate(self._bms_systems):
             battery_msg = BatteryState()
-            diagnostics_msg = DiagnosticStatus()
+            diagnostic_status = DiagnosticStatus()
 
-            bms_system.parse_bms_data(bms_system.get_bms_data(bms_system.command))
+            res = 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
+            if res:
+                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)
+                self._batterystate_publishers[i].publish(battery_msg)
 
-            diagnostics_msg.name = f"Freya battery status {i}"
-
-            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
+                diagnostic_status.name = f"Freya battery status {i}"
+            
+                if bms_system.percent_capacity * 100 < 1:
+                    diagnostic_status.level = DiagnosticStatus.ERROR
+                elif bms_system.percent_capacity * 100 < 20:
+                    diagnostic_status.level = DiagnosticStatus.WARN
+                else:
+                    diagnostic_status.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)])
+                diagnostic_status.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)"
+                diagnostic_status.message = "level indicates whether the battery \
+                level is above 20 (OK), below 20 (WARN), or below 1 (ERROR)"
+
+                diagnostic_array.status.append(diagnostic_status)
+            else:
+                self.get_logger().warn(f"No data to be published. Battery {i} may not be connected")
 
-            self._diagnostics_publishers[i].publish(diagnostics_msg)
+        self._diagnostics_publisher.publish(diagnostic_array)
 
 def create_key_value_pair(key: str, value) -> KeyValue:
     kv = KeyValue()

diff --git a/sensors/bms/bms/old_freya_bms.py b/sensors/bms/bms/old_freya_bms.py
diff --git a/sensors/bms/bms/old_freya_bms_node.py b/sensors/bms/bms/old_freya_bms_node.py
diff --git a/sensors/bms/bms/testfile.py b/sensors/bms/bms/testfile.py
diff --git a/sensors/bms/install/.colcon_install_layout b/sensors/bms/install/.colcon_install_layout