From 680d9a5bab98ac219f7bdb86ced30f9404abdb22 Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Thu, 12 Jan 2023 10:36:02 -0500 Subject: [PATCH 01/19] [examples] Add example programs for AprilTags detection [#4929]. --- .../examples/AprilTagsDetection/cpp/Robot.cpp | 155 ++++++++++++++++++ .../src/main/cpp/examples/examples.json | 11 ++ .../examples/apriltagsdetection/Main.java | 25 +++ .../examples/apriltagsdetection/Robot.java | 127 ++++++++++++++ .../wpi/first/wpilibj/examples/examples.json | 12 ++ 5 files changed, 330 insertions(+) create mode 100644 wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Main.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java diff --git a/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp new file mode 100644 index 00000000000..d36e3b0ec36 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp @@ -0,0 +1,155 @@ +// 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 + +/** + * 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. + */ +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 = frc::AprilTagPoseEstimator(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 = cv::Scalar(0, 255, 0); + cv::Scalar crossColor = cv::Scalar(0, 0, 255); + + 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 dashbaord + std::stringstream dashboardString; + dashboardString << "Translation: " << units::length::to_string(pose.X()) << ", " << units::length::to_string(pose.Y()) << ", " << units::length::to_string(pose.Z()); + frc::Rotation3d rotation = pose.Rotation(); + dashboardString << "; Rotation: " << units::angle::to_string(rotation.X()) << ", " << units::angle::to_string(rotation.Y()) << ", " << units::angle::to_string(rotation.Z()); + frc::SmartDashboard::PutString("pose_" + std::to_string(detection->GetId()), dashboardString.str()); + } + + // put list of tags onto dashboard + std::stringstream tags_s; + if (tags.size() > 0) { + if (tags.size() > 1) { + std::copy(tags.begin(), tags.end()-1, std::ostream_iterator(tags_s, ",")); + } + tags_s << tags.back(); + } + frc::SmartDashboard::PutString("tags", tags_s.str()); + + // 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/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 5f30ac0760c..090f49d9a6b 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 Detection", + "description": "An example program that acquires images from an attached USB camera, detects AprilTags, outlines the tags, sends the image to the dashboard, and sends information about the detected tags to SmartDashboard.", + "tags": [ + "Vision", + "Complete List" + ], + "foldername": "AprilTagsDetection", + "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/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Main.java new file mode 100644 index 00000000000..f2fb11fafdb --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/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.intermediatevision; + +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/apriltagsdetection/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java new file mode 100644 index 00000000000..11dcc8c42b8 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java @@ -0,0 +1,127 @@ +// 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 frc.robot; + +import java.util.ArrayList; + +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; + +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.Transform3d; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * 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. + */ +public class Robot extends TimedRobot { + + @Override + public void robotInit() { + Thread visionThread = new Thread(() -> apriltagVisionThreadProc()); + visionThread.setDaemon(true); + visionThread.start(); + } + + void apriltagVisionThreadProc() { + AprilTagDetector 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) + AprilTagPoseEstimator.Config poseEstConfig = new AprilTagPoseEstimator.Config(0.1524, 699.3778103158814, 677.7161226393544, 345.6059345433618, 207.12741326228522); + AprilTagPoseEstimator 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 this Mat. + Mat mat = new Mat(); + Mat grayMat = new Mat(); + + // Instantiate once + ArrayList tags = new ArrayList<>(); + Scalar outlineColor = new Scalar(0, 255, 0); + Scalar crossColor = new Scalar(0, 0, 255); + + // 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(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 + var dashboardString = pose.toString(); + SmartDashboard.putString("pose_" + detection.getId(), dashboardString); + } + + // put list of tags onto dashboard + SmartDashboard.putString("tags", tags.toString()); + + // Give the output stream a new image to display + outputStream.putFrame(mat); + } + + 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..a56a799c025 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 Detection", + "description": "An example program that acquires images from an attached USB camera, detects AprilTags, outlines the tags, sends the image to the dashboard, and send information about the detected tags to SmartDashboard.", + "tags": [ + "Vision", + "Complete List" + ], + "foldername": "apriltagsdetection", + "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", From 27b60f567a44029256ed27d12f2aa6b0dae10a4a Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Thu, 12 Jan 2023 11:09:49 -0500 Subject: [PATCH 02/19] Fix incorrect package names. --- .../edu/wpi/first/wpilibj/examples/apriltagsdetection/Main.java | 2 +- .../wpi/first/wpilibj/examples/apriltagsdetection/Robot.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Main.java index f2fb11fafdb..8610c7e2d08 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Main.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Main.java @@ -2,7 +2,7 @@ // 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.intermediatevision; +package edu.wpi.first.wpilibj.examples.apriltagsdetection; import edu.wpi.first.wpilibj.RobotBase; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java index 11dcc8c42b8..32c6a30a745 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java @@ -2,7 +2,7 @@ // 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 frc.robot; +package edu.wpi.first.wpilibj.examples.intermediatevision; import java.util.ArrayList; From fb539613e2b795c37974b9f4313e67b3d60b0291 Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Thu, 12 Jan 2023 11:49:07 -0500 Subject: [PATCH 03/19] *Correctly* fix incorrect package names. --- .../wpi/first/wpilibj/examples/apriltagsdetection/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java index 32c6a30a745..92f44381486 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java @@ -2,7 +2,7 @@ // 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.intermediatevision; +package edu.wpi.first.wpilibj.examples.apriltagsdetection; import java.util.ArrayList; From d89067165ffce0bd59179b47a468a632cbbb0bec Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Thu, 12 Jan 2023 12:03:57 -0500 Subject: [PATCH 04/19] Lint. --- .../examples/AprilTagsDetection/cpp/Robot.cpp | 48 ++++++++++++------- .../examples/apriltagsdetection/Robot.java | 5 +- 2 files changed, 32 insertions(+), 21 deletions(-) diff --git a/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp index d36e3b0ec36..c6134e07c2b 100644 --- a/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp @@ -3,28 +3,28 @@ // 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 #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 + * The image is acquired from the USB camera, then any detected AprilTags * are marked up on the image and sent to the dashboard. */ class Robot : public frc::TimedRobot { @@ -43,9 +43,9 @@ class Robot : public frc::TimedRobot { .fx = 699.3778103158814, .fy = 677.7161226393544, .cx = 345.6059345433618, - .cy = 207.12741326228522 - }; - frc::AprilTagPoseEstimator estimator = frc::AprilTagPoseEstimator(poseEstConfig); + .cy = 207.12741326228522}; + frc::AprilTagPoseEstimator estimator = + frc::AprilTagPoseEstimator(poseEstConfig); // Get the USB camera from CameraServer cs::UsbCamera camera = frc::CameraServer::StartAutomaticCapture(); @@ -82,7 +82,8 @@ class Robot : public frc::TimedRobot { 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); + frc::AprilTagDetector::Results detections = + detector.Detect(g_size.width, g_size.height, grayMat.data); // have not seen any tags yet tags.clear(); @@ -96,27 +97,38 @@ class Robot : public frc::TimedRobot { 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); + 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); + 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); + 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 dashbaord std::stringstream dashboardString; - dashboardString << "Translation: " << units::length::to_string(pose.X()) << ", " << units::length::to_string(pose.Y()) << ", " << units::length::to_string(pose.Z()); + dashboardString << "Translation: " << units::length::to_string(pose.X()) + << ", " << units::length::to_string(pose.Y()) << ", " + << units::length::to_string(pose.Z()); frc::Rotation3d rotation = pose.Rotation(); - dashboardString << "; Rotation: " << units::angle::to_string(rotation.X()) << ", " << units::angle::to_string(rotation.Y()) << ", " << units::angle::to_string(rotation.Z()); - frc::SmartDashboard::PutString("pose_" + std::to_string(detection->GetId()), dashboardString.str()); + dashboardString << "; Rotation: " + << units::angle::to_string(rotation.X()) << ", " + << units::angle::to_string(rotation.Y()) << ", " + << units::angle::to_string(rotation.Z()); + frc::SmartDashboard::PutString("pose_" + std::to_string(detection->GetId()), + dashboardString.str()); } // put list of tags onto dashboard diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java index 92f44381486..d8fbbbf9a8b 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java @@ -24,11 +24,10 @@ /** * This is a demo program showing the detection of AprilTags. - * The image is acquired from the USB camera, then any detected AprilTags + * The image is acquired from the USB camera, then any detected AprilTags * are marked up on the image and sent to the dashboard. */ public class Robot extends TimedRobot { - @Override public void robotInit() { Thread visionThread = new Thread(() -> apriltagVisionThreadProc()); @@ -44,7 +43,7 @@ void apriltagVisionThreadProc() { // Set up Pose Estimator - parameters are for a Microsoft Lifecam HD-3000 (https://www.chiefdelphi.com/t/wpilib-apriltagdetector-sample-code/421411/21) AprilTagPoseEstimator.Config poseEstConfig = new AprilTagPoseEstimator.Config(0.1524, 699.3778103158814, 677.7161226393544, 345.6059345433618, 207.12741326228522); AprilTagPoseEstimator estimator = new AprilTagPoseEstimator(poseEstConfig); - + // Get the UsbCamera from CameraServer UsbCamera camera = CameraServer.startAutomaticCapture(); // Set the resolution From 06846fcb53a29984e9c21bf72532620fc2d46f3b Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Thu, 12 Jan 2023 12:11:52 -0500 Subject: [PATCH 05/19] More lint. --- .../examples/AprilTagsDetection/cpp/Robot.cpp | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp index c6134e07c2b..f95db40ea8a 100644 --- a/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp @@ -8,7 +8,6 @@ #include #include - #include #include #include @@ -45,7 +44,7 @@ class Robot : public frc::TimedRobot { .cx = 345.6059345433618, .cy = 207.12741326228522}; frc::AprilTagPoseEstimator estimator = - frc::AprilTagPoseEstimator(poseEstConfig); + frc::AprilTagPoseEstimator(poseEstConfig); // Get the USB camera from CameraServer cs::UsbCamera camera = frc::CameraServer::StartAutomaticCapture(); @@ -83,7 +82,7 @@ class Robot : public frc::TimedRobot { cv::Size g_size = grayMat.size(); frc::AprilTagDetector::Results detections = - detector.Detect(g_size.width, g_size.height, grayMat.data); + detector.Detect(g_size.width, g_size.height, grayMat.data); // have not seen any tags yet tags.clear(); @@ -98,21 +97,21 @@ class Robot : public frc::TimedRobot { 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); + 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); + crossColor, 2); line(mat, cv::Point(c.x, c.y - ll), cv::Point(c.x, c.y + ll), - crossColor, 2); + 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); + cv::Point(c.x + ll, c.y), cv::FONT_HERSHEY_SIMPLEX, 1, + crossColor, 3); // determine pose frc::Transform3d pose = estimator.Estimate(*detection); @@ -120,15 +119,16 @@ class Robot : public frc::TimedRobot { // put pose into dashbaord std::stringstream dashboardString; dashboardString << "Translation: " << units::length::to_string(pose.X()) - << ", " << units::length::to_string(pose.Y()) << ", " - << units::length::to_string(pose.Z()); + << ", " << units::length::to_string(pose.Y()) << ", " + << units::length::to_string(pose.Z()); frc::Rotation3d rotation = pose.Rotation(); dashboardString << "; Rotation: " - << units::angle::to_string(rotation.X()) << ", " - << units::angle::to_string(rotation.Y()) << ", " - << units::angle::to_string(rotation.Z()); - frc::SmartDashboard::PutString("pose_" + std::to_string(detection->GetId()), - dashboardString.str()); + << units::angle::to_string(rotation.X()) << ", " + << units::angle::to_string(rotation.Y()) << ", " + << units::angle::to_string(rotation.Z()); + frc::SmartDashboard::PutString( + "pose_" + std::to_string(detection->GetId()), + dashboardString.str()); } // put list of tags onto dashboard From 9a7c5851468adbff6b1e62f77bfeed40bb3c7edf Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Thu, 12 Jan 2023 12:18:48 -0500 Subject: [PATCH 06/19] /format --- .../cpp/examples/AprilTagsDetection/cpp/Robot.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp index f95db40ea8a..42a07644fef 100644 --- a/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp @@ -104,25 +104,25 @@ class Robot : public frc::TimedRobot { 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); + crossColor, 2); line(mat, cv::Point(c.x, c.y - ll), cv::Point(c.x, c.y + ll), - crossColor, 2); + 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); + 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 dashbaord std::stringstream dashboardString; - dashboardString << "Translation: " << units::length::to_string(pose.X()) + dashboardString << "Translation: " << units::length::to_string(pose.X()) << ", " << units::length::to_string(pose.Y()) << ", " << units::length::to_string(pose.Z()); frc::Rotation3d rotation = pose.Rotation(); - dashboardString << "; Rotation: " + dashboardString << "; Rotation: " << units::angle::to_string(rotation.X()) << ", " << units::angle::to_string(rotation.Y()) << ", " << units::angle::to_string(rotation.Z()); @@ -135,7 +135,8 @@ class Robot : public frc::TimedRobot { std::stringstream tags_s; if (tags.size() > 0) { if (tags.size() > 1) { - std::copy(tags.begin(), tags.end()-1, std::ostream_iterator(tags_s, ",")); + std::copy(tags.begin(), tags.end() - 1, + std::ostream_iterator(tags_s, ",")); } tags_s << tags.back(); } From d3db42ab6bde77b998d6600058f1392008b25645 Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Thu, 12 Jan 2023 12:24:16 -0500 Subject: [PATCH 07/19] sooner or later. --- .../src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp index 42a07644fef..daf70687e09 100644 --- a/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp @@ -97,7 +97,7 @@ class Robot : public frc::TimedRobot { 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); + outlineColor, 2); } // mark the center of the tag @@ -124,7 +124,7 @@ class Robot : public frc::TimedRobot { frc::Rotation3d rotation = pose.Rotation(); dashboardString << "; Rotation: " << units::angle::to_string(rotation.X()) << ", " - << units::angle::to_string(rotation.Y()) << ", " + << units::angle::to_string(rotation.Y()) << ", " << units::angle::to_string(rotation.Z()); frc::SmartDashboard::PutString( "pose_" + std::to_string(detection->GetId()), From a8e4aca959f9c06c9a761700e70ee0ac54868348 Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Thu, 12 Jan 2023 12:55:12 -0500 Subject: [PATCH 08/19] Did a local :wpilibjexamples:checkstyleMain --- .../examples/apriltagsdetection/Robot.java | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java index d8fbbbf9a8b..900fc3823ec 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java @@ -4,13 +4,6 @@ package edu.wpi.first.wpilibj.examples.apriltagsdetection; -import java.util.ArrayList; - -import org.opencv.core.Mat; -import org.opencv.core.Point; -import org.opencv.core.Scalar; -import org.opencv.imgproc.Imgproc; - import edu.wpi.first.apriltag.AprilTagDetection; import edu.wpi.first.apriltag.AprilTagDetector; import edu.wpi.first.apriltag.AprilTagPoseEstimator; @@ -21,6 +14,11 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +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. @@ -41,7 +39,9 @@ void apriltagVisionThreadProc() { 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) - AprilTagPoseEstimator.Config poseEstConfig = new AprilTagPoseEstimator.Config(0.1524, 699.3778103158814, 677.7161226393544, 345.6059345433618, 207.12741326228522); + AprilTagPoseEstimator.Config poseEstConfig = new AprilTagPoseEstimator.Config( + 0.1524, 699.3778103158814, 677.7161226393544, 345.6059345433618, 207.12741326228522 + ); AprilTagPoseEstimator estimator = new AprilTagPoseEstimator(poseEstConfig); // Get the UsbCamera from CameraServer @@ -103,7 +103,8 @@ void apriltagVisionThreadProc() { 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); + 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); From 09372697bc3f8728c089c8d5700547be4b774a3f Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Thu, 12 Jan 2023 13:15:36 -0500 Subject: [PATCH 09/19] Finally did a .\gradlew :wpilibjexamples:spotlessApply --- .../examples/apriltagsdetection/Robot.java | 25 +++++++++++-------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java index 900fc3823ec..32b924e1578 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java @@ -21,9 +21,8 @@ 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. + * 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. */ public class Robot extends TimedRobot { @Override @@ -38,10 +37,11 @@ void apriltagVisionThreadProc() { // 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) - AprilTagPoseEstimator.Config poseEstConfig = new AprilTagPoseEstimator.Config( - 0.1524, 699.3778103158814, 677.7161226393544, 345.6059345433618, 207.12741326228522 - ); + // Set up Pose Estimator - parameters are for a Microsoft Lifecam HD-3000 + // (https://www.chiefdelphi.com/t/wpilib-apriltagdetector-sample-code/421411/21) + AprilTagPoseEstimator.Config poseEstConfig = + new AprilTagPoseEstimator.Config( + 0.1524, 699.3778103158814, 677.7161226393544, 345.6059345433618, 207.12741326228522); AprilTagPoseEstimator estimator = new AprilTagPoseEstimator(poseEstConfig); // Get the UsbCamera from CameraServer @@ -103,8 +103,14 @@ void apriltagVisionThreadProc() { 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); + 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); @@ -123,5 +129,4 @@ void apriltagVisionThreadProc() { detector.close(); } - } From 641b6b22b6674378745d41d61245d381fd3a6670 Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Thu, 12 Jan 2023 13:21:56 -0500 Subject: [PATCH 10/19] Forcing a recheck. From 21bd4be135943631157022e7ad20bcbc80eee5be Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Thu, 12 Jan 2023 14:27:11 -0500 Subject: [PATCH 11/19] Tweak wording in the examples.json files. --- wpilibcExamples/src/main/cpp/examples/examples.json | 4 ++-- .../main/java/edu/wpi/first/wpilibj/examples/examples.json | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 090f49d9a6b..b1ab0e6b1af 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -261,10 +261,10 @@ }, { "name": "AprilTags Detection", - "description": "An example program that acquires images from an attached USB camera, detects AprilTags, outlines the tags, sends the image to the dashboard, and sends information about the detected tags to SmartDashboard.", + "description": "on-roboRIO detection of AprilTags using an attached USB camera.", "tags": [ "Vision", - "Complete List" + "AprilTags", ], "foldername": "AprilTagsDetection", "gradlebase": "cpp", 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 a56a799c025..8f62fab180a 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 @@ -297,10 +297,10 @@ }, { "name": "AprilTags Detection", - "description": "An example program that acquires images from an attached USB camera, detects AprilTags, outlines the tags, sends the image to the dashboard, and send information about the detected tags to SmartDashboard.", + "description": "on-roboRIO detection of AprilTags using an attached USB camera.", "tags": [ "Vision", - "Complete List" + "AprilTags" ], "foldername": "apriltagsdetection", "gradlebase": "java", From da0726a213d9521fdb46e872a842f7f36a9ac9c1 Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Thu, 12 Jan 2023 14:38:07 -0500 Subject: [PATCH 12/19] ...and the simplest changes are the hardest to get right. --- wpilibcExamples/src/main/cpp/examples/examples.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index b1ab0e6b1af..a2929115dd5 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -264,7 +264,7 @@ "description": "on-roboRIO detection of AprilTags using an attached USB camera.", "tags": [ "Vision", - "AprilTags", + "AprilTags" ], "foldername": "AprilTagsDetection", "gradlebase": "cpp", From 40c9ccc35c7f4a9d236dee98e10eadfcd475b6f7 Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Thu, 12 Jan 2023 14:47:50 -0500 Subject: [PATCH 13/19] Force recheck again. checks on github threw: error: error reading /__w/allwpilib/allwpilib/build/wpimath/thirdparty/ejml/ejml-core-0.41.jar; zip file is empty error: error reading /__w/allwpilib/allwpilib/build/wpimath/thirdparty/ejml/ejml-zdense-0.41.jar; zip file is empty not my fault. From eff8c5993e47a229b0a4739242d7a7f11b8bdc9f Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Fri, 13 Jan 2023 13:27:46 -0500 Subject: [PATCH 14/19] @PeterJohnson @calcmogul @StarLight220 suggestions. --- .../cpp/Robot.cpp | 32 +++++++++++-------- .../src/main/cpp/examples/examples.json | 4 +-- .../Main.java | 2 +- .../Robot.java | 31 ++++++++++-------- .../wpi/first/wpilibj/examples/examples.json | 4 +-- 5 files changed, 41 insertions(+), 32 deletions(-) rename wpilibcExamples/src/main/cpp/examples/{AprilTagsDetection => AprilTagsVision}/cpp/Robot.cpp (85%) rename wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/{apriltagsdetection => apriltagsvision}/Main.java (92%) rename wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/{apriltagsdetection => apriltagsvision}/Robot.java (82%) diff --git a/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/AprilTagsVision/cpp/Robot.cpp similarity index 85% rename from wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp rename to wpilibcExamples/src/main/cpp/examples/AprilTagsVision/cpp/Robot.cpp index daf70687e09..9c53a96db33 100644 --- a/wpilibcExamples/src/main/cpp/examples/AprilTagsDetection/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/AprilTagsVision/cpp/Robot.cpp @@ -25,6 +25,9 @@ * 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) @@ -43,8 +46,7 @@ class Robot : public frc::TimedRobot { .fy = 677.7161226393544, .cx = 345.6059345433618, .cy = 207.12741326228522}; - frc::AprilTagPoseEstimator estimator = - frc::AprilTagPoseEstimator(poseEstConfig); + frc::AprilTagPoseEstimator estimator(poseEstConfig); // Get the USB camera from CameraServer cs::UsbCamera camera = frc::CameraServer::StartAutomaticCapture(); @@ -63,13 +65,12 @@ class Robot : public frc::TimedRobot { // Instantiate once std::vector tags; - cv::Scalar outlineColor = cv::Scalar(0, 255, 0); - cv::Scalar crossColor = cv::Scalar(0, 0, 255); + cv::Scalar outlineColor{0, 255, 0}; + cv::Scalar crossColor{0, 0, 255}; 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 + // put it in the source mat. If there is an error notify the // output. if (cvSink.GrabFrame(mat) == 0) { // Send the output the error. @@ -118,14 +119,19 @@ class Robot : public frc::TimedRobot { // put pose into dashbaord std::stringstream dashboardString; - dashboardString << "Translation: " << units::length::to_string(pose.X()) - << ", " << units::length::to_string(pose.Y()) << ", " - << units::length::to_string(pose.Z()); + dashboardString << "Translation: "; + dashboardString << units::length::to_string(pose.X()); + dashboardString << ", "; + dashboardString << units::length::to_string(pose.Y()); + dashboardString << ", "; + dashboardString << units::length::to_string(pose.Z()); frc::Rotation3d rotation = pose.Rotation(); - dashboardString << "; Rotation: " - << units::angle::to_string(rotation.X()) << ", " - << units::angle::to_string(rotation.Y()) << ", " - << units::angle::to_string(rotation.Z()); + dashboardString << "; Rotation: "; + dashboardString << units::angle::to_string(rotation.X()); + dashboardString << ", "; + dashboardString << units::angle::to_string(rotation.Y()); + dashboardString << ", "; + dashboardString << units::angle::to_string(rotation.Z()); frc::SmartDashboard::PutString( "pose_" + std::to_string(detection->GetId()), dashboardString.str()); diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index a2929115dd5..76a6591d953 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -260,13 +260,13 @@ "commandversion": 2 }, { - "name": "AprilTags Detection", + "name": "AprilTags Vision", "description": "on-roboRIO detection of AprilTags using an attached USB camera.", "tags": [ "Vision", "AprilTags" ], - "foldername": "AprilTagsDetection", + "foldername": "AprilTagsVision", "gradlebase": "cpp", "commandversion": 2 }, diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Main.java similarity index 92% rename from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Main.java rename to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Main.java index 8610c7e2d08..0bb625158bd 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Main.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Main.java @@ -2,7 +2,7 @@ // 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.apriltagsdetection; +package edu.wpi.first.wpilibj.examples.apriltagsvision; import edu.wpi.first.wpilibj.RobotBase; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Robot.java similarity index 82% rename from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java rename to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Robot.java index 32b924e1578..5565cffdf97 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsdetection/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Robot.java @@ -2,7 +2,7 @@ // 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.apriltagsdetection; +package edu.wpi.first.wpilibj.examples.apriltagsvision; import edu.wpi.first.apriltag.AprilTagDetection; import edu.wpi.first.apriltag.AprilTagDetector; @@ -21,28 +21,31 @@ 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. + * 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() { - Thread visionThread = new Thread(() -> apriltagVisionThreadProc()); + var visionThread = new Thread(() -> apriltagVisionThreadProc()); visionThread.setDaemon(true); visionThread.start(); } void apriltagVisionThreadProc() { - AprilTagDetector detector = new AprilTagDetector(); + 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) - AprilTagPoseEstimator.Config poseEstConfig = - new AprilTagPoseEstimator.Config( - 0.1524, 699.3778103158814, 677.7161226393544, 345.6059345433618, 207.12741326228522); - AprilTagPoseEstimator estimator = new AprilTagPoseEstimator(poseEstConfig); + 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(); @@ -54,14 +57,14 @@ void apriltagVisionThreadProc() { // 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 this Mat. - Mat mat = new Mat(); - Mat grayMat = new Mat(); + // Mats are very memory expensive. Lets reuse these. + var mat = new Mat(); + var grayMat = new Mat(); // Instantiate once ArrayList tags = new ArrayList<>(); - Scalar outlineColor = new Scalar(0, 255, 0); - Scalar crossColor = new Scalar(0, 0, 255); + var outlineColor = new Scalar(0, 255, 0); + var crossColor = new Scalar(0, 0, 255); // This cannot be 'true'. The program will never exit if it is. This // lets the robot stop this thread when restarting robot code or 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 8f62fab180a..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 @@ -296,13 +296,13 @@ "commandversion": 2 }, { - "name": "AprilTags Detection", + "name": "AprilTags Vision", "description": "on-roboRIO detection of AprilTags using an attached USB camera.", "tags": [ "Vision", "AprilTags" ], - "foldername": "apriltagsdetection", + "foldername": "apriltagsvision", "gradlebase": "java", "mainclass": "Main", "commandversion": 2 From e3c9881df901e113dc57550c7dd9d8ee6cfc1876 Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Fri, 13 Jan 2023 18:26:39 -0500 Subject: [PATCH 15/19] Mollify checkstyleMain. --- .../edu/wpi/first/wpilibj/examples/apriltagsvision/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 5565cffdf97..51e817e8d57 100644 --- 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 @@ -25,7 +25,7 @@ * 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 + *

Be aware that the performance on this is much worse than a coprocessor * solution! */ public class Robot extends TimedRobot { From 61cece5e54c1fc01147e1220260fc1e12aab2fea Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Fri, 13 Jan 2023 19:41:00 -0500 Subject: [PATCH 16/19] Ran .\gradlew :wpilibjexamples:javaFormat --- .../wpilibj/examples/apriltagsvision/Robot.java | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) 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 index 51e817e8d57..73e8d5daa51 100644 --- 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 @@ -21,12 +21,10 @@ 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. + * 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! + *

Be aware that the performance on this is much worse than a coprocessor solution! */ public class Robot extends TimedRobot { @Override @@ -43,8 +41,9 @@ void apriltagVisionThreadProc() { // 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 poseEstConfig = + new AprilTagPoseEstimator.Config( + 0.1524, 699.3778103158814, 677.7161226393544, 345.6059345433618, 207.12741326228522); var estimator = new AprilTagPoseEstimator(poseEstConfig); // Get the UsbCamera from CameraServer From e84279c62788ef033a2465c041a3a10adf1eaab0 Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Fri, 13 Jan 2023 21:25:54 -0500 Subject: [PATCH 17/19] Tab characters are bad. --- wpilibcExamples/src/main/cpp/examples/examples.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 76a6591d953..eb2d25d041b 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -264,7 +264,7 @@ "description": "on-roboRIO detection of AprilTags using an attached USB camera.", "tags": [ "Vision", - "AprilTags" + "AprilTags" ], "foldername": "AprilTagsVision", "gradlebase": "cpp", From eb003cd346c87c820b3c297744ac1b5479f74bf1 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Fri, 13 Jan 2023 22:04:06 -0800 Subject: [PATCH 18/19] Publish as numbers instead of strings --- .../examples/AprilTagsVision/cpp/Robot.cpp | 49 ++++++++----------- .../examples/apriltagsvision/Robot.java | 25 +++++++--- 2 files changed, 39 insertions(+), 35 deletions(-) diff --git a/wpilibcExamples/src/main/cpp/examples/AprilTagsVision/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/AprilTagsVision/cpp/Robot.cpp index 9c53a96db33..0ed53fd8536 100644 --- a/wpilibcExamples/src/main/cpp/examples/AprilTagsVision/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/AprilTagsVision/cpp/Robot.cpp @@ -9,12 +9,14 @@ #include #include +#include #include #include #include #include #include -#include +#include +#include #include #include #include @@ -64,10 +66,15 @@ class Robot : public frc::TimedRobot { cv::Mat grayMat; // Instantiate once - std::vector tags; + 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 @@ -117,36 +124,20 @@ class Robot : public frc::TimedRobot { // determine pose frc::Transform3d pose = estimator.Estimate(*detection); - // put pose into dashbaord - std::stringstream dashboardString; - dashboardString << "Translation: "; - dashboardString << units::length::to_string(pose.X()); - dashboardString << ", "; - dashboardString << units::length::to_string(pose.Y()); - dashboardString << ", "; - dashboardString << units::length::to_string(pose.Z()); + // put pose into NT frc::Rotation3d rotation = pose.Rotation(); - dashboardString << "; Rotation: "; - dashboardString << units::angle::to_string(rotation.X()); - dashboardString << ", "; - dashboardString << units::angle::to_string(rotation.Y()); - dashboardString << ", "; - dashboardString << units::angle::to_string(rotation.Z()); - frc::SmartDashboard::PutString( - "pose_" + std::to_string(detection->GetId()), - dashboardString.str()); + 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 dashboard - std::stringstream tags_s; - if (tags.size() > 0) { - if (tags.size() > 1) { - std::copy(tags.begin(), tags.end() - 1, - std::ostream_iterator(tags_s, ",")); - } - tags_s << tags.back(); - } - frc::SmartDashboard::PutString("tags", tags_s.str()); + // put list of tags onto NT + pubTags.Set(tags); // Give the output stream a new image to display outputStream.PutFrame(mat); 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 index 73e8d5daa51..1334d14debd 100644 --- 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 @@ -11,9 +11,12 @@ 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 edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.ArrayList; import org.opencv.core.Mat; import org.opencv.core.Point; @@ -61,10 +64,14 @@ void apriltagVisionThreadProc() { var grayMat = new Mat(); // Instantiate once - ArrayList tags = new ArrayList<>(); + 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. @@ -87,7 +94,7 @@ void apriltagVisionThreadProc() { for (AprilTagDetection detection : detections) { // remember we saw this tag - tags.add(detection.getId()); + tags.add((long) detection.getId()); // draw lines around the tag for (var i = 0; i <= 3; i++) { @@ -118,17 +125,23 @@ void apriltagVisionThreadProc() { Transform3d pose = estimator.estimate(detection); // put pose into dashbaord - var dashboardString = pose.toString(); - SmartDashboard.putString("pose_" + detection.getId(), dashboardString); + 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 - SmartDashboard.putString("tags", tags.toString()); + pubTags.set(tags.stream().mapToLong(Long::longValue).toArray()); // Give the output stream a new image to display outputStream.putFrame(mat); } + pubTags.close(); detector.close(); } } From 05aee9ed3d0b49755a63029bff21e06ec84b78a6 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Fri, 13 Jan 2023 22:07:24 -0800 Subject: [PATCH 19/19] Reformat to include fmt in right place --- wpilibcExamples/.styleguide | 1 + .../src/main/cpp/examples/I2CCommunication/cpp/Robot.cpp | 1 - wpilibcExamples/src/main/cpp/templates/timed/cpp/Robot.cpp | 1 - .../src/test/cpp/examples/AddressableLED/cpp/RainbowTest.cpp | 2 +- 4 files changed, 2 insertions(+), 3 deletions(-) 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/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/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