mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
60 lines
1.9 KiB
C++
60 lines
1.9 KiB
C++
/*----------------------------------------------------------------------------*/
|
|
/* Copyright (c) 2016-2018 FIRST. 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 <thread>
|
|
|
|
#include <opencv2/core/mat.hpp>
|
|
|
|
#include "CameraServerShared.h"
|
|
|
|
using namespace frc;
|
|
|
|
VisionRunnerBase::VisionRunnerBase(cs::VideoSource videoSource)
|
|
: m_image(std::make_unique<cv::Mat>()),
|
|
m_cvSink("VisionRunner CvSink"),
|
|
m_enabled(true) {
|
|
m_cvSink.SetSource(videoSource);
|
|
}
|
|
|
|
// Located here and not in header due to cv::Mat forward declaration.
|
|
VisionRunnerBase::~VisionRunnerBase() {}
|
|
|
|
void VisionRunnerBase::RunOnce() {
|
|
auto csShared = frc::GetCameraServerShared();
|
|
auto res = csShared->GetRobotMainThreadId();
|
|
if (res.second && (std::this_thread::get_id() == res.first)) {
|
|
csShared->SetVisionRunnerError(
|
|
"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();
|
|
csShared->ReportDriverStationError(error);
|
|
} else {
|
|
DoProcess(*m_image);
|
|
}
|
|
}
|
|
|
|
void VisionRunnerBase::RunForever() {
|
|
auto csShared = frc::GetCameraServerShared();
|
|
auto res = csShared->GetRobotMainThreadId();
|
|
if (res.second && (std::this_thread::get_id() == res.first)) {
|
|
csShared->SetVisionRunnerError(
|
|
"VisionRunner::RunForever() cannot be called from the main robot "
|
|
"thread");
|
|
return;
|
|
}
|
|
while (m_enabled) {
|
|
RunOnce();
|
|
}
|
|
}
|
|
|
|
void VisionRunnerBase::Stop() { m_enabled = false; }
|