Merge pull request #229 from robotpy/examples

Add examples to repo, run tests in CI

Author: virtuald

.github/workflows/dist.yml

@@ -261,6 +261,10 @@ jobs:
       run: |
         python -m devtools ci install-test-pure-wheels
 
+    - name: Test examples
+      run: |
+        python -m devtools test-examples
+
     - name: Ensure all headers are accounted for
       run: |
         python -m devtools ci scan-headers

devtools/__main__.py

@@ -6,6 +6,7 @@
 
 from .ctx import Context
 from . import ci
+from . import examples
 from . import update_pyproject
 
 
@@ -34,6 +35,7 @@ def main(ctx: click.Context, verbose: bool):
 
 
 main.add_command(ci.ci)
+main.add_command(examples.test_examples)
 main.add_command(update_pyproject.update_pyproject)
 
 

devtools/examples.py

@@ -0,0 +1,105 @@
+import dataclasses
+import os
+import pathlib
+import subprocess
+import sys
+import typing as T
+
+import click
+import tomlkit
+
+from .util import parse_input, run_cmd
+
+
+def _validate_example_list(root: pathlib.Path, expected_dirs: T.Sequence[str]) -> None:
+    expected = sorted(f"{name}/robot.py" for name in expected_dirs)
+    actual = sorted(p.relative_to(root).as_posix() for p in root.rglob("robot.py"))
+
+    if expected == actual:
+        return
+
+    missing = sorted(set(expected) - set(actual))
+    extra = sorted(set(actual) - set(expected))
+    for path in missing:
+        print(f"Missing: {path}")
+    for path in extra:
+        print(f"Extra: {path}")
+
+    if not os.environ.get("FORCE_ANYWAYS"):
+        print("ERROR: Not every robot.py file is in the list of tests!")
+        sys.exit(1)
+
+
+@dataclasses.dataclass
+class ExamplesTests:
+    base: T.List[str]
+    ignored: T.List[str]
+
+
+@dataclasses.dataclass
+class ExamplesConfig:
+    tests: ExamplesTests
+
+
+def _load_tests_config(config_path: pathlib.Path) -> ExamplesConfig:
+    try:
+        data = tomlkit.parse(config_path.read_text(encoding="utf-8"))
+    except FileNotFoundError:
+        raise click.ClickException(f"Missing tests config: {config_path}")
+    except Exception as exc:
+        raise click.ClickException(f"Invalid tests config: {config_path}: {exc}")
+
+    try:
+        return parse_input(data, ExamplesConfig, config_path)
+    except Exception as exc:
+        raise click.ClickException(str(exc))
+
+
+@click.command(name="test-examples")
+@click.argument("test_name", required=False)
+@click.option("-x", "--exitfirst", is_flag=True, help="Exit on first failed test.")
+def test_examples(test_name: str | None, exitfirst: bool) -> None:
+    """Run tests on robot examples."""
+    root = pathlib.Path(__file__).parent.parent / "examples" / "robot"
+    config_path = root / "examples.toml"
+
+    cfg = _load_tests_config(config_path)
+    base_tests = cfg.tests.base
+    ignored_tests = cfg.tests.ignored
+
+    every_tests = [*base_tests, *ignored_tests]
+    _validate_example_list(root, every_tests)
+
+    tests_to_run = base_tests
+    if test_name:
+        if test_name not in every_tests:
+            raise click.BadParameter(f"unknown example {test_name}")
+        tests_to_run = [test_name]
+
+    failed_tests = []
+
+    for example_name in tests_to_run:
+        test_dir = root / example_name
+        print(test_dir.resolve())
+        try:
+            run_cmd(
+                sys.executable,
+                "-m",
+                "robotpy",
+                "test",
+                "--builtin",
+                cwd=test_dir,
+            )
+        except subprocess.CalledProcessError:
+            print(f"Test in {test_dir.resolve()} failed")
+            failed_tests.append(example_name)
+            if exitfirst:
+                break
+
+    if failed_tests:
+        print("Failed tests:")
+        for name in failed_tests:
+            print(f"- {name}")
+        sys.exit(1)
+
+    print("All tests successful!")

examples/robot/.gitignore

@@ -0,0 +1,22 @@
+*.py[ocd]
+__pycache__
+.coverage
+.cache
+
+.vscode/
+.deploy_cfg
+deploy.json
+
+.project
+.pydevproject
+
+pyproject.toml
+wpilib_preferences.json
+
+imgui.ini
+simgui*.json
+
+opkg_cache
+pip_cache
+
+networktables.*

examples/robot/AddressableLED/robot.py

@@ -0,0 +1,45 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+import wpimath.units
+
+
+class MyRobot(wpilib.TimedRobot):
+    def __init__(self) -> None:
+        super().__init__()
+
+        # SmartIO port 1
+        self.led = wpilib.AddressableLED(1)
+
+        # Reuse buffer
+        # Default to a length of 60
+        self.ledData = [wpilib.AddressableLED.LEDData() for _ in range(60)]
+        self.led.setLength(len(self.ledData))
+
+        # Set the data
+        self.led.setData(self.ledData)
+
+        # Create an LED pattern that will display a rainbow across
+        # all hues at maximum saturation and half brightness
+        self.rainbow = wpilib.LEDPattern.rainbow(255, 128)
+
+        # Our LED strip has a density of 120 LEDs per meter
+        self.kLedSpacing = 1 / 120.0
+
+        # Create a new pattern that scrolls the rainbow pattern across the LED strip, moving at a
+        # speed of 1 meter per second.
+        self.scrollingRainbow = self.rainbow.scrollAtAbsoluteSpeed(
+            1,
+            self.kLedSpacing,
+        )
+
+    def robotPeriodic(self) -> None:
+        # Update the buffer with the rainbow animation
+        self.scrollingRainbow.applyTo(self.ledData)
+        # Set the LEDs
+        self.led.setData(self.ledData)

examples/robot/AprilTagsVision/robot.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+
+import wpilib
+import wpilib.cameraserver
+
+
+class MyRobot(wpilib.TimedRobot):
+    """
+    This is a demo program showing the detection of AprilTags. The image is acquired from the USB
+    camera, then any detected AprilTags are marked up on the image and sent to the dashboard.
+    Be aware that the performance on this is much worse than a coprocessor solution!
+    """
+
+    def __init__(self):
+        super().__init__()
+        # Your image processing code will be launched via a stub that will set up logging and initialize NetworkTables
+        # to talk to your robot code.
+        # https://robotpy.readthedocs.io/en/stable/vision/roborio.html#important-notes
+
+        wpilib.CameraServer.launch("vision.py:main")

examples/robot/AprilTagsVision/vision.py

