From cf1a411acf3c494e556f325001c1d451e360aba7 Mon Sep 17 00:00:00 2001 From: Doug Wegscheid <5349196+fovea1959@users.noreply.github.com> Date: Sat, 14 Jan 2023 02:08:45 -0500 Subject: [PATCH] [examples] Add example programs for AprilTags detection (#4932) Co-authored-by: Peter Johnson --- wpilibcExamples/.styleguide | 1 + .../examples/AprilTagsVision/cpp/Robot.cpp | 165 ++++++++++++++++++ .../examples/I2CCommunication/cpp/Robot.cpp | 1 - .../src/main/cpp/examples/examples.json | 11 ++ .../main/cpp/templates/timed/cpp/Robot.cpp | 1 - .../AddressableLED/cpp/RainbowTest.cpp | 2 +- .../examples/apriltagsvision/Main.java | 25 +++ .../examples/apriltagsvision/Robot.java | 147 ++++++++++++++++ .../wpi/first/wpilibj/examples/examples.json | 12 ++ 9 files changed, 362 insertions(+), 3 deletions(-) create mode 100644 wpilibcExamples/src/main/cpp/examples/AprilTagsVision/cpp/Robot.cpp create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Main.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Robot.java diff --git a/wpilibcExamples/.styleguide b/wpilibcExamples/.styleguide index 38c02ccb5ca..677fedf91d3 100644 --- a/wpilibcExamples/.styleguide +++ b/wpilibcExamples/.styleguide @@ -11,6 +11,7 @@ cppSrcFileInclude { includeOtherLibs { ^cameraserver/ ^cscore + ^fmt/ ^frc/ ^frc2/ ^hal/ diff --git a/wpilibcExamples/src/main/cpp/examples/AprilTagsVision/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/AprilTagsVision/cpp/Robot.cpp new file mode 100644 index 00000000000..0ed53fd8536 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/AprilTagsVision/cpp/Robot.cpp @@ -0,0 +1,165 @@ +// 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. + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * 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! + */ +class Robot : public frc::TimedRobot { +#if defined(__linux__) || defined(_WIN32) + + private: + static void VisionThread() { + frc::AprilTagDetector detector; + // look for tag16h5, don't correct any error bits + detector.AddFamily("tag16h5", 0); + + // Set up Pose Estimator - parameters are for a Microsoft Lifecam HD-3000 + // (https://www.chiefdelphi.com/t/wpilib-apriltagdetector-sample-code/421411/21) + frc::AprilTagPoseEstimator::Config poseEstConfig = { + .tagSize = units::length::inch_t(6.0), + .fx = 699.3778103158814, + .fy = 677.7161226393544, + .cx = 345.6059345433618, + .cy = 207.12741326228522}; + frc::AprilTagPoseEstimator estimator(poseEstConfig); + + // Get the USB camera from CameraServer + cs::UsbCamera camera = frc::CameraServer::StartAutomaticCapture(); + // Set the resolution + camera.SetResolution(640, 480); + + // Get a CvSink. This will capture Mats from the Camera + cs::CvSink cvSink = frc::CameraServer::GetVideo(); + // Setup a CvSource. This will send images back to the Dashboard + cs::CvSource outputStream = + frc::CameraServer::PutVideo("Detected", 640, 480); + + // Mats are very memory expensive. Lets reuse this Mat. + cv::Mat mat; + cv::Mat grayMat; + + // Instantiate once + std::vector tags; + cv::Scalar outlineColor{0, 255, 0}; + cv::Scalar crossColor{0, 0, 255}; + + // We'll output to NT + auto tagsTable = + nt::NetworkTableInstance::GetDefault().GetTable("apriltags"); + auto pubTags = tagsTable->GetIntegerArrayTopic("tags").Publish(); + + 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 the error. + outputStream.NotifyError(cvSink.GetError()); + // skip the rest of the current iteration + continue; + } + + cv::cvtColor(mat, grayMat, cv::COLOR_BGR2GRAY); + + cv::Size g_size = grayMat.size(); + frc::AprilTagDetector::Results detections = + detector.Detect(g_size.width, g_size.height, grayMat.data); + + // have not seen any tags yet + tags.clear(); + + for (const frc::AprilTagDetection* detection : detections) { + // remember we saw this tag + tags.push_back(detection->GetId()); + + // draw lines around the tag + for (int i = 0; i <= 3; i++) { + int j = (i + 1) % 4; + const frc::AprilTagDetection::Point pti = detection->GetCorner(i); + const frc::AprilTagDetection::Point ptj = detection->GetCorner(j); + line(mat, cv::Point(pti.x, pti.y), cv::Point(ptj.x, ptj.y), + outlineColor, 2); + } + + // mark the center of the tag + const frc::AprilTagDetection::Point c = detection->GetCenter(); + int ll = 10; + line(mat, cv::Point(c.x - ll, c.y), cv::Point(c.x + ll, c.y), + crossColor, 2); + line(mat, cv::Point(c.x, c.y - ll), cv::Point(c.x, c.y + ll), + crossColor, 2); + + // identify the tag + putText(mat, std::to_string(detection->GetId()), + cv::Point(c.x + ll, c.y), cv::FONT_HERSHEY_SIMPLEX, 1, + crossColor, 3); + + // determine pose + frc::Transform3d pose = estimator.Estimate(*detection); + + // put pose into NT + frc::Rotation3d rotation = pose.Rotation(); + tagsTable->GetEntry(fmt::format("pose_{}", detection->GetId())) + .SetDoubleArray( + {{ pose.X().value(), + pose.Y().value(), + pose.Z().value(), + rotation.X().value(), + rotation.Y().value(), + rotation.Z().value() }}); + } + + // put list of tags onto NT + pubTags.Set(tags); + + // Give the output stream a new image to display + outputStream.PutFrame(mat); + } + } +#endif + + void RobotInit() override { + // We need to run our vision program in a separate thread. If not, our robot + // program will not run. +#if defined(__linux__) || defined(_WIN32) + std::thread visionThread(VisionThread); + visionThread.detach(); +#else + std::fputs("Vision only available on Linux or Windows.\n", stderr); + std::fflush(stderr); +#endif + } +}; + +#ifndef RUNNING_FRC_TESTS +int main() { + return frc::StartRobot(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/I2CCommunication/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/I2CCommunication/cpp/Robot.cpp index c927fb4d2bc..839f8cd48e5 100644 --- a/wpilibcExamples/src/main/cpp/examples/I2CCommunication/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/I2CCommunication/cpp/Robot.cpp @@ -5,7 +5,6 @@ #include "Robot.h" #include - #include #include diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 5f30ac0760c..eb2d25d041b 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -259,6 +259,17 @@ "gradlebase": "cpp", "commandversion": 2 }, + { + "name": "AprilTags Vision", + "description": "on-roboRIO detection of AprilTags using an attached USB camera.", + "tags": [ + "Vision", + "AprilTags" + ], + "foldername": "AprilTagsVision", + "gradlebase": "cpp", + "commandversion": 2 + }, { "name": "I2C Communication", "description": "An example program that communicates with external devices (such as an Arduino) using the roboRIO's I2C port", diff --git a/wpilibcExamples/src/main/cpp/templates/timed/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/timed/cpp/Robot.cpp index 2fcc80b26e1..f74262bef9e 100644 --- a/wpilibcExamples/src/main/cpp/templates/timed/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/templates/timed/cpp/Robot.cpp @@ -5,7 +5,6 @@ #include "Robot.h" #include - #include void Robot::RobotInit() { diff --git a/wpilibcExamples/src/test/cpp/examples/AddressableLED/cpp/RainbowTest.cpp b/wpilibcExamples/src/test/cpp/examples/AddressableLED/cpp/RainbowTest.cpp index 553a2bbaec7..fa891b4f13f 100644 --- a/wpilibcExamples/src/test/cpp/examples/AddressableLED/cpp/RainbowTest.cpp +++ b/wpilibcExamples/src/test/cpp/examples/AddressableLED/cpp/RainbowTest.cpp @@ -2,11 +2,11 @@ // 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. -#include #include #include +#include #include #include #include diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Main.java new file mode 100644 index 00000000000..0bb625158bd --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Main.java @@ -0,0 +1,25 @@ +// 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. + +package edu.wpi.first.wpilibj.examples.apriltagsvision; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Robot.java new file mode 100644 index 00000000000..1334d14debd --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Robot.java @@ -0,0 +1,147 @@ +// 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. + +package edu.wpi.first.wpilibj.examples.apriltagsvision; + +import edu.wpi.first.apriltag.AprilTagDetection; +import edu.wpi.first.apriltag.AprilTagDetector; +import edu.wpi.first.apriltag.AprilTagPoseEstimator; +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.CvSink; +import edu.wpi.first.cscore.CvSource; +import edu.wpi.first.cscore.UsbCamera; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.networktables.IntegerArrayPublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.TimedRobot; +import java.util.ArrayList; +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; + +/** + * 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! + */ +public class Robot extends TimedRobot { + @Override + public void robotInit() { + var visionThread = new Thread(() -> apriltagVisionThreadProc()); + visionThread.setDaemon(true); + visionThread.start(); + } + + void apriltagVisionThreadProc() { + var detector = new AprilTagDetector(); + // look for tag16h5, don't correct any error bits + detector.addFamily("tag16h5", 0); + + // Set up Pose Estimator - parameters are for a Microsoft Lifecam HD-3000 + // (https://www.chiefdelphi.com/t/wpilib-apriltagdetector-sample-code/421411/21) + var poseEstConfig = + new AprilTagPoseEstimator.Config( + 0.1524, 699.3778103158814, 677.7161226393544, 345.6059345433618, 207.12741326228522); + var estimator = new AprilTagPoseEstimator(poseEstConfig); + + // Get the UsbCamera from CameraServer + UsbCamera camera = CameraServer.startAutomaticCapture(); + // Set the resolution + camera.setResolution(640, 480); + + // Get a CvSink. This will capture Mats from the camera + CvSink cvSink = CameraServer.getVideo(); + // Setup a CvSource. This will send images back to the Dashboard + CvSource outputStream = CameraServer.putVideo("Detected", 640, 480); + + // Mats are very memory expensive. Lets reuse these. + var mat = new Mat(); + var grayMat = new Mat(); + + // Instantiate once + ArrayList tags = new ArrayList<>(); + var outlineColor = new Scalar(0, 255, 0); + var crossColor = new Scalar(0, 0, 255); + + // We'll output to NT + NetworkTable tagsTable = NetworkTableInstance.getDefault().getTable("apriltags"); + IntegerArrayPublisher pubTags = tagsTable.getIntegerArrayTopic("tags").publish(); + + // This cannot be 'true'. The program will never exit if it is. This + // lets the robot stop this thread when restarting robot code or + // deploying. + while (!Thread.interrupted()) { + // 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 the error. + outputStream.notifyError(cvSink.getError()); + // skip the rest of the current iteration + continue; + } + + Imgproc.cvtColor(mat, grayMat, Imgproc.COLOR_RGB2GRAY); + + AprilTagDetection[] detections = detector.detect(grayMat); + + // have not seen any tags yet + tags.clear(); + + for (AprilTagDetection detection : detections) { + // remember we saw this tag + tags.add((long) detection.getId()); + + // draw lines around the tag + for (var i = 0; i <= 3; i++) { + var j = (i + 1) % 4; + var pt1 = new Point(detection.getCornerX(i), detection.getCornerY(i)); + var pt2 = new Point(detection.getCornerX(j), detection.getCornerY(j)); + Imgproc.line(mat, pt1, pt2, outlineColor, 2); + } + + // mark the center of the tag + var cx = detection.getCenterX(); + var cy = detection.getCenterY(); + var ll = 10; + Imgproc.line(mat, new Point(cx - ll, cy), new Point(cx + ll, cy), crossColor, 2); + Imgproc.line(mat, new Point(cx, cy - ll), new Point(cx, cy + ll), crossColor, 2); + + // identify the tag + Imgproc.putText( + mat, + Integer.toString(detection.getId()), + new Point(cx + ll, cy), + Imgproc.FONT_HERSHEY_SIMPLEX, + 1, + crossColor, + 3); + + // determine pose + Transform3d pose = estimator.estimate(detection); + + // put pose into dashbaord + Rotation3d rot = pose.getRotation(); + tagsTable + .getEntry("pose_" + detection.getId()) + .setDoubleArray( + new double[] { + pose.getX(), pose.getY(), pose.getZ(), rot.getX(), rot.getY(), rot.getZ() + }); + } + + // put list of tags onto dashboard + pubTags.set(tags.stream().mapToLong(Long::longValue).toArray()); + + // Give the output stream a new image to display + outputStream.putFrame(mat); + } + + pubTags.close(); + detector.close(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index 468b1f4d7e7..69a81c1e73a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -295,6 +295,18 @@ "mainclass": "Main", "commandversion": 2 }, + { + "name": "AprilTags Vision", + "description": "on-roboRIO detection of AprilTags using an attached USB camera.", + "tags": [ + "Vision", + "AprilTags" + ], + "foldername": "apriltagsvision", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, { "name": "Shuffleboard Sample", "description": "An example program that adds data to various Shuffleboard tabs that demonstrates the Shuffleboard API",