Adds CPP version of vision pipeline (#399)

wpilibc/athena/include/RobotBase.h

@@ -9,6 +9,7 @@
 
 #include <cstdio>
 #include <iostream>
+#include <thread>
 
 #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

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"

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

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 <functional>
+#include <memory>
+
+#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<cv::Mat> 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 <typename T>
+class VisionRunner : public VisionRunnerBase {
+ public:
+  VisionRunner(cs::VideoSource videoSource, T* pipeline,
+               std::function<void(T&)> listener);
+  virtual ~VisionRunner() = default;
+
+ protected:
+  void DoProcess(cv::Mat& image) override;
+
+ private:
+  T* m_pipeline;
+  std::function<void(T&)> m_listener;
+};
+}  // namespace frc
+
+#include "VisionRunner.inc"

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 <typename T>
+VisionRunner<T>::VisionRunner(cs::VideoSource videoSource, T* pipeline,
+                              std::function<void(T&)> listener)
+    : VisionRunnerBase(videoSource),
+      m_pipeline(pipeline),
+      m_listener(listener) {}
+
+template <typename T>
+void VisionRunner<T>::DoProcess(cv::Mat& image) {
+  m_pipeline->Process(image);
+  m_listener(*m_pipeline);
+}
+
+}  // namespace frc

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; }

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<cv::Mat>()), 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.
+ *
+ * <p>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.</p>
+ */
+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.
+ *
+ * <p><strong>Do not call this method directly from the main
+ * thread.</strong></p>
+ */
+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();
+  }
+}

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<VisionTester> runner(source, &tester,
+                                    [](VisionTester& t) { t.TestThing(); });
+
+  runner.RunOnce();
+  runner.RunForever();
+}