@@ -0,0 +1,152 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+
+import ntcore
+import robotpy_apriltag
+from cscore import CameraServer
+
+import cv2
+import numpy as np
+
+
+#
+# This code will work both on a RoboRIO and on other platforms. The exact mechanism
+# to run it differs depending on whether you’re on a RoboRIO or a coprocessor
+#
+# https://robotpy.readthedocs.io/en/stable/vision/code.html
+
+
+def main():
+    detector = robotpy_apriltag.AprilTagDetector()
+
+    # look for tag36h11, correct 1 error bit (hamming distance 1)
+    # hamming 1 allocates 781KB, 2 allocates 27.4 MB, 3 allocates 932 MB
+    # max of 1 recommended for RoboRIO 1, while hamming 2 is feasible on the RoboRIO 2
+    detector.addFamily("tag36h11", 1)
+
+    # Set up Pose Estimator - parameters are for a Microsoft Lifecam HD-3000
+    # (https://www.chiefdelphi.com/t/wpilib-apriltagdetector-sample-code/421411/21)
+    poseEstConfig = robotpy_apriltag.AprilTagPoseEstimator.Config(
+        0.1651,
+        699.3778103158814,
+        677.7161226393544,
+        345.6059345433618,
+        207.12741326228522,
+    )
+    estimator = robotpy_apriltag.AprilTagPoseEstimator(poseEstConfig)
+
+    # Get the UsbCamera from CameraServer
+    camera = CameraServer.startAutomaticCapture()
+
+    # Set the resolution
+    camera.setResolution(640, 480)
+
+    # Get a CvSink. This will capture Mats from the camera
+    cvSink = CameraServer.getVideo()
+
+    # Set up a CvSource. This will send images back to the Dashboard
+    outputStream = CameraServer.putVideo("Detected", 640, 480)
+
+    # Mats are very memory expensive. Let's reuse these.
+    mat = np.zeros((480, 640, 3), dtype=np.uint8)
+    grayMat = np.zeros(shape=(480, 640), dtype=np.uint8)
+
+    # Instantiate once
+    tags = []
+    outlineColor = (0, 255, 0)
+    crossColor = (0, 0, 255)
+
+    # Output the list to Network Tables
+    tagsTable = ntcore.NetworkTableInstance.getDefault().getTable("apriltags")
+    pubTags = tagsTable.getIntegerArrayTopic("tags").publish()
+
+    try:
+        while True:
+            # Tell the CvSink to grab a frame from the camera and put it
+            # in the source mat.  If there is an error notify the output.
+            if cvSink.grabFrame(mat) == 0:
+                # Send the output frame the error
+                outputStream.notifyError(cvSink.getError())
+
+                # Skip the rest of the current iteration
+                continue
+
+            cv2.cvtColor(mat, cv2.COLOR_RGB2GRAY, dst=grayMat)
+
+            detections = detector.detect(grayMat)
+
+            # have not seen any tags yet
+            tags.clear()
+
+            for detection in detections:
+                # remember we saw this tag
+                tags.append(detection.getId())
+
+                # draw lines around the tag
+                for i in range(4):
+                    j = (i + 1) % 4
+                    point1 = (
+                        int(detection.getCorner(i).x),
+                        int(detection.getCorner(i).y),
+                    )
+                    point2 = (
+                        int(detection.getCorner(j).x),
+                        int(detection.getCorner(j).y),
+                    )
+                    mat = cv2.line(mat, point1, point2, outlineColor, 2)
+
+                # mark the center of the tag
+                cx = int(detection.getCenter().x)
+                cy = int(detection.getCenter().y)
+                ll = 10
+                mat = cv2.line(
+                    mat,
+                    (cx - ll, cy),
+                    (cx + ll, cy),
+                    crossColor,
+                    2,
+                )
+                mat = cv2.line(
+                    mat,
+                    (cx, cy - ll),
+                    (cx, cy + ll),
+                    crossColor,
+                    2,
+                )
+
+                # identify the tag
+                mat = cv2.putText(
+                    mat,
+                    str(detection.getId()),
+                    (cx + ll, cy),
+                    cv2.FONT_HERSHEY_SIMPLEX,
+                    1,
+                    crossColor,
+                    3,
+                )
+
+                # determine pose
+                pose = estimator.estimate(detection)
+
+                # put pose into dashboard
+                rot = pose.rotation()
+                tagsTable.getEntry(f"pose_{detection.getId()}").setDoubleArray(
+                    [pose.X(), pose.Y(), pose.Z(), rot.X(), rot.Y(), rot.Z()]
+                )
+
+            # put list of tags onto dashboard
+            pubTags.set(tags)
+
+            # Give output stream a new image to display
+            outputStream.putFrame(mat)
+    finally:
+        pubTags.close()
+        detector.close()
+
+    # The camera code will be killed when the robot.py program exits. If you wish to perform cleanup,
+    # you should register an atexit handler. The child process will NOT be launched when running the robot code in
+    # simulation or unit testing mode