Merge pull request #1 from Robocubs/always-send-targets

Merge 'Always send targets'. Use as default backend strategy for tracking

.gitignore

@@ -1,3 +1,7 @@
+#OpenCV build artifacts
+docker/cv2/
+docker/lib/
+
 # Byte-compiled / optimized / DLL files
 __pycache__/
 *.py[cod]

calibration/CalibrationSession.py

@@ -37,9 +37,12 @@ def process_frame(self, image: cv2.Mat, save: bool) -> None:
 
                 # Save corners
                 if save:
-                    self._all_charuco_corners.append(charuco_corners)
-                    self._all_charuco_ids.append(charuco_ids)
-                    print("Saved calibration frame")
+                    if charuco_ids.size > 4:
+                        self._all_charuco_corners.append(charuco_corners)
+                        self._all_charuco_ids.append(charuco_ids)
+                        print("Saved calibration frame")
+                    else:
+                        print("Bad frame: not enough ID's captured")
 
     def finish(self) -> None:
         if len(self._all_charuco_corners) == 0:

config/ConfigSource.py

@@ -67,8 +67,11 @@ def update(self, config_store: ConfigStore) -> None:
                 "camera_gain").subscribe(RemoteConfig.camera_gain)
             self._fiducial_size_m_sub = nt_table.getDoubleTopic(
                 "fiducial_size_m").subscribe(RemoteConfig.fiducial_size_m)
