Merge branch 'main' into add-ambient-pressure

pyproject.toml

@@ -15,7 +15,6 @@ dependencies = [
     'numpy>=1.26',
     'pytest-qt',
     'pytest-xvfb',
-    'Pyside2',  # can be removed when ament_pep257 is gone
     'TCA9555@git+https://github.com/InvincibleRMC/TCA9555',
     'pymavlink',
     'mavproxy'

src/surface/rov_flir/launch/flir_launch.py

@@ -62,7 +62,7 @@ def generate_launch_description() -> LaunchDescription:
         exec_name='front_cam',
         emulate_tty=True,
         output='screen',
-        parameters=[Parameter('serial_number', '23473577'),
+        parameters=[Parameter('serial_number', '23473566'),
                     Parameter('parameter_file', parameter_file),
                     parameters],
         condition=IfCondition(front_arg)
@@ -76,7 +76,7 @@ def generate_launch_description() -> LaunchDescription:
         exec_name='bottom_cam',
         emulate_tty=True,
         output='screen',
-        parameters=[Parameter('serial_number', '23473566'),
+        parameters=[Parameter('serial_number', '23473577'),
                     Parameter('parameter_file', parameter_file),
                     parameters],
         condition=IfCondition(bottom_arg)

src/surface/rov_flir/rov_flir/flir_watchdog.py

@@ -1,30 +1,64 @@
 import atexit
-import sys
+import os
+import subprocess
 from subprocess import Popen
+import re
+from signal import SIGINT
 
 import rclpy
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
 
 WATCHDOG_RATE = 10
 NAMESPACE = "surface"
+MIN_FPS = 1.0
 
 
 class Watchdog():
-    def __init__(self, node: Node, args: list[str]) -> None:
+    def __init__(self, name: str, node: Node, args: list[str]) -> None:
+        self.name = name
         self.node = node
         self.args = args
         self.process: Popen[bytes]
 
         self.start_process()
 
     def start_process(self) -> None:
-        self.process = Popen(self.args, stdout=sys.stdout, stderr=sys.stderr)
+        self.process = Popen(self.args, stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
+
+        if self.process.stdout is None:
+            raise RuntimeError("Child process has not stdout")
+
+        os.set_blocking(self.process.stdout.fileno(), False)
+
+    def read_stdout(self) -> bytes:
+        if self.process.stdout is None:
+            raise RuntimeError("Child process has not stdout")
+
+        return self.process.stdout.readline()
 
     def poll(self) -> None:
         if self.process.poll() is not None:
-            self.node.get_logger().warning("Detected camera crash, restarting...")
+            self.node.get_logger().warning(f"{self.name} has crashed, restarting...")
             self.start_process()
+            return
+
+        line = self.read_stdout()
+        while line:
+            match = re.search(r"rate \[Hz] in +([\d\.]+) out", line.decode().strip())
+            if match:
+                try:
+                    rate = float(match.group(1))
+                except ValueError:
+                    continue
+
+                if rate < MIN_FPS:
+                    # If we're receiving less than 1 fps, assume the camera has disconnected
+                    self.node.get_logger().warning(f"{self.name} frozen, killing...")
+                    self.process.send_signal(SIGINT)
+                    return
+
+            line = self.read_stdout()
 
     def kill(self) -> None:
         self.process.kill()
@@ -39,11 +73,13 @@ def __init__(self) -> None:
         self.get_logger().info("Starting camera drivers")
 
         self.front_watchdog = Watchdog(
+            name="Front cam",
             node=self,
             args=["ros2", "launch", "rov_flir", "flir_launch.py",
                   "launch_bottom:=false", f"ns:={NAMESPACE}"]
         )
         self.bottom_watchdog = Watchdog(
+            name="Bottom cam",
             node=self,
             args=["ros2", "launch", "rov_flir", "flir_launch.py",
                   "launch_front:=false", f"ns:={NAMESPACE}"]