diff --git a/wpilibc/athena/include/RobotBase.h b/wpilibc/athena/include/RobotBase.h index ff0639fab2..a6284c2fe3 100644 --- a/wpilibc/athena/include/RobotBase.h +++ b/wpilibc/athena/include/RobotBase.h @@ -9,6 +9,7 @@ #include #include +#include #include "Base.h" #include "HAL/HAL.h" @@ -47,6 +48,7 @@ class RobotBase { bool IsOperatorControl() const; bool IsTest() const; bool IsNewDataAvailable() const; + static std::thread::id GetThreadId(); virtual void StartCompetition() = 0; protected: @@ -57,6 +59,8 @@ class RobotBase { RobotBase& operator=(const RobotBase&) = delete; DriverStation& m_ds; + + static std::thread::id m_threadId; }; } // namespace frc diff --git a/wpilibc/athena/include/WPILib.h b/wpilibc/athena/include/WPILib.h index 3607fc6112..affedf1817 100644 --- a/wpilibc/athena/include/WPILib.h +++ b/wpilibc/athena/include/WPILib.h @@ -87,3 +87,4 @@ #include "interfaces/Accelerometer.h" #include "interfaces/Gyro.h" #include "interfaces/Potentiometer.h" +#include "vision/VisionRunner.h" diff --git a/wpilibc/athena/include/vision/VisionPipeline.h b/wpilibc/athena/include/vision/VisionPipeline.h new file mode 100644 index 0000000000..296ba80f2c --- /dev/null +++ b/wpilibc/athena/include/vision/VisionPipeline.h @@ -0,0 +1,32 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +namespace cv { +class Mat; +} + +namespace frc { + +/** + * A vision pipeline is responsible for running a group of + * OpenCV algorithms to extract data from an image. + * + * @see VisionRunner + */ +class VisionPipeline { + public: + virtual ~VisionPipeline() = default; + + /** + * Processes the image input and sets the result objects. + * Implementations should make these objects accessible. + */ + virtual void Process(cv::Mat& mat) = 0; +}; +} // namespace frc diff --git a/wpilibc/athena/include/vision/VisionRunner.h b/wpilibc/athena/include/vision/VisionRunner.h new file mode 100644 index 0000000000..370f0bce98 --- /dev/null +++ b/wpilibc/athena/include/vision/VisionRunner.h @@ -0,0 +1,65 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "ErrorBase.h" +#include "cscore.h" +#include "vision/VisionPipeline.h" + +namespace frc { + +/** + * Non-template base class for VisionRunner. + */ +class VisionRunnerBase : public ErrorBase { + public: + explicit VisionRunnerBase(cs::VideoSource videoSource); + ~VisionRunnerBase() override; + + VisionRunnerBase(const VisionRunnerBase&) = delete; + VisionRunnerBase& operator=(const VisionRunnerBase&) = delete; + + void RunOnce(); + + void RunForever(); + + protected: + virtual void DoProcess(cv::Mat& image) = 0; + + private: + std::unique_ptr m_image; + cs::CvSink m_cvSink; +}; + +/** + * A vision runner is a convenient wrapper object to make it easy to run vision + * pipelines from robot code. The easiest way to use this is to run it in a + * std::thread and use the listener to take snapshots of the pipeline's outputs. + * + * @see VisionPipeline + */ +template +class VisionRunner : public VisionRunnerBase { + public: + VisionRunner(cs::VideoSource videoSource, T* pipeline, + std::function listener); + virtual ~VisionRunner() = default; + + protected: + void DoProcess(cv::Mat& image) override; + + private: + T* m_pipeline; + std::function m_listener; +}; +} // namespace frc + +#include "VisionRunner.inc" diff --git a/wpilibc/athena/include/vision/VisionRunner.inc b/wpilibc/athena/include/vision/VisionRunner.inc new file mode 100644 index 0000000000..48508c3674 --- /dev/null +++ b/wpilibc/athena/include/vision/VisionRunner.inc @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +namespace frc { + +/** + * Creates a new vision runner. It will take images from the {@code + * videoSource}, send them to the {@code pipeline}, and call the {@code + * listener} when the pipeline has finished to alert user code when it is safe + * to access the pipeline's outputs. + * + * @param videoSource the video source to use to supply images for the pipeline + * @param pipeline the vision pipeline to run + * @param listener a function to call after the pipeline has finished + * running + */ +template +VisionRunner::VisionRunner(cs::VideoSource videoSource, T* pipeline, + std::function listener) + : VisionRunnerBase(videoSource), + m_pipeline(pipeline), + m_listener(listener) {} + +template +void VisionRunner::DoProcess(cv::Mat& image) { + m_pipeline->Process(image); + m_listener(*m_pipeline); +} + +} // namespace frc diff --git a/wpilibc/athena/src/RobotBase.cpp b/wpilibc/athena/src/RobotBase.cpp index b55a76e48b..2a97670d67 100644 --- a/wpilibc/athena/src/RobotBase.cpp +++ b/wpilibc/athena/src/RobotBase.cpp @@ -20,6 +20,8 @@ using namespace frc; +std::thread::id RobotBase::m_threadId; + /** * Constructor for a generic robot program. * @@ -32,6 +34,8 @@ using namespace frc; * boot so ensure that it runs. */ RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) { + m_threadId = std::this_thread::get_id(); + RobotState::SetImplementation(DriverStation::GetInstance()); HLUsageReporting::SetImplementation(new HardwareHLReporting()); @@ -87,3 +91,8 @@ bool RobotBase::IsTest() const { return m_ds.IsTest(); } * function was called? */ bool RobotBase::IsNewDataAvailable() const { return m_ds.IsNewControlData(); } + +/** + * Gets the ID of the main robot thread + */ +std::thread::id RobotBase::GetThreadId() { return m_threadId; } diff --git a/wpilibc/athena/src/vision/VisionRunner.cpp b/wpilibc/athena/src/vision/VisionRunner.cpp new file mode 100644 index 0000000000..4653344cbc --- /dev/null +++ b/wpilibc/athena/src/vision/VisionRunner.cpp @@ -0,0 +1,76 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "vision/VisionRunner.h" + +#include "DriverStation.h" +#include "RobotBase.h" +#include "opencv2/core/mat.hpp" + +using namespace frc; + +/** + * Creates a new vision runner. It will take images from the {@code + * videoSource}, and call the virtual DoProcess() method. + * + * @param videoSource the video source to use to supply images for the pipeline + */ +VisionRunnerBase::VisionRunnerBase(cs::VideoSource videoSource) + : m_image(std::make_unique()), m_cvSink("VisionRunner CvSink") { + m_cvSink.SetSource(videoSource); +} + +// Located here and not in header due to cv::Mat forward declaration. +VisionRunnerBase::~VisionRunnerBase() {} + +/** + * Runs the pipeline one time, giving it the next image from the video source + * specified in the constructor. This will block until the source either has an + * image or throws an error. If the source successfully supplied a frame, the + * pipeline's image input will be set, the pipeline will run, and the listener + * specified in the constructor will be called to notify it that the pipeline + * ran. This must be run in a dedicated thread, and cannot be used in the main + * robot thread because it will freeze the robot program. + * + *

This method is exposed to allow teams to add additional functionality or + * have their own ways to run the pipeline. Most teams, however, should just + * use {@link #runForever} in its own thread using a std::thread.

+ */ +void VisionRunnerBase::RunOnce() { + if (std::this_thread::get_id() == RobotBase::GetThreadId()) { + wpi_setErrnoErrorWithContext( + "VisionRunner::RunOnce() cannot be called from the main robot thread"); + return; + } + auto frameTime = m_cvSink.GrabFrame(*m_image); + if (frameTime == 0) { + auto error = m_cvSink.GetError(); + DriverStation::ReportError(error); + } else { + DoProcess(*m_image); + } +} + +/** + * A convenience method that calls {@link #runOnce()} in an infinite loop. This + * must be run in a dedicated thread, and cannot be used in the main robot + * thread because it will freeze the robot program. + * + *

Do not call this method directly from the main + * thread.

+ */ +void VisionRunnerBase::RunForever() { + if (std::this_thread::get_id() == RobotBase::GetThreadId()) { + wpi_setErrnoErrorWithContext( + "VisionRunner::RunForever() cannot be called from the main robot " + "thread"); + return; + } + while (true) { + RunOnce(); + } +} diff --git a/wpilibcIntegrationTests/src/VisionTest.cpp b/wpilibcIntegrationTests/src/VisionTest.cpp new file mode 100644 index 0000000000..e5da1e138d --- /dev/null +++ b/wpilibcIntegrationTests/src/VisionTest.cpp @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "vision/VisionRunner.h" + +using namespace frc; + +class VisionTester : public VisionPipeline { + public: + virtual ~VisionTester() = default; + void Process(cv::Mat& mat) override {} + void TestThing() {} +}; + +void TestVisionInitialization() { + cs::CvSource source; + VisionTester tester; + VisionRunner runner(source, &tester, + [](VisionTester& t) { t.TestThing(); }); + + runner.RunOnce(); + runner.RunForever(); +}