-            self._tag_layout_sub = nt_table.getStringTopic(
+
+            global_config_table = ntcore.NetworkTableInstance.getDefault().getTable("CubStar/config")
+            self._tag_layout_sub = global_config_table.getStringTopic(
                 "tag_layout").subscribe("")
+
             self._init_complete = True
 
         # Read config data

docker/Dockerfile

@@ -0,0 +1,40 @@
+FROM ubuntu:22.04
+
+RUN mkdir -p /usr/src/app
+WORKDIR /usr/src/app 
+
+# No interaction when installing packages
+ARG DEBIAN_FRONTEND=noninteractive
+
+# Note: looks like the OPi's are running 3.10.6, but the subversion for default python in ubuntu 22.04 is 3.10.12
+# python3.11 isn't needed here, but I'm scared to change stuff
+# Install minimal prerequisites
+RUN apt-get update && apt-get install -y cmake g++ wget unzip build-essential apt-utils git python3.11 python3-dev python3-numpy openssh-server
+RUN apt-get install -y libavcodec-dev libavformat-dev libswscale-dev libgstreamer-plugins-base1.0-dev libgstreamer1.0-dev libgtk-3-dev
+
+# Kick off SSH server if stuff breaks
+RUN sed -i 's/PermitRootLogin prohibit-password/PermitRootLogin yes/' /etc/ssh/sshd_config
+RUN useradd -m -s /bin/bash user
+RUN echo "user:1701robocubs" | chpasswd
+EXPOSE 22
+ENTRYPOINT service ssh start && bash
+
+# Download and unpack sources
+RUN wget -O opencv.zip https://github.com/opencv/opencv/archive/4.x.zip
+RUN wget -O opencv_contrib.zip https://github.com/opencv/opencv_contrib/archive/4.x.zip
+RUN unzip opencv.zip
+RUN unzip opencv_contrib.zip
+
+# Make CMAKE build dir
+RUN mkdir -p build
+WORKDIR /usr/src/app/build
+
+# Configure
+RUN cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DWITH_GSTREAMER=ON -DOPENCV_EXTRA_MODULES_PATH=../opencv_contrib-4.x/modules ../opencv-4.x
+
+# Build
+RUN cmake --build .
+
+# Do standard build and install so it's easier to grab the necessary python module
+RUN make 
+RUN make install

docker/build_opencv.sh

@@ -0,0 +1,23 @@
+# Start by building the docker image
+docker build --tag 'opencv_build' .
+
+# Run the container and grab the id
+CONTAINER_ID=$(docker run --detach 'opencv_build')
+
+# Clean any previous dirs
+rm -r ./cv2
+rm -r ./lib
+
+# Copy the python package directory
+docker cp "$CONTAINER_ID":/usr/local/lib/python3.10/dist-packages/cv2/ .
+
+# Copy the built shared objects
+"""
+WARNING: This copies EVERYTHING, including the shared objects. Since we're in a docker container, 
+there really shouldn't be anything else installed here.
+"""
+docker cp "$CONTAINER_ID":/usr/local/lib/ .
+
+# TODO: Get these automatically implemented, do not copy dirs in lib though
+#scp -r ./lib/ [email protected]:/usr/local/lib 
+#scp -r ./cv2 [email protected]:/usr/local/lib/python3.10/dist-packages/

main.py

@@ -10,7 +10,7 @@
 from calibration.CalibrationSession import CalibrationSession
 from config.config import ConfigStore, LocalConfig, RemoteConfig
 from config.ConfigSource import ConfigSource, FileConfigSource, NTConfigSource
-from output.OutputPublisher import NTOutputPublisher, OutputPublisher
+from output.OutputPublisher import NTPacketPublisher, OutputPublisher
 from output.overlay_util import *
 from output.StreamServer import MjpegServer
 from pipeline.CameraPoseEstimator import MultiTargetCameraPoseEstimator
@@ -30,7 +30,7 @@
     fiducial_detector = ArucoFiducialDetector(cv2.aruco.DICT_APRILTAG_36h11)
     camera_pose_estimator = MultiTargetCameraPoseEstimator()
     tag_pose_estimator = SquareTargetPoseEstimator()
-    output_publisher: OutputPublisher = NTOutputPublisher()
+    output_publisher: OutputPublisher = NTPacketPublisher()
     stream_server = MjpegServer()
     calibration_session = CalibrationSession()
 
@@ -77,15 +77,15 @@
             # Normal mode
             image_observations = fiducial_detector.detect_fiducials(image, config)
             [overlay_image_observation(image, x) for x in image_observations]
-            camera_pose_observation = camera_pose_estimator.solve_camera_pose(
+            fiducial_pose_observations, camera_pose_observation = camera_pose_estimator.solve_camera_pose(
                 [x for x in image_observations if x.tag_id != DEMO_ID], config)
             demo_image_observations = [x for x in image_observations if x.tag_id == DEMO_ID]
             demo_pose_observation: Union[FiducialPoseObservation, None] = None
             if len(demo_image_observations) > 0:
                 demo_pose_observation = tag_pose_estimator.solve_fiducial_pose(demo_image_observations[0], config)
-            latency = int((time.time() - frame_capture_time) * 1000)
+            latency = (time.time() - frame_capture_time) * 1000
             latency_sum += latency
-            output_publisher.send(config, timestamp, camera_pose_observation, demo_pose_observation, fps, latency)
+            output_publisher.send(config, timestamp, fiducial_pose_observations, camera_pose_observation, demo_pose_observation, fps, latency)
 
         else:
             # No calibration

output/OutputPublisher.py

@@ -1,23 +1,93 @@
 import math
+import time
 from typing import List, Union
 
 import ntcore
 from config.config import ConfigStore
+from output.Packet import Packet
 from vision_types import CameraPoseObservation, FiducialPoseObservation
 
 
 class OutputPublisher:
-    def send(self, config_store: ConfigStore, timestamp: float, observation: Union[CameraPoseObservation, None], demo_observation: Union[FiducialPoseObservation, None], fps: Union[int, None]) -> None:
+    def send(self, config_store: ConfigStore, timestamp: float, fiducial_pose_observations: List[FiducialPoseObservation], observation: Union[CameraPoseObservation, None], demo_observation: Union[FiducialPoseObservation, None], fps: Union[int, None], latency: float) -> None:
         raise NotImplementedError
 
 
+class NTPacketPublisher():
+    _init_complete: bool = False
+    _observations_pub: ntcore.RawPublisher
+    _demo_observations_pub: ntcore.RawPublisher
+    _fps_pub: ntcore.IntegerPublisher
+    _heartbeat_pub: ntcore.IntegerPublisher
+
+    def send(self, config_store: ConfigStore, timestamp: float, fiducial_pose_observations: List[FiducialPoseObservation], observation: Union[CameraPoseObservation, None], demo_observation: Union[FiducialPoseObservation, None], fps: Union[int, None], latency: float) -> None:
+        # Initialize publishers on first call
+        if not self._init_complete:
+            nt_table = ntcore.NetworkTableInstance.getDefault().getTable(
+                "CubStar/" + config_store.local_config.device_id + "/output")
+            nt_table = ntcore.NetworkTableInstance.getDefault().getTable(
+                "CubStar/" + config_store.local_config.device_id + "/output")
+            self._observations_pub = nt_table.getRawTopic("observations").publish("ObservationsPacket",
+                ntcore.PubSubOptions(periodic=0, sendAll=True, keepDuplicates=True))
+            self._demo_observations_pub = nt_table.getRawTopic("demo_observations").publish("FiducialPoseObservation",
+                ntcore.PubSubOptions(periodic=0, sendAll=True, keepDuplicates=True))
+            self._fps_pub = nt_table.getIntegerTopic("fps").publish()
+            self._heartbeat_pub = nt_table.getIntegerTopic("heartbeat").publish()
+
+        # Build the observation
+        observation_packet = Packet()
+
+        observation_packet.encodeDouble(latency)
+
+        observation_packet.encode8(len(fiducial_pose_observations))
+        for fiducial_observation in fiducial_pose_observations:
+            observation_packet.encode8(fiducial_observation.tag_id)
+            if fiducial_observation.error_0 < fiducial_observation.error_1:
+                observation_packet.encodeDouble(fiducial_observation.error_0)
+                observation_packet.encodeTransform(fiducial_observation.pose_0)
+                observation_packet.encodeDouble(fiducial_observation.error_1)
+                observation_packet.encodeTransform(fiducial_observation.pose_1)
+            else:
+                observation_packet.encodeDouble(fiducial_observation.error_1)
+                observation_packet.encodeTransform(fiducial_observation.pose_1)
+                observation_packet.encodeDouble(fiducial_observation.error_0)
+                observation_packet.encodeTransform(fiducial_observation.pose_0)
+
+        if observation == None:
+            observation_packet.encode8(0)
+        else:
+            observation_packet.encode8(len(observation.tag_ids))
+            for tag_id in observation.tag_ids:
+                observation_packet.encode8(tag_id)
+            observation_packet.encodeDouble(observation.error_0)
+            observation_packet.encodeTransform(observation.pose_0)
+            if observation.error_1 != None and observation.pose_1 != None:
+                observation_packet.encodeDouble(observation.error_1)
+                observation_packet.encodeTransform(observation.pose_1)
+
+        # Build demo observation
+        demo_observation_packet = Packet()
+        if demo_observation != None:
+            demo_observation_packet.encode8(demo_observation.tag_id)
+            demo_observation_packet.encodeDouble(demo_observation.error_0)
+            demo_observation_packet.encodeTransform(demo_observation.pose_0)
+            demo_observation_packet.encodeDouble(demo_observation.error_1)
+            demo_observation_packet.encodeTransform(demo_observation.pose_1)
+
+        # Send data
+        self._observations_pub.set(bytes(observation_packet.getData()))
+        self._demo_observations_pub.set(bytes(demo_observation_packet.getData()), math.floor(timestamp * 1000000))
+        if fps != None:
+            self._fps_pub.set(fps)
+
+
 class NTOutputPublisher(OutputPublisher):
     _init_complete: bool = False
     _observations_pub: ntcore.DoubleArrayPublisher
-    _observations_pub: ntcore.DoubleArrayPublisher
+    _demo_observations_pub: ntcore.DoubleArrayPublisher
     _fps_pub: ntcore.IntegerPublisher
 
-    def send(self, config_store: ConfigStore, timestamp: float, observation: Union[CameraPoseObservation, None], demo_observation: Union[FiducialPoseObservation, None], fps: Union[int, None], latency: int) -> None:
+    def send(self, config_store: ConfigStore, timestamp: float, fiducial_pose_observations: Union[FiducialPoseObservation, None], observation: Union[CameraPoseObservation, None], demo_observation: Union[FiducialPoseObservation, None], fps: Union[int, None], latency: float) -> None:
         # Initialize publishers on first call
         if not self._init_complete:
             nt_table = ntcore.NetworkTableInstance.getDefault().getTable(

output/Packet.py

@@ -0,0 +1,45 @@
+from typing import Any
+from wpimath.geometry import Transform3d
+import struct
+
+class Packet:
+    def __init__(self):
+        self._data: list[int] = []
+
+    def getData(self) -> list[int]:
+        return self._data
+
+    def getSize(self) -> int:
+        return len(self._data)
+
+    def _encodeGeneric(self, packFormat: str, *value: Any) -> None:
+        self._data.extend(struct.pack(packFormat, *value))
+
+    def encode8(self, value: int) -> None:
+        self._encodeGeneric('>b', value)
+        return self
+
+    def encode32(self, value: int) -> None:
+        self._encodeGeneric('>l', value)
+        return self
+
+    def encodeDouble(self, value: float) -> None:
+        self._encodeGeneric('>d', value)
+        return self
+
+    def encodeBoolean(self, value: bool) -> None:
+        self.encode8(1 if value else 0)
+        return self
+
+    def encodeTransform(self, value: Transform3d) -> None:
+        self.encodeDouble(value.X())
+        self.encodeDouble(value.Y())
+        self.encodeDouble(value.Z())
+
+        quaternion = value.rotation().getQuaternion()
+        self.encodeDouble(quaternion.W())
+        self.encodeDouble(quaternion.X())
+        self.encodeDouble(quaternion.Y())
+        self.encodeDouble(quaternion.Z())
+
+        return self

output/StreamServer.py

@@ -27,7 +27,7 @@ class MjpegServer(StreamServer):
     _frame: cv2.Mat
     _has_frame: bool = False
     _fps: float = 0.0
-    _latency: int = 0
+    _latency: float = 0.0
     _last_served_timestamp: float = 0.0
 
     def _make_handler(self_mjpeg):  # type: ignore
@@ -80,7 +80,7 @@ def do_GET(self):
                                 timestamp = time.time()
                                 frame = self_mjpeg._frame.copy()
                                 cv2.putText(frame, f"FPS: {self_mjpeg._fps:.2f}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
-                                cv2.putText(frame, f"Latency: {self_mjpeg._latency}ms", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
+                                cv2.putText(frame, f"Latency: {self_mjpeg._latency:.0f}ms", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
                                 cv2.putText(frame, f"Stream FPS: {(1 / (timestamp - self_mjpeg._last_served_timestamp)):.2f}", (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
                                 self_mjpeg._last_served_timestamp = timestamp
 
@@ -115,7 +115,7 @@ def _run(self, port: int) -> None:
     def start(self, config_store: ConfigStore) -> None:
         threading.Thread(target=self._run, daemon=True, args=(config_store.local_config.stream_port,)).start()
 
-    def set_frame(self, frame: cv2.Mat, fps: Union[float, None], latency: Union[int, None]) -> None:
+    def set_frame(self, frame: cv2.Mat, fps: Union[float, None], latency: Union[float, None]) -> None:
         if fps != None:
             self._fps = fps
         if latency != None: