From b85ca78fa58e50a9f19da8e8cc64ba380393834e Mon Sep 17 00:00:00 2001 From: Eric Pedley Date: Mon, 24 Jun 2024 00:30:03 -0700 Subject: [PATCH] Add mocks for siyi sdk and script to test perception class. (#230) --- README.md | 4 + scripts/camera_test_node.py | 19 ++-- scripts/run_perception.py | 64 +++++++++++++ siyi_sdk/siyi_sdk/__init__.py | 3 +- siyi_sdk/siyi_sdk/mocks.py | 139 ++++++++++++++++++++++++++++ uavf_2024/imaging/camera_control.py | 4 +- uavf_2024/imaging/perception.py | 1 + 7 files changed, 218 insertions(+), 16 deletions(-) create mode 100644 scripts/run_perception.py create mode 100644 siyi_sdk/siyi_sdk/mocks.py diff --git a/README.md b/README.md index 3f566359..6b3a368f 100644 --- a/README.md +++ b/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 diff --git a/scripts/camera_test_node.py b/scripts/camera_test_node.py index 2636903a..66a148ec 100644 --- a/scripts/camera_test_node.py +++ b/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: diff --git a/scripts/run_perception.py b/scripts/run_perception.py new file mode 100644 index 00000000..cfc6a2ac --- /dev/null +++ b/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() \ No newline at end of file diff --git a/siyi_sdk/siyi_sdk/__init__.py b/siyi_sdk/siyi_sdk/__init__.py index 9febf157..4822eb6c 100644 --- a/siyi_sdk/siyi_sdk/__init__.py +++ b/siyi_sdk/siyi_sdk/__init__.py @@ -1,2 +1,3 @@ from .siyi_sdk import SIYISDK -from .siyi_stream import SIYISTREAM \ No newline at end of file +from .siyi_stream import SIYISTREAM +from .mocks import MockSIYISDK, MockSIYISTREAM \ No newline at end of file diff --git a/siyi_sdk/siyi_sdk/mocks.py b/siyi_sdk/siyi_sdk/mocks.py new file mode 100644 index 00000000..cde225a4 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/uavf_2024/imaging/camera_control.py b/uavf_2024/imaging/camera_control.py index a7e89b36..4a7d9260 100644 --- a/uavf_2024/imaging/camera_control.py +++ b/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: diff --git a/uavf_2024/imaging/perception.py b/uavf_2024/imaging/perception.py index 83ceb29e..fc1068a0 100644 --- a/uavf_2024/imaging/perception.py +++ b/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')