17#include <opencv2/core/core.hpp>
18#include <opencv2/imgproc/imgproc.hpp>
19#include <opencv2/core/core_c.h>
42 if (config.
check(key)) {
43 return config.
find(key).asFloat64();
53 case cameraFeature_id_t::YARP_FEATURE_BRIGHTNESS:
55 case cameraFeature_id_t::YARP_FEATURE_SHUTTER:
56 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
58 case cameraFeature_id_t::YARP_FEATURE_SHARPNESS:
60 case cameraFeature_id_t::YARP_FEATURE_HUE:
62 case cameraFeature_id_t::YARP_FEATURE_SATURATION:
64 case cameraFeature_id_t::YARP_FEATURE_GAMMA:
66 case cameraFeature_id_t::YARP_FEATURE_GAIN:
68 case cameraFeature_id_t::YARP_FEATURE_IRIS:
124 use_exposure_absolute =
false;
134 configIntrins =
false;
148int V4L_camera::convertV4L_to_YARP_format(
int format)
182void V4L_camera::populateConfigurations()
193 frmsize.pixel_format = fmt.pixelformat;
200 frmival.pixel_format = fmt.pixelformat;
209 c.framerate = (1.0 *
frmival.discrete.denominator) /
frmival.discrete.numerator;
229 if (!fromConfig(config)) {
249 if (-1 == param.
fd) {
264 if (-1 == param.
fd) {
273 enumerate_controls();
282 populateConfigurations();
285 setFeature(cameraFeature_id_t::YARP_FEATURE_GAIN, checkDouble(config,
"gain"));
286 setFeature(cameraFeature_id_t::YARP_FEATURE_EXPOSURE, checkDouble(config,
"exposure"));
287 setFeature(cameraFeature_id_t::YARP_FEATURE_BRIGHTNESS, checkDouble(config,
"brightness"));
288 setFeature(cameraFeature_id_t::YARP_FEATURE_SHARPNESS, checkDouble(config,
"sharpness"));
293 setFeature(cameraFeature_id_t::YARP_FEATURE_HUE, checkDouble(config,
"hue"));
294 setFeature(cameraFeature_id_t::YARP_FEATURE_SATURATION, checkDouble(config,
"saturation"));
295 setFeature(cameraFeature_id_t::YARP_FEATURE_GAMMA, checkDouble(config,
"gamma"));
296 setFeature(cameraFeature_id_t::YARP_FEATURE_SHUTTER, checkDouble(config,
"shutter"));
297 setFeature(cameraFeature_id_t::YARP_FEATURE_IRIS, checkDouble(config,
"iris"));
331 bool res = deviceInit();
335 return ReturnValue::return_code::return_value_error_method_failed;
342 bool b = configFx && configFy;
344 return ReturnValue::return_code::return_value_error_method_failed;
350 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
371 return ReturnValue::return_code::return_value_error_method_failed;
378 if (config.
check(
"verbose")) {
382 if (!config.
check(
"width")) {
389 if (!config.
check(
"height")) {
396 if (!config.
check(
"framerate")) {
400 param.
fps = config.
find(
"framerate").asInt32();
403 if (!config.
check(
"d")) {
404 yCError(
USBCAMERA) <<
"No camera identifier was specified! (e.g. '--d /dev/video0' on Linux OS)";
411 if (!config.
check(
"camModel")) {
412 yCInfo(
USBCAMERA) <<
"No 'camModel' was specified, working with 'standard' uvc";
415 std::map<std::string, supported_cams>::iterator
it = camMap.find(config.
find(
"camModel").asString());
416 if (
it != camMap.end()) {
418 yCDebug(
USBCAMERA) <<
"cam model name : " << config.
find(
"camModel").asString() <<
" -- number : " <<
it->second;
422 for (
it = camMap.begin();
it != camMap.end();
it++) {
431 yCDebug(
USBCAMERA) <<
"-------------------------------\nusbCamera: Using leopard camera!!";
432 bit_shift = config.
check(
"shift",
Value(bit_shift),
"right shift of <n> bits").asInt32();
433 bit_bayer = config.
check(
"bit_bayer",
Value(bit_bayer),
"uses <n> bits bayer conversion").asInt32();
448 yCError(
USBCAMERA) <<
"bayer conversion with " << bit_bayer <<
"not supported";
453 yCDebug(
USBCAMERA) << bit_shift <<
"bits of right shift applied to raw data";
458 if (config.
check(
"crop")) {
465 Value isDual = config.
check(
"dual",
Value(0),
"Is this a dual camera? Two cameras merged into a single frame");
467 if (config.
find(
"dual").asBool()) {
475 if (!config.
check(
"pixelType")) {
480 type = config.
find(
"pixelType").asInt32();
486 param.
pixelType = convertV4L_to_YARP_format(param.
src_fmt.fmt.pix.pixelformat);
495 yCError(
USBCAMERA,
"no valid pixel format found!! This should not happen!!");
501 configFx = config.
check(
"horizontalFov");
502 configFy = config.
check(
"verticalFov");
503 configPPx = config.
check(
"principalPointX");
504 configPPy = config.
check(
"principalPointY");
505 configRet = config.
check(
"rectificationMatrix");
506 configDistM = config.
check(
"distortionModel");
508 bt = config.
findGroup(
"cameraDistortionModelGroup");
514 "fields k1, k2, k3, t1, t2, name are required when using cameraDistortionModelGroup";
515 configIntrins =
false;
519 configIntrins =
true;
522 configIntrins =
false;
524 param.
horizontalFov = config.
check(
"horizontalFov",
Value(0.0),
"desired horizontal fov of test image").asFloat64();
525 param.
verticalFov = config.
check(
"verticalFov",
Value(0.0),
"desired vertical fov of test image").asFloat64();
526 if (config.
check(
"mirror")) {
529 "mirroring disabled by default")
540 *
retM = config.
check(
"rectificationMatrix", *
retM,
"Matrix that describes the lens' distortion");
542 for (
size_t i = 0; i<
retM->asList()->size(); i++)
567int V4L_camera::getfd()
572bool V4L_camera::threadInit()
581void V4L_camera::run()
583 if (full_FrameRead()) {
590 if ((timeElapsed = timeNow - timeStart) > 1.0f) {
591 yCInfo(
USBCAMERA,
"frames acquired %d in %f sec", frameCounter, timeElapsed);
597void V4L_camera::threadRelease()
606bool V4L_camera::deviceInit()
671 if (_v4lconvert_data ==
nullptr) {
672 yCError(
USBCAMERA) <<
"Failed to initialize v4lconvert. Conversion to required format may not work";
706 yCWarning(
USBCAMERA) <<
"Conversion from HW supported configuration into user requested format will require addictional step.\n"
707 <<
"Performance issue may arise.";
756 if (param.
fps != -1) {
761 frameint.parm.capture.timeperframe.numerator = 1;
762 frameint.parm.capture.timeperframe.denominator = param.
fps;
791 readInit(param.
src_fmt.fmt.pix.sizeimage);
799 userptrInit(param.
src_fmt.fmt.pix.sizeimage);
811bool V4L_camera::deviceUninit()
848 if (param.
buffers !=
nullptr) {
884 if (param.
fd != -1) {
920 return ReturnValue::return_code::return_value_error_method_failed;
939 return ReturnValue::return_code::return_value_error_method_failed;
976int V4L_camera::xioctl(
int fd,
int request,
void*
argp)
994void V4L_camera::enumerate_menu()
1012bool V4L_camera::enumerate_controls()
1063bool V4L_camera::full_FrameRead()
1078 for (
unsigned int i = 0; i < count; i++) {
1086 r = select(param.
fd + 1, &
fds,
nullptr,
nullptr, &
tv);
1121bool V4L_camera::frameRead()
1134 timeStamp.
update(toEpochOffset + buf.timestamp.tv_sec + buf.timestamp.tv_usec / 1000000.0);
1158 timeStamp.
update(toEpochOffset + buf.timestamp.tv_sec + buf.timestamp.tv_usec / 1000000.0);
1181 if (buf.m.userptr == (
unsigned long)param.
buffers[i].start && buf.length == param.
buffers[i].length) {
1192 timeStamp.
update(toEpochOffset + buf.timestamp.tv_sec + buf.timestamp.tv_usec / 1000000.0);
1212void V4L_camera::imagePreProcess()
1227 param.
src_fmt.fmt.pix.bytesperline = param.
src_fmt.fmt.pix.width;
1228 param.
src_fmt.fmt.pix.pixelformat = pixel_fmt_leo;
1241void V4L_camera::imageProcess()
1243 static bool initted =
false;
1260 if ((err % 20) == 0) {
1297 timeTot += timeElapsed;
1299 if ((myCounter % 60) == 0) {
1311void V4L_camera::captureStop()
1335void V4L_camera::captureStart()
1376 buf.m.userptr = (
unsigned long)param.
buffers[i].start;
1377 buf.length = param.
buffers[i].length;
1395bool V4L_camera::readInit(
unsigned int buffer_size)
1399 if (param.
buffers ==
nullptr) {
1404 param.
buffers[0].length = buffer_size;
1407 if (param.
buffers[0].start ==
nullptr) {
1414bool V4L_camera::mmapInit()
1432 if (param.
req.count < 1) {
1437 if (param.
req.count == 1) {
1438 yCError(
USBCAMERA,
"Only 1 buffer was available, you may encounter performance issue acquiring images from device %s", param.
deviceId.c_str());
1443 if (param.
buffers ==
nullptr) {
1471bool V4L_camera::userptrInit(
unsigned int buffer_size)
1495 if (param.
buffers ==
nullptr) {
1564bool V4L_camera::check_V4L2_control(
uint32_t id)
1639 case cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE:
1645 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
1663 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
1664 if (use_exposure_absolute) {
1672 ret = set_V4L2_control(convertYARP_to_V4L(
feature), value);
1676 return ReturnValue::return_code::return_value_error_method_failed;
1683 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
1684 if (use_exposure_absolute) {
1692 tmp = get_V4L2_control(convertYARP_to_V4L(
feature));
1697 return ReturnValue::return_code::return_value_error_method_failed;
1706 if (
feature == cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE) {
1713 else return ReturnValue::return_code::return_value_error_method_failed;
1715 return ReturnValue::return_code::return_value_error_method_failed;
1720 if (
feature == cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE) {
1723 bool b = !((value1 == -1) || (value2 == -1));
1725 else return ReturnValue::return_code::return_value_error_method_failed;
1727 return ReturnValue::return_code::return_value_error_method_failed;
1736 case cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE:
1737 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
1763 case cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE:
1770 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
1803 return ReturnValue::return_code::return_value_error_method_failed;
1811 case cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE:
1822 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
1843 case cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE:
1847 case cameraFeature_id_t::YARP_FEATURE_BRIGHTNESS:
1851 case cameraFeature_id_t::YARP_FEATURE_GAIN:
1855 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
1859 case cameraFeature_id_t::YARP_FEATURE_HUE:
1872 if (
feature == cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE) {
1877 if (
feature == cameraFeature_id_t::YARP_FEATURE_EXPOSURE) {
1888 case cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE:
1903 case cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE:
1904 if (
mode == FeatureMode::MODE_AUTO) {
1911 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
1916 if (
mode == FeatureMode::MODE_AUTO) {
1922 ret =
mode != FeatureMode::MODE_AUTO;
1926 case cameraFeature_id_t::YARP_FEATURE_GAIN:
1927 if (
mode == FeatureMode::MODE_AUTO) {
1936 case cameraFeature_id_t::YARP_FEATURE_BRIGHTNESS:
1942 if (
mode == FeatureMode::MODE_AUTO) {
1948 ret =
mode != FeatureMode::MODE_AUTO;
1953 case cameraFeature_id_t::YARP_FEATURE_HUE:
1954 if (
mode == FeatureMode::MODE_AUTO) {
1967 return ReturnValue::return_code::return_value_error_method_failed;
1974 case cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE:
1981 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
1985 mode = FeatureMode::MODE_MANUAL;
1990 mode = FeatureMode::MODE_MANUAL;
1992 mode = FeatureMode::MODE_AUTO;
1997 case cameraFeature_id_t::YARP_FEATURE_BRIGHTNESS:
2001 mode = FeatureMode::MODE_MANUAL;
2008 case cameraFeature_id_t::YARP_FEATURE_GAIN:
2012 mode = FeatureMode::MODE_MANUAL;
2019 case cameraFeature_id_t::YARP_FEATURE_HUE:
2023 mode = FeatureMode::MODE_MANUAL;
2031 mode = FeatureMode::MODE_MANUAL;
2040 if (
feature == cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE) {
2047 return ReturnValue::return_code::return_value_error_method_failed;
2051 return ReturnValue::return_code::return_value_error_method_failed;
@ VOCAB_PIXEL_ENCODING_BAYER_BGGR16
@ VOCAB_PIXEL_ENCODING_BAYER_BGGR8
@ VOCAB_PIXEL_ENCODING_BAYER_RGGB8
@ VOCAB_PIXEL_ENCODING_BAYER_GRBG8
@ VOCAB_PIXEL_ENCODING_BAYER_GBRG8
const yarp::os::LogComponent & USBCAMERA()
struct v4l2_queryctrl queryctrl
static double getEpochTimeShift()
struct v4l2_querymenu querymenu
#define DEFAULT_FRAMERATE
#define VIDIOC_REQBUFS_COUNT
int getRgbWidth() override
Return the width of each frame.
yarp::dev::ReturnValue getRgbResolution(int &width, int &height) override
Get the resolution of the rgb image from the camera.
yarp::dev::ReturnValue setRgbFOV(double horizontalFov, double verticalFov) override
Set the field of view (FOV) of the rgb camera.
yarp::dev::ReturnValue getFeature(yarp::dev::cameraFeature_id_t feature, double &value) override
Get the current value for the requested feature.
yarp::dev::ReturnValue getRgbSupportedConfigurations(std::vector< yarp::dev::CameraConfig > &configurations) override
Get the possible configurations of the camera.
yarp::os::Stamp getLastInputStamp() override
Return the time stamp relative to the last acquisition.
yarp::dev::ReturnValue setMode(yarp::dev::cameraFeature_id_t feature, yarp::dev::FeatureMode mode) override
Set the requested mode for the feature.
yarp::dev::ReturnValue hasAuto(yarp::dev::cameraFeature_id_t feature, bool &hasAuto) override
Check if the requested feature has the 'auto' mode.
yarp::dev::ReturnValue getImage(yarp::sig::ImageOf< yarp::sig::PixelRgb > &image) override
Get an image from the frame grabber.
yarp::dev::ReturnValue setRgbMirroring(bool mirror) override
Set the mirroring setting of the sensor.
yarp::dev::ReturnValue hasManual(yarp::dev::cameraFeature_id_t feature, bool &hasManual) override
Check if the requested feature has the 'manual' mode.
yarp::dev::ReturnValue setActive(yarp::dev::cameraFeature_id_t feature, bool onoff) override
Set the requested feature on or off.
bool close() override
close device
yarp::dev::ReturnValue setRgbResolution(int width, int height) override
Set the resolution of the rgb image from the camera.
int getRgbHeight() override
Return the height of each frame.
yarp::dev::ReturnValue getActive(yarp::dev::cameraFeature_id_t feature, bool &isActive) override
Get the current status of the feature, on or off.
yarp::dev::ReturnValue getRgbFOV(double &horizontalFov, double &verticalFov) override
Get the field of view (FOV) of the rgb camera.
int width() const override
Return the width of each frame.
yarp::dev::ReturnValue hasOnOff(yarp::dev::cameraFeature_id_t feature, bool &HasOnOff) override
Check if the camera has the ability to turn on/off the requested feature.
yarp::dev::ReturnValue getMode(yarp::dev::cameraFeature_id_t feature, yarp::dev::FeatureMode &mode) override
Get the current mode for the feature.
yarp::dev::ReturnValue setFeature(yarp::dev::cameraFeature_id_t feature, double value) override
Set the requested feature to a value (saturation, brightness ... )
yarp::dev::ReturnValue hasOnePush(yarp::dev::cameraFeature_id_t feature, bool &hasOnePush) override
Check if the requested feature has the 'onePush' mode.
yarp::dev::ReturnValue getRgbIntrinsicParam(yarp::sig::IntrinsicParams &intrinsic) override
Get the intrinsic parameters of the rgb camera.
yarp::dev::ReturnValue getRgbMirroring(bool &mirror) override
Get the mirroring setting of the sensor.
int height() const override
Return the height of each frame.
yarp::dev::ReturnValue setOnePush(yarp::dev::cameraFeature_id_t feature) override
Set the requested feature to a value (saturation, brightness ... )
yarp::dev::ReturnValue hasFeature(yarp::dev::cameraFeature_id_t feature, bool &hasFeature) override
Check if camera has the requested feature (saturation, brightness ... )
bool open(yarp::os::Searchable &config) override
open device
yarp::dev::ReturnValue getCameraDescription(yarp::dev::CameraDescriptor &camera) override
Get a basic description of the camera hw.
virtual std::string id() const
Return the id assigned to the PolyDriver.
FeatureMode toFeatureMode(bool _auto)
A simple collection of objects that can be described and transmitted in a portable way.
bool check(const std::string &key) const override
Check if there exists a property of the given name.
bool isNull() const override
Checks if the object is invalid.
Value & find(const std::string &key) const override
Gets a value corresponding to a given keyword.
A mini-server for performing network communication in the background.
BufferedPort()
Constructor.
An abstraction for a periodic thread.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
A base class for nested structures that can be searched.
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
virtual Bottle & findGroup(const std::string &key) const =0
Gets a list corresponding to a given keyword.
void wait()
Decrement the counter, even if we must wait to do that.
void post()
Increment the counter.
An abstraction for a time stamp and/or sequence number.
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
A single value (typically within a Bottle).
static Value * makeList()
Create a list Value.
bool isNull() const override
Checks if the object is invalid.
static int32_t fromString(const std::string &input)
CameraDistortionType type
yarp::sig::DistortionModelData distortionModel
yarp::sig::Vector rectificationMatrix3X3
#define YARP_NULLPTR
Expands to either the standard nullptr or to 0 elsewhere.
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCWarning(component,...)
#define yCDebug(component,...)
void list_cap_v4l2(int fd)
void query_current_image_fmt_v4l2(int fd)
For streams capable of holding different kinds of content, check what they actually have.
double now()
Return the current time in seconds, relative to an arbitrary starting point.
void delay(double seconds)
Wait for a certain number of seconds.
An interface to the operating system, including Port based communication.
int stat(const char *path)
Portable wrapper for the stat() function.
struct v4l2_format dst_fmt
yarp::sig::IntrinsicParams intrinsic
unsigned char * read_image
struct v4l2_format src_fmt
unsigned char * raw_image
unsigned char * dst_image_rgb
std::vector< yarp::dev::CameraConfig > configurations
struct v4l2_requestbuffers req
unsigned char * src_image
unsigned int dst_image_size_rgb
unsigned int src_image_size
unsigned int raw_image_size
std::string deviceDescription
The IntrinsicParams struct to handle the intrinsic parameter of cameras(RGB and RGBD either).