Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[cscore] Use frame time in Linux UsbCameraImpl (#7609)

Browse Source
This commit is contained in:
Matt
2025-01-07 09:33:20 -07:00
committed by GitHub
parent 8e459a4f2a
commit 2de03c9601
13 changed files with 267 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
cscore/src/main/native/cpp/Frame.cpp
View File

@@ -16,18 +16,22 @@
using namespace cs;
Frame::Frame(SourceImpl& source, std::string_view error, Time time)
Frame::Frame(SourceImpl& source, std::string_view error, Time time,
WPI_TimestampSource timeSrc)
: m_impl{source.AllocFrameImpl().release()} {
m_impl->refcount = 1;
m_impl->error = error;
m_impl->time = time;
m_impl->timeSource = timeSrc;
}
Frame::Frame(SourceImpl& source, std::unique_ptr<Image> image, Time time)
Frame::Frame(SourceImpl& source, std::unique_ptr<Image> image, Time time,
WPI_TimestampSource timeSrc)
: m_impl{source.AllocFrameImpl().release()} {
m_impl->refcount = 1;
m_impl->error.resize(0);
m_impl->time = time;
m_impl->timeSource = timeSrc;
m_impl->images.push_back(image.release());
}

10
cscore/src/main/native/cpp/Frame.h
View File

@@ -39,6 +39,7 @@ class Frame {
wpi::recursive_mutex mutex;
std::atomic_int refcount{0};
Time time{0};
WPI_TimestampSource timeSource{WPI_TIMESRC_UNKNOWN};
SourceImpl& source;
std::string error;
wpi::SmallVector<Image*, 4> images;
@@ -48,9 +49,11 @@ class Frame {
public:
Frame() noexcept = default;
Frame(SourceImpl& source, std::string_view error, Time time);
Frame(SourceImpl& source, std::string_view error, Time time,
WPI_TimestampSource timeSrc);
Frame(SourceImpl& source, std::unique_ptr<Image> image, Time time);
Frame(SourceImpl& source, std::unique_ptr<Image> image, Time time,
WPI_TimestampSource timeSrc);
Frame(const Frame& frame) noexcept : m_impl{frame.m_impl} {
if (m_impl) {
@@ -75,6 +78,9 @@ class Frame {
}
Time GetTime() const { return m_impl ? m_impl->time : 0; }
WPI_TimestampSource GetTimeSource() const {
return m_impl ? m_impl->timeSource : WPI_TIMESRC_UNKNOWN;
}
std::string_view GetError() const {
if (!m_impl) {

2
cscore/src/main/native/cpp/RawSinkImpl.cpp
View File

@@ -120,6 +120,8 @@ uint64_t RawSinkImpl::GrabFrameImpl(WPI_RawFrame& rawFrame,
rawFrame.pixelFormat = newImage->pixelFormat;
rawFrame.size = newImage->size();
std::copy(newImage->data(), newImage->data() + rawFrame.size, rawFrame.data);
rawFrame.timestamp = incomingFrame.GetTime();
rawFrame.timestampSrc = incomingFrame.GetTimeSource();
return incomingFrame.GetTime();
}

19
cscore/src/main/native/cpp/SourceImpl.cpp
View File

@@ -29,7 +29,7 @@ SourceImpl::SourceImpl(std::string_view name, wpi::Logger& logger,
m_notifier(notifier),
m_telemetry(telemetry),
m_name{name} {
m_frame = Frame{*this, std::string_view{}, 0};
m_frame = Frame{*this, std::string_view{}, 0, WPI_TIMESRC_UNKNOWN};
}
SourceImpl::~SourceImpl() {
@@ -95,7 +95,8 @@ Frame SourceImpl::GetNextFrame(double timeout, Frame::Time lastFrameTime) {
if (!m_frameCv.wait_for(
lock, std::chrono::milliseconds(static_cast<int>(timeout * 1000)),
[=, this] { return m_frame.GetTime() != lastFrameTime; })) {
m_frame = Frame{*this, "timed out getting frame", wpi::Now()};
m_frame = Frame{*this, "timed out getting frame", wpi::Now(),
WPI_TIMESRC_UNKNOWN};
}
return m_frame;
}
@@ -103,7 +104,7 @@ Frame SourceImpl::GetNextFrame(double timeout, Frame::Time lastFrameTime) {
void SourceImpl::Wakeup() {
{
std::scoped_lock lock{m_frameMutex};
m_frame = Frame{*this, std::string_view{}, 0};
m_frame = Frame{*this, std::string_view{}, 0, WPI_TIMESRC_UNKNOWN};
}
m_frameCv.notify_all();
}
@@ -463,7 +464,8 @@ std::unique_ptr<Image> SourceImpl::AllocImage(
}
void SourceImpl::PutFrame(VideoMode::PixelFormat pixelFormat, int width,
int height, std::string_view data, Frame::Time time) {
int height, std::string_view data, Frame::Time time,
WPI_TimestampSource timeSrc) {
if (pixelFormat == VideoMode::PixelFormat::kBGRA) {
// Write BGRA as BGR to save a copy
auto image =
@@ -480,10 +482,11 @@ void SourceImpl::PutFrame(VideoMode::PixelFormat pixelFormat, int width,
fmt::ptr(data.data()), data.size());
std::memcpy(image->data(), data.data(), data.size());
PutFrame(std::move(image), time);
PutFrame(std::move(image), time, timeSrc);
}
void SourceImpl::PutFrame(std::unique_ptr<Image> image, Frame::Time time) {
void SourceImpl::PutFrame(std::unique_ptr<Image> image, Frame::Time time,
WPI_TimestampSource timeSrc) {
// Update telemetry
m_telemetry.RecordSourceFrames(*this, 1);
m_telemetry.RecordSourceBytes(*this, static_cast<int>(image->size()));
@@ -491,7 +494,7 @@ void SourceImpl::PutFrame(std::unique_ptr<Image> image, Frame::Time time) {
// Update frame
{
std::scoped_lock lock{m_frameMutex};
m_frame = Frame{*this, std::move(image), time};
m_frame = Frame{*this, std::move(image), time, timeSrc};
}
// Signal listeners
@@ -502,7 +505,7 @@ void SourceImpl::PutError(std::string_view msg, Frame::Time time) {
// Update frame
{
std::scoped_lock lock{m_frameMutex};
m_frame = Frame{*this, msg, time};
m_frame = Frame{*this, msg, time, WPI_TIMESRC_UNKNOWN};
}
// Signal listeners

7
cscore/src/main/native/cpp/SourceImpl.h
View File

@@ -13,6 +13,7 @@
#include <vector>
#include <wpi/Logger.h>
#include <wpi/RawFrame.h>
#include <wpi/condition_variable.h>
#include <wpi/json_fwd.h>
#include <wpi/mutex.h>
@@ -141,8 +142,10 @@ class SourceImpl : public PropertyContainer {
std::string_view valueStr) override;
void PutFrame(VideoMode::PixelFormat pixelFormat, int width, int height,
std::string_view data, Frame::Time time);
void PutFrame(std::unique_ptr<Image> image, Frame::Time time);
std::string_view data, Frame::Time time,
WPI_TimestampSource timeSrc = WPI_TIMESRC_FRAME_DEQUEUE);
void PutFrame(std::unique_ptr<Image> image, Frame::Time time,
WPI_TimestampSource timeSrc = WPI_TIMESRC_FRAME_DEQUEUE);
void PutError(std::string_view msg, Frame::Time time);
// Notification functions for corresponding atomics

26
cscore/src/main/native/include/cscore_cv.h
View File

@@ -8,6 +8,7 @@
#include <functional>
#include <opencv2/core/mat.hpp>
#include <wpi/RawFrame.h>
#include "cscore_oo.h"
#include "cscore_raw.h"
@@ -172,6 +173,23 @@ class CvSink : public ImageSink {
uint64_t GrabFrameDirectLastTime(cv::Mat& image, uint64_t lastFrameTime,
double timeout = 0.225);
/**
* Get the last time a frame was grabbed. This uses the same time base as
* wpi::Now().
*
* @return Time in 1 us increments.
*/
[[nodiscard]]
uint64_t LastFrameTime();
/**
* Get the time source for the timestamp the last frame was grabbed at.
*
* @return Time source
*/
[[nodiscard]]
WPI_TimestampSource LastFrameTimeSource();
private:
constexpr int GetCvFormat(WPI_PixelFormat pixelFormat);
@@ -405,6 +423,14 @@ inline uint64_t CvSink::GrabFrameDirectLastTime(cv::Mat& image,
return timestamp;
}
inline uint64_t CvSink::LastFrameTime() {
return rawFrame.timestamp;
}
inline WPI_TimestampSource CvSink::LastFrameTimeSource() {
return static_cast<WPI_TimestampSource>(rawFrame.timestampSrc);
}
} // namespace cs
#endif // CSCORE_CSCORE_CV_H_

45
cscore/src/main/native/linux/UsbCameraImpl.cpp
View File

@@ -555,8 +555,51 @@ void UsbCameraImpl::CameraThreadMain() {
good = false;
}
if (good) {
Frame::Time frameTime{wpi::Now()};
WPI_TimestampSource timeSource{WPI_TIMESRC_FRAME_DEQUEUE};
// check the timestamp time
auto tsFlags = buf.flags & V4L2_BUF_FLAG_TIMESTAMP_MASK;
SDEBUG4("Flags {}", tsFlags);
if (tsFlags == V4L2_BUF_FLAG_TIMESTAMP_UNKNOWN) {
SDEBUG4("Got unknown time for frame - default to wpi::Now");
} else if (tsFlags == V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC) {
SDEBUG4("Got valid monotonic time for frame");
// we can't go directly to frametime, since the rest of cscore
// expects us to use wpi::Now, which is in an arbitrary timebase
// (see timestamp.cpp). Best I can do is (approximately) translate
// between timebases
// grab current time in the same timebase as buf.timestamp
struct timespec ts;
if (clock_gettime(CLOCK_MONOTONIC, &ts) == 0) {
int64_t nowTime = {ts.tv_sec * 1'000'000 + ts.tv_nsec / 1000};
int64_t bufTime = {buf.timestamp.tv_sec * 1'000'000 +
buf.timestamp.tv_usec};
// And offset frameTime by the latency
int64_t offset{nowTime - bufTime};
frameTime -= offset;
// Figure out the timestamp's source
int tsrcFlags = buf.flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
if (tsrcFlags == V4L2_BUF_FLAG_TSTAMP_SRC_EOF) {
timeSource = WPI_TIMESRC_V4L_EOF;
} else if (tsrcFlags == V4L2_BUF_FLAG_TSTAMP_SRC_SOE) {
timeSource = WPI_TIMESRC_V4L_SOE;
} else {
timeSource = WPI_TIMESRC_UNKNOWN;
}
SDEBUG4("Frame was {} uS old, flags {}, source {}", offset,
tsrcFlags, static_cast<int>(timeSource));
} else {
// Can't do anything if we can't access the clock, leave default
}
} else if (tsFlags == V4L2_BUF_FLAG_TIMESTAMP_COPY) {
SDEBUG4("Got valid copy time for frame - default to wpi::Now");
}
PutFrame(static_cast<VideoMode::PixelFormat>(m_mode.pixelFormat),
width, height, image, wpi::Now()); // TODO: time
width, height, image, frameTime, timeSource);
}
}