Add mocks for siyi sdk and script to test perception class. (#230)

README.md

@@ -87,3 +87,7 @@ I copied a lot of the config from this tutorial: https://docs.ros.org/en/foxy/Ho
 ### Jetson Installation
 pytorch: https://forums.developer.nvidia.com/t/pytorch-for-jetson/72048
 as of jan 23, the jetson is on Jetpack 6
+
+### Rosbag playing example
+cd /forge/ws/logs/bagfiles/rosbag2_2024_06_12-17_23_07
+while true; do rso2 bag play rosbag2_2024_06_12-17_23_07.db3; done

scripts/camera_test_node.py

@@ -3,26 +3,19 @@
 import rclpy
 from rclpy.node import Node
 from uavf_2024.imaging import Camera
-from time import sleep
+from time import sleep, strftime
 
 class CameraTestNode(Node):
     def __init__(self) -> None:
         super().__init__('imaging_node')
-        self.camera = Camera()
+        self.camera = Camera(log_dir = f"/home/forge/ws/logs/{strftime('%m-%d %Hh%Mm')}/camera")
+        self.camera.start_recording()
         self.camera.setAbsoluteZoom(1)
-        self.camera.cam.requestAbsolutePosition(0, 0)
-        sleep(2)
+        self.camera.request_center()
 
     def loop(self):
-        while True:
-            print(self.camera.cam.getAttitude())
-            sleep(1/10)
-            # if self.camera.cam.requestGimbalAttitude():
-            #     attitude = self.camera.cam.getAttitude()
-            #     self.get_logger().info(str(attitude))
-            # else:
-            #     self.get_logger().info(":(")
-            # sleep(1)
+        for _ in range(1000):
+            sleep(0.1)
 
 
 def main(args=None) -> None:

scripts/run_perception.py

@@ -0,0 +1,64 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from uavf_2024.imaging import Perception, PoseProvider
+from concurrent.futures import as_completed
+import traceback
+
+class PerceptionMinimalNode(Node):
+    ''' Receives input from every ongoing process.
+        Output to PX4 Interface Node.
+    '''
+
+    def __init__(self) -> None:
+        super().__init__('perception_test_node')
+        self.perception = Perception(PoseProvider(self))
+        self.timer_period = 1.0  # seconds
+        self.create_timer(self.timer_period, self.timer_cb)
+        self.perception_futures = []
+        self.time_alive = 0
+
+    def log(self, msg):
+        self.get_logger().info(msg)
+
+    def timer_cb(self):
+        self.perception_futures.append(self.perception.get_image_down_async())
+        self.time_alive += 1
+        if self.time_alive > 10:
+            self.log('Shutting down...')
+            self.log(f"results: {self.collect_results()}")
+            self.destroy_node()
+            rclpy.shutdown()
+
+    def collect_results(self):
+        detections = []
+        timeout = 2.0
+        try:
+            for future in as_completed(self.perception_futures, timeout=timeout):
+                detections.extend(future.result())
+
+            self.log(f"Successfully retrieved imaging detections: {detections}")
+        except TimeoutError:
+            self.log("Timed out waiting for imaging detections.")
+
+        self.perception_futures = []
+
+        return detections
+
+
+
+def main(args=None) -> None:
+    print('Starting commander node...')
+    rclpy.init(args=args)
+    node = PerceptionMinimalNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    try:
+        main()
+    except Exception as e:
+        traceback.print_exc()

siyi_sdk/siyi_sdk/__init__.py

@@ -1,2 +1,3 @@
 from .siyi_sdk import SIYISDK
-from .siyi_stream import SIYISTREAM
+from .siyi_stream import SIYISTREAM
+from .mocks import MockSIYISDK, MockSIYISTREAM

siyi_sdk/siyi_sdk/mocks.py

@@ -0,0 +1,139 @@
+import numpy as np
+
+class MockSIYISTREAM:
+    def __init__(self, server_ip : str = "192.168.144.25", port : int = 8554, name : str = "main.264", debug : bool = False):
+        """
+        
+        Params
+        --
+        - server_ip [str] IP address of the camera
+        - port: [int] UDP port of the camera
+        - name: [str] name of the stream
+        """
+        pass
+
+    def connect(self):
+        return True
+
+    def disconnect(self):
+        return
+
+    def get_frame(self) -> np.ndarray | None:
+        return np.random.randint(0,255, (1080, 1920, 3)).astype(np.float32)
+
+class MockSIYISDK:
+    def __init__(self, server_ip="192.168.144.25", port=37260, debug=False):
+        self.zoom = 1
+        self.attitude = (0,0,0)
+
+    def resetVars(self):
+        return True
+
+    def connect(self, maxWaitTime=3.0):
+        return True
+
+    def disconnect(self):
+        pass
+
+    def checkConnection(self):
+        pass
+
+    def connectionLoop(self, t):
+        pass
+
+    def isConnected(self):
+        return True
+
+    def requestFirmwareVersion(self):
+        return True
+
+    def requestHardwareID(self):
+        return True
+
+    def requestGimbalAttitude(self):
+        return True
+
+    def requestGimbalInfo(self):
+        return True
+
+    def requestFunctionFeedback(self):
+        return True
+
+    def requestAutoFocus(self):
+        return True
+
+    def requestAbsoluteZoom(self, level, msg=None):
+        return True
+
+    def requestAbsolutePosition(self, yaw, pitch):
+        self.attitude = (yaw, pitch, 0)
+
+    def requestZoomIn(self):
+        return True
+
+    def requestZoomOut(self):
+        return True
+
+    def requestZoomHold(self):
+        return True
+
+    def requestLongFocus(self):
+        return True
+
+    def requestCloseFocus(self):
+        return True
+
+    def requestFocusHold(self):
+        return True
+
+    def requestCenterGimbal(self):
+        return True
+
+    def requestGimbalSpeed(self, yaw_speed:int, pitch_speed:int):
+        return True
+
+    def requestPhoto(self):
+        return True
+
+    def requestRecording(self):
+        return True
+
+    def requestFPVMode(self):
+        return True
+
+    def requestLockMode(self):
+        return True
+
+    def requestFollowMode(self):
+        return True
+
+    def getAttitude(self):
+        return self.attitude
+
+    def getAttitudeSpeed(self):
+        return (0,0,0)
+
+    def getFirmwareVersion(self):
+        return (0,0,0)
+
+    def getHardwareID(self):
+        return 0
+
+    def getRecordingState(self):
+        return 0
+
+    def getMotionMode(self):
+        return 0
+
+    def getMountingDirection(self):
+        return 0
+
+    def getFunctionFeedback(self):
+        return 0
+
+    def getZoomLevel(self):
+        return self.zoom
+
+    def setAbsoluteZoom(self, level : float) -> bool:
+        self.zoom = level
+        return True

uavf_2024/imaging/camera_control.py

@@ -2,9 +2,8 @@
 from pathlib import Path
 import threading
 import time
-from siyi_sdk import SIYISTREAM,SIYISDK
+from siyi_sdk import MockSIYISTREAM as SIYISTREAM, MockSIYISDK as SIYISDK
 from uavf_2024.imaging.imaging_types import Image, HWC
-import matplotlib.image 
 from scipy.spatial.transform import Rotation
 import numpy as np
 import json
@@ -184,6 +183,7 @@ def _recording_worker(self):
 
             if self.logging:
                 self.threaded_logger.log_async(image, metadata, img_stamp)
+            time.sleep(0.1)
 
     def start_recording(self):
         if self.recording:

uavf_2024/imaging/perception.py

@@ -56,6 +56,7 @@ def __init__(self, pose_provider: PoseProvider, zoom_level: int = 3, logs_path:
         # Set up camera
         self.camera = Camera(LOGS_PATH / 'camera')
         self.camera.setAbsoluteZoom(zoom_level)
+        self.camera_state = False
 
         self.image_processor = ImageProcessor(LOGS_PATH / 'image_processor')