Stathi game piece detection

.vscode/settings.json

@@ -0,0 +1,5 @@
+{
+	"python.testing.pytestArgs": ["."],
+	"python.testing.unittestEnabled": false,
+	"python.testing.pytestEnabled": true
+}

camera_config.py

@@ -18,3 +18,9 @@ def get_horizontal_fov(self) -> float:
 
     def get_vertical_fov(self) -> float:
         return 2 * atan2(self.height / 2, self.K[1, 1])
+
+    def get_fx(self) -> float:
+        return self.K[0, 0]
+
+    def get_fy(self) -> float:
+        return self.K[1, 1]

camera_manager.py

@@ -2,11 +2,35 @@
 import cv2
 from camera_config import CameraParams
 import sys
+from abc import ABC, abstractmethod
 
 
-class CameraManager:
+class BaseCameraManager(ABC):
+    @abstractmethod
+    def get_params(self) -> CameraParams:
+        ...
+
+    @abstractmethod
+    def get_frame(self) -> tuple[int, np.ndarray]:
+        ...
+
+    @abstractmethod
+    def send_frame(self, frame: np.ndarray) -> None:
+        ...
+
+    @abstractmethod
+    def get_error(self) -> str:
+        ...
+
+    @abstractmethod
+    def notify_error(self, error: str) -> None:
+        ...
+
+
+class CameraManager(BaseCameraManager):
     def __init__(
         self,
+        camera_id: int,
         path: str,
         params: CameraParams,
         pixel_format: str,
@@ -20,9 +44,11 @@ def __init__(
         """
         from cscore import CameraServer, VideoMode
 
-        self.cs = CameraServer.getInstance()
+        self.camera_id = camera_id
 
-        self.camera = self.cs.startAutomaticCapture(name=params.name, path=path)
+        self.params = params
+
+        self.camera = CameraServer.startAutomaticCapture(name=params.name, path=path)
         self.camera.setVideoMode(
             getattr(VideoMode.PixelFormat, pixel_format),
             params.width,
@@ -33,8 +59,8 @@ def __init__(
         # In this, source and sink are inverted from the cscore documentation.
         # self.sink is a CvSource and self.sources are CvSinks. This is because it makes more sense for a reader.
         # We get images from a source, and put images to a sink.
-        self.source = self.cs.getVideo(camera=self.camera)
-        self.sink = self.cs.putVideo("Driver_Stream", params.width, params.height)
+        self.source = CameraServer.getVideo(camera=self.camera)
+        self.sink = CameraServer.putVideo("Driver_Stream", params.width, params.height)
         # Width and Height are reversed here because the order of putVideo's width and height
         # parameters are the opposite of numpy's (technically it is an array, not an actual image).
         self.frame = np.zeros(shape=(params.height, params.width, 3), dtype=np.uint8)
@@ -43,7 +69,7 @@ def __init__(
         self.set_camera_property("exposure_auto_priority", 0)
         self.set_camera_property("exposure_auto", 1)
         self.set_camera_property("focus_auto", 0)
-        self.set_camera_property("exposure_absolute", 1)
+        self.set_camera_property("exposure_absolute", 10)
         self.set_camera_property("raw_contrast", 255)
         self.set_camera_property("contrast", 100)
         self.set_camera_property("raw_saturation", 255)
@@ -54,6 +80,20 @@ def __init__(
         self.set_camera_property("brightness", 50)
         self.set_camera_property("raw_brightness", 127)
 
+    def get_id(self) -> int:
+        """Get the unique camera id
+        Returns:
+            int
+        """
+        return self.camera_id
+
+    def get_params(self) -> CameraParams:
+        """Gets the camera parameters of the associated camera
+        Returns:
+            CameraParams
+        """
+        return self.params
+
     def get_frame(self) -> tuple[int, np.ndarray]:
         """Gets a frame from the camera.
         Returns:
@@ -86,18 +126,21 @@ def notify_error(self, error: str) -> None:
         print(error, file=sys.stderr)
         self.sink.notifyError(error)
 
-    def set_camera_property(self, property, value) -> None:
+    def set_camera_property(self, property: str, value: int) -> None:
         self.camera.getProperty(property).set(value)
 
 
-class MockImageManager:
-    def __init__(self, image: np.ndarray, display_output: bool = False) -> None:
+class MockImageManager(BaseCameraManager):
+    def __init__(
+        self, image: np.ndarray, params: CameraParams, display_output: bool = False
+    ) -> None:
         """Initialises a Mock Image Manager
         Args:
             image: A BGR numpy image array
         """
         self.image = image
         self.display_output = display_output
+        self.params = params
 
     def change_image(self, new_image: np.ndarray) -> None:
         """Changes self.image.
@@ -129,11 +172,11 @@ def notify_error(self, error: str) -> None:
         """
         print(error, file=sys.stderr)
 
-    def set_camera_property(self, property, value) -> None:
-        pass
+    def get_params(self) -> CameraParams:
+        return self.params
 
 
-class MockVideoManager:
+class MockVideoManager(BaseCameraManager):
     def __init__(self, video: cv2.VideoCapture, display_output: bool = False):
         """Initialises a Mock Video Manager.
         Args:
@@ -170,19 +213,17 @@ def notify_error(self, error: str) -> None:
         """
         print(error, file=sys.stderr)
 
-    def set_camera_property(self, property, value) -> None:
-        pass
-
 
-class WebcamCameraManager:
-    def __init__(self, camera: int = 0) -> None:
+class WebcamCameraManager(BaseCameraManager):
+    def __init__(self, camera: int, params: CameraParams) -> None:
         """Initialises a Webcam Camera Manager. Designed to run on a non-pi computer.
         Initialises it with the first detected system camera, for example a webcam.
 
         Args:
             camera: Which camera to use. Default is 0th, probably a builtin webcam for most people.
         """
         self.video = cv2.VideoCapture(camera)
+        self.params = params
 
     def get_frame(self) -> tuple[int, np.ndarray]:
         """Returns the current video frame.
@@ -206,5 +247,5 @@ def notify_error(self, error: str) -> None:
         """
         print(error, file=sys.stderr)
 
-    def set_camera_property(self, property, value) -> None:
-        pass
+    def get_params(self) -> CameraParams:
+        return self.params

connection.py

@@ -1,62 +1,127 @@
+import math
 import time
+from wpimath.geometry import Pose2d
+from networktables import NetworkTablesInstance, NetworkTables
+from abc import ABC, abstractmethod
 
-from networktables import NetworkTablesInstance
 
-NetworkTables = NetworkTablesInstance.getDefault()
+RIO_IP = {True: "127.0.0.1", False: "10.47.74.2"}
 
-RIO_IP = "10.47.74.2"
-UDP_RECV_PORT = 5005
-UDP_SEND_PORT = 5006
 
-Results = tuple[float, float, float]
+def nt_data_to_node_data(self, data: list[str]) -> list[tuple[int, bool]]:
+    nodes: list[tuple[int, bool]] = []
+    for node in data:
+        as_array = str(node)
+        a = (int(f"{as_array[0]}{as_array[1]}"), as_array[2] == "1")
+        nodes.append(a)
+    return nodes
 
 
-class NTConnection:
-    def __init__(self, inst: NetworkTablesInstance = NetworkTables) -> None:
-        inst.initialize(server=RIO_IP)
+class BaseConnection(ABC):
+    @abstractmethod
+    def send_results(self, positive: list[int], negatives: list[int]) -> None:
+        ...
+
+    @abstractmethod
+    def get_latest_pose(self) -> Pose2d:
+        ...
+
+    @abstractmethod
+    def get_debug(self) -> bool:
+        ...
+
+    @abstractmethod
+    def set_nodes(self, value: list[str]) -> None:
+        ...
+
+
+class NTConnection(BaseConnection):
+    inst: NetworkTablesInstance
+
+    def __init__(
+        self, name: str, inst: NetworkTablesInstance = NetworkTables, sim: bool = False
+    ) -> None:
+        inst.initialize(server=RIO_IP[sim])
         self.inst = inst
 
-        nt = inst.getTable("/vision")
-        self.entry = nt.getEntry("data")
-        self.ping = nt.getEntry("ping")
-        self.raspi_pong = nt.getEntry("raspi_pong")
-        self.rio_pong = nt.getEntry("rio_pong")
+        nt = self.inst.getTable(name)
+        self.nodes_entry = nt.getEntry("nodes")
+        self.true_entry = nt.getEntry("results_true")
+        self.false_entry = nt.getEntry("results_false")
+        self.timestamp_entry = nt.getEntry("timestamp")
         self.fps_entry = nt.getEntry("fps")
+        # wether to stream an annotated image back
+        self.debug_entry = nt.getEntry("debug_stream")
+        self.debug_entry.setBoolean(False)
 
-        self.old_fps_time = 0.0
+        pose_table = self.inst.getTable("SmartDashboard").getSubTable("Field")
+        self.pose_entry = pose_table.getEntry("fused_pose")
 
-        self.last_ping_time = 0.0
-        self.time_to_pong = 0.00000001
+        self.old_fps_time = 0.0
         self._get_time = time.monotonic
 
-    def send_results(self, results: Results) -> None:
-        self.entry.setDoubleArray(results)
+    def send_results(self, positives: list[int], negatives: list[int]) -> None:
+        # self.true_entry.setIntegerArray(positives)
+        # self.false_entry.setIntegerArray(negatives)
+
+        current_time = self._get_time()
+        self.timestamp_entry.setDouble(current_time)
+        fps = 1 / (current_time - self.old_fps_time)
+        self.old_fps_time = current_time
+        self.fps_entry.setDouble(fps)
+
         self.inst.flush()
 
-    def pong(self) -> None:
-        self.ping_time = self.ping.getNumber(0)
-        if abs(self.ping_time - self.last_ping_time) > self.time_to_pong:
-            self.rio_pong.setNumber(self.ping_time)
-            self.raspi_pong.setNumber(self._get_time())
-            self.last_ping_time = self.ping_time
+    def set_nodes(self, value: list[str]) -> None:
+        self.nodes_entry.setStringArray(value)
 
-    def set_fps(self) -> None:
-        current_time = time.monotonic()
-        fps = 1 / (current_time - self.old_fps_time)
+        current_time = self._get_time()
+        self.timestamp_entry.setDouble(current_time)
+        fps = 1 / (
+            (current_time - self.old_fps_time)
+            if (current_time - self.old_fps_time) != 0
+            else 1
+        )
         self.old_fps_time = current_time
         self.fps_entry.setDouble(fps)
 
+        self.inst.flush()
+
+    def get_latest_pose(self) -> Pose2d:
+        arr: list[float] = self.pose_entry.getDoubleArray([0, 0, 0])  # type: ignore
+        print(arr)
+        return Pose2d(arr[0], arr[1], math.radians(arr[2]))
+
+    def get_debug(self) -> bool:
+        # return self.debug_entry.getBoolean(True)
+        return True
+
+
+zero_pose = Pose2d()
+
+
+class DummyConnection(BaseConnection):
+    def __init__(self, pose=zero_pose, do_print=False, do_annotate=False):
+        self.pose = pose
+        self.debug = do_annotate
+        self.results = [[], []]
+        self.string_array = []
 
-class DummyConnection:
-    def __init__(self):
-        self.results = None
+    def send_results(self, positives: list[int], negatives: list[int]) -> None:
+        self.results = [positives, negatives]
+        print(
+            "results being sent, positive sightings:",
+            positives,
+            ", negative signtings:",
+            negatives,
+        )
 
-    def send_results(self, results: Results) -> None:
-        print("results being sent", results)
-        self.results = results
+    def get_latest_pose(self) -> Pose2d:
+        return self.pose
 
-    def pong(self) -> None:
-        pass
+    def get_debug(self) -> bool:
+        return self.debug
 
-    def set_fps(self) -> None:
-        pass
+    def set_nodes(self, value: list[str]) -> None:
+        self.string_array = value
+        print(f"Setting nodes to {value}")