Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11:43 +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

19
cscore/src/main/java/edu/wpi/first/cscore/CvSink.java
View File

@@ -6,6 +6,7 @@ package edu.wpi.first.cscore;
import edu.wpi.first.util.PixelFormat;
import edu.wpi.first.util.RawFrame;
import edu.wpi.first.util.TimestampSource;
import java.nio.ByteBuffer;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
@@ -220,4 +221,22 @@ public class CvSink extends ImageSink {
}
return rv;
}
/**
* Get the last time a frame was grabbed. This uses the same time base as wpi::Now().
*
* @return Time in 1 us increments.
*/
public long getLastFrameTime() {
return m_frame.getTimestamp();
}
/**
* Get the time source for the timestamp the last frame was grabbed at.
*
* @return Time source
*/
public TimestampSource getLastFrameTimeSource() {
return m_frame.getTimestampSource();
}
}

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

42
wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java
View File

@@ -18,6 +18,8 @@ public class RawFrame implements AutoCloseable {
private int m_height;
private int m_stride;
private PixelFormat m_pixelFormat = PixelFormat.kUnknown;
private long m_time;
private TimestampSource m_timeSource = TimestampSource.kUnknown;
/** Construct a new empty RawFrame. */
public RawFrame() {
@@ -43,12 +45,15 @@ public class RawFrame implements AutoCloseable {
* @param stride The number of bytes in each row of image data
* @param pixelFormat The PixelFormat of the frame
*/
void setDataJNI(ByteBuffer data, int width, int height, int stride, int pixelFormat) {
void setDataJNI(
ByteBuffer data, int width, int height, int stride, int pixelFormat, long time, int timeSrc) {
m_data = data;
m_width = width;
m_height = height;
m_stride = stride;
m_pixelFormat = PixelFormat.getFromInt(pixelFormat);
m_time = time;
m_timeSource = TimestampSource.getFromInt(timeSrc);
}
/**
@@ -59,11 +64,13 @@ public class RawFrame implements AutoCloseable {
* @param stride The number of bytes in each row of image data
* @param pixelFormat The PixelFormat of the frame
*/
void setInfoJNI(int width, int height, int stride, int pixelFormat) {
void setInfoJNI(int width, int height, int stride, int pixelFormat, long time, int timeSrc) {
m_width = width;
m_height = height;
m_stride = stride;
m_pixelFormat = PixelFormat.getFromInt(pixelFormat);
m_time = time;
m_timeSource = TimestampSource.getFromInt(timeSrc);
}
/**
@@ -110,6 +117,19 @@ public class RawFrame implements AutoCloseable {
pixelFormat.getValue());
}
/**
* Update this frame's timestamp info.
*
* @param frameTime the time this frame was grabbed at. This uses the same time base as
* wpi::Now(), in us.
* @param frameTimeSource the time source for the timestamp this frame was grabbed at.
*/
public void setTimeInfo(long frameTime, TimestampSource frameTimeSource) {
m_time = frameTime;
m_timeSource = frameTimeSource;
WPIUtilJNI.setRawFrameTime(m_nativeObj, frameTime, frameTimeSource.getValue());
}
/**
* Get the pointer to native representation of this frame.
*
@@ -185,4 +205,22 @@ public class RawFrame implements AutoCloseable {
public PixelFormat getPixelFormat() {
return m_pixelFormat;
}
/**
* Get the time this frame was grabbed at. This uses the same time base as wpi::Now(), in us.
*
* @return Time in 1 us increments.
*/
public long getTimestamp() {
return m_time;
}
/**
* Get the time source for the timestamp this frame was grabbed at.
*
* @return Time source
*/
public TimestampSource getTimestampSource() {
return m_timeSource;
}
}

51
wpiutil/src/main/java/edu/wpi/first/util/TimestampSource.java Normal file
View File

@@ -0,0 +1,51 @@
// 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.util;
/**
* Options for where the timestamp an {@link RawFrame} was captured at can be measured relative to.
*/
public enum TimestampSource {
/** unknown. */
kUnknown(0),
/**
* wpi::Now when the new frame was dequeued by CSCore. Does not account for camera exposure time
* or V4L latency.
*/
kFrameDequeue(1),
/** End of Frame. Same as V4L2_BUF_FLAG_TSTAMP_SRC_EOF, translated into wpi::Now's timebase. */
kV4LEOF(2),
/**
* Start of Exposure. Same as V4L2_BUF_FLAG_TSTAMP_SRC_SOE, translated into wpi::Now's timebase.
*/
kV4LSOE(3);
private final int value;
TimestampSource(int value) {
this.value = value;
}
/**
* Gets the integer value of the pixel format.
*
* @return Integer value
*/
public int getValue() {
return value;
}
private static final TimestampSource[] s_values = values();
/**
* Gets a TimestampSource enum value from its integer value.
*
* @param timestampSource integer value
* @return Enum value
*/
public static TimestampSource getFromInt(int timestampSource) {
return s_values[timestampSource];
}
}

2
wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java
View File

@@ -175,6 +175,8 @@ public class WPIUtilJNI {
static native void setRawFrameInfo(
long frame, int size, int width, int height, int stride, int pixelFormat);
static native void setRawFrameTime(long frame, long timestamp, int timeSource);
/**
* Waits for a handle to be signaled.
*

18
wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp
View File

@@ -426,6 +426,24 @@ Java_edu_wpi_first_util_WPIUtilJNI_setRawFrameData
f->pixelFormat = pixelFormat;
}
/*
* Class: edu_wpi_first_util_WPIUtilJNI
* Method: setRawFrameTime
* Signature: (JJI)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_util_WPIUtilJNI_setRawFrameTime
(JNIEnv* env, jclass, jlong frame, jlong time, jint timeSource)
{
auto* f = reinterpret_cast<wpi::RawFrame*>(frame);
if (!f) {
wpi::ThrowNullPointerException(env, "frame is null");
return;
}
f->timestamp = time;
f->timestampSrc = timeSource;
}
/*
* Class: edu_wpi_first_util_WPIUtilJNI
* Method: setRawFrameInfo

47
wpiutil/src/main/native/include/wpi/RawFrame.h
View File

@@ -34,13 +34,15 @@ typedef struct WPI_RawFrame { // NOLINT
uint8_t* data;
// function to free image data (may be NULL)
void (*freeFunc)(void* cbdata, void* data, size_t capacity);
void* freeCbData; // data passed to freeFunc
size_t capacity; // data buffer capacity, in bytes
size_t size; // actual size of data, in bytes
int pixelFormat; // WPI_PixelFormat
int width; // width of image, in pixels
int height; // height of image, in pixels
int stride; // size of each row of data, in bytes (may be 0)
void* freeCbData; // data passed to freeFunc
size_t capacity; // data buffer capacity, in bytes
size_t size; // actual size of data, in bytes
int pixelFormat; // WPI_PixelFormat
int width; // width of image, in pixels
int height; // height of image, in pixels
int stride; // size of each row of data, in bytes (may be 0)
uint64_t timestamp; // image capture timestamp
int timestampSrc; // WPI_TimestampSource
} WPI_RawFrame;
/**
@@ -58,6 +60,21 @@ enum WPI_PixelFormat {
WPI_PIXFMT_BGRA, // BGRA 8-8-8-8-, 32 bpp
};
/**
* Timestamp metadata. Timebase is the same as wpi::Now
*/
enum WPI_TimestampSource {
WPI_TIMESRC_UNKNOWN = 0, // unknown
WPI_TIMESRC_FRAME_DEQUEUE, // wpi::Now when the new frame was dequeued by
// CSCore. Does not account for camera exposure
// time or V4L latency.
WPI_TIMESRC_V4L_EOF, // End of Frame. Same as V4L2_BUF_FLAG_TSTAMP_SRC_EOF,
// translated into wpi::Now's timebase.
WPI_TIMESRC_V4L_SOE, // Start of Exposure. Same as
// V4L2_BUF_FLAG_TSTAMP_SRC_SOE, translated into
// wpi::Now's timebase.
};
// Returns nonzero if the frame data was allocated/reallocated
int WPI_AllocateRawFrameData(WPI_RawFrame* frame, size_t requestedSize);
void WPI_FreeRawFrameData(WPI_RawFrame* frame);
@@ -82,6 +99,8 @@ struct RawFrame : public WPI_RawFrame {
pixelFormat = WPI_PIXFMT_UNKNOWN;
width = 0;
height = 0;
timestamp = 0;
timestampSrc = WPI_TIMESRC_UNKNOWN;
}
RawFrame(const RawFrame&) = delete;
RawFrame& operator=(const RawFrame&) = delete;
@@ -120,19 +139,23 @@ template <std::same_as<wpi::RawFrame> T>
void SetFrameData(JNIEnv* env, jclass rawFrameCls, jobject jframe,
const T& frame, bool newData) {
if (newData) {
static jmethodID setData = env->GetMethodID(rawFrameCls, "setDataJNI",
"(Ljava/nio/ByteBuffer;IIII)V");
static jmethodID setData = env->GetMethodID(
rawFrameCls, "setDataJNI", "(Ljava/nio/ByteBuffer;IIIIJI)V");
env->CallVoidMethod(
jframe, setData, env->NewDirectByteBuffer(frame.data, frame.size),
static_cast<jint>(frame.width), static_cast<jint>(frame.height),
static_cast<jint>(frame.stride), static_cast<jint>(frame.pixelFormat));
static_cast<jint>(frame.stride), static_cast<jint>(frame.pixelFormat),
static_cast<jlong>(frame.timestamp),
static_cast<jint>(frame.timestampSrc));
} else {
static jmethodID setInfo =
env->GetMethodID(rawFrameCls, "setInfoJNI", "(IIII)V");
env->GetMethodID(rawFrameCls, "setInfoJNI", "(IIIIJI)V");
env->CallVoidMethod(jframe, setInfo, static_cast<jint>(frame.width),
static_cast<jint>(frame.height),
static_cast<jint>(frame.stride),
static_cast<jint>(frame.pixelFormat));
static_cast<jint>(frame.pixelFormat),
static_cast<jlong>(frame.timestamp),
static_cast<jint>(frame.timestampSrc));
}
}
#endif