YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
V4L_camera.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: LGPL-2.1-or-later
4 */
5
6
7#include "V4L_camera.h"
8#include "list.h"
10
11#include <yarp/os/LogStream.h>
12#include <yarp/os/Time.h>
13#include <yarp/os/Value.h>
14
15#include <cstdio>
16#include <ctime>
17#include <opencv2/core/core.hpp>
18#include <opencv2/imgproc/imgproc.hpp>
19#include <opencv2/core/core_c.h>
20
21using namespace yarp::os;
22using namespace yarp::dev;
23using namespace yarp::sig;
24
25
26static double getEpochTimeShift()
27{
28 struct timeval epochtime;
29 struct timespec vsTime;
30
31 gettimeofday(&epochtime, nullptr);
33
34 double uptime = vsTime.tv_sec + vsTime.tv_nsec / 1000000000.0;
35 double epoch = epochtime.tv_sec + epochtime.tv_usec / 1000000.0;
36 return epoch - uptime;
37}
38
39
40double V4L_camera::checkDouble(yarp::os::Searchable& config, const char* key)
41{
42 if (config.check(key)) {
43 return config.find(key).asFloat64();
44 }
45
46 return -1.0;
47}
48
49#define NOT_PRESENT -1
50int V4L_camera::convertYARP_to_V4L(cameraFeature_id_t feature)
51{
52 switch (feature) {
53 case cameraFeature_id_t::YARP_FEATURE_BRIGHTNESS:
55 case cameraFeature_id_t::YARP_FEATURE_SHUTTER: // this maps also on exposure
56 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
57 return V4L2_CID_EXPOSURE;
58 case cameraFeature_id_t::YARP_FEATURE_SHARPNESS:
59 return V4L2_CID_SHARPNESS;
60 case cameraFeature_id_t::YARP_FEATURE_HUE:
61 return V4L2_CID_HUE;
62 case cameraFeature_id_t::YARP_FEATURE_SATURATION:
64 case cameraFeature_id_t::YARP_FEATURE_GAMMA:
65 return V4L2_CID_GAMMA;
66 case cameraFeature_id_t::YARP_FEATURE_GAIN:
67 return V4L2_CID_GAIN;
68 case cameraFeature_id_t::YARP_FEATURE_IRIS:
70 default:
71 return NOT_PRESENT;
72 // case YARP_FEATURE_WHITE_BALANCE: -> this has to e mapped on the couple V4L2_CID_BLUE_BALANCE && V4L2_CID_RED_BALANCE
73
75 // not yet implemented //
77 // case YARP_FEATURE_FOCUS: return DC1394_FEATURE_FOCUS;
78 // case YARP_FEATURE_TEMPERATURE: return DC1394_FEATURE_TEMPERATURE;
79 // case YARP_FEATURE_TRIGGER: return DC1394_FEATURE_TRIGGER;
80 // case YARP_FEATURE_TRIGGER_DELAY: return DC1394_FEATURE_TRIGGER_DELAY;
81 // case YARP_FEATURE_FRAME_RATE: return DC1394_FEATURE_FRAME_RATE;
82 // case YARP_FEATURE_ZOOM: return DC1394_FEATURE_ZOOM;
83 // case YARP_FEATURE_PAN: return DC1394_FEATURE_PAN;
84 // case YARP_FEATURE_TILT: return DC1394_FEATURE_TILT;
85 }
86 return NOT_PRESENT;
87}
88
90 PeriodicThread(1.0 / DEFAULT_FRAMERATE), doCropping(false), toEpochOffset(getEpochTimeShift())
91{
92 verbose = false;
93 param.fps = DEFAULT_FRAMERATE;
94 param.io = IO_METHOD_MMAP;
95 param.deviceId = "/dev/video0";
96 param.fd = -1;
97 param.n_buffers = 0;
98 param.buffers = nullptr;
99 param.camModel = STANDARD_UVC;
100 param.dual = false;
101
102 param.addictionalResize = false;
103 param.resizeOffset_x = 0;
104 param.resizeOffset_y = 0;
105 param.resizeWidth = 0;
106 param.resizeHeight = 0;
107
108 _v4lconvert_data = YARP_NULLPTR;
109 myCounter = 0;
110 timeTot = 0;
111
114 param.raw_image = YARP_NULLPTR;
115 param.raw_image_size = 0;
116 param.read_image = YARP_NULLPTR;
117
118 param.src_image = YARP_NULLPTR;
119 param.src_image_size = 0;
120
122 param.dst_image_size_rgb = 0;
123
124 use_exposure_absolute = false;
125 camMap["default"] = STANDARD_UVC;
126 camMap["leopard_python"] = LEOPARD_PYTHON;
127
128 configFx = false;
129 configFy = false;
130 configPPx = false;
131 configPPy = false;
132 configRet = false;
133 configDistM = false;
134 configIntrins = false;
135 configured = false;
136
137 // leopard debugging
138 pixel_fmt_leo = V4L2_PIX_FMT_SGRBG8;
139 bit_shift = 2; // after firmware update, the shift has to be 2 instead of 4
140 bit_bayer = 8;
141}
142
144{
145 return timeStamp;
146}
147
148int V4L_camera::convertV4L_to_YARP_format(int format)
149{
150 switch (format) {
152 return VOCAB_PIXEL_MONO;
153 case V4L2_PIX_FMT_Y16:
154 return VOCAB_PIXEL_MONO16;
156 return VOCAB_PIXEL_RGB;
157// case V4L2_PIX_FMT_ABGR32 : return VOCAB_PIXEL_BGRA; //unsupported by linux travis configuration
159 return VOCAB_PIXEL_BGR;
171 return VOCAB_PIXEL_YUV_420;
173 return VOCAB_PIXEL_YUV_444;
175 return VOCAB_PIXEL_YUV_422;
177 return VOCAB_PIXEL_YUV_411;
178 }
179 return NOT_PRESENT;
180}
181
182void V4L_camera::populateConfigurations()
183{
184 struct v4l2_fmtdesc fmt;
187
188 fmt.index = 0;
190
191 while (ioctl(param.fd, VIDIOC_ENUM_FMT, &fmt) >= 0) {
192 memset(&frmsize, 0, sizeof(v4l2_frmsizeenum));
193 frmsize.pixel_format = fmt.pixelformat;
194 frmsize.index = 0;
196 while (xioctl(param.fd, VIDIOC_ENUM_FRAMESIZES, &frmsize) >= 0) {
198 memset(&frmival, 0, sizeof(v4l2_frmivalenum));
199 frmival.index = 0;
200 frmival.pixel_format = fmt.pixelformat;
201 frmival.width = frmsize.discrete.width;
202 frmival.height = frmsize.discrete.height;
204 while (xioctl(param.fd, VIDIOC_ENUM_FRAMEINTERVALS, &frmival) >= 0) {
206 c.pixelCoding = (YarpVocabPixelTypesEnum)convertV4L_to_YARP_format(frmival.pixel_format);
207 c.width = frmival.width;
208 c.height = frmival.height;
209 c.framerate = (1.0 * frmival.discrete.denominator) / frmival.discrete.numerator;
210 param.configurations.push_back(c);
211 frmival.index++;
212 }
213 }
214 frmsize.index++;
215 }
216 fmt.index++;
217 }
218}
219
224{
225 struct stat st;
226 yCTrace(USBCAMERA) << "input params are " << config.toString();
227
228
229 if (!fromConfig(config)) {
230 return false;
231 }
232
233 // stat file
234 if (-1 == stat(param.deviceId.c_str(), &st)) {
235 yCError(USBCAMERA, "Cannot identify '%s': %d, %s", param.deviceId.c_str(), errno, strerror(errno));
236 return false;
237 }
238
239 // check if it is a device
240 if (!S_ISCHR(st.st_mode)) {
241 yCError(USBCAMERA, "%s is no device", param.deviceId.c_str());
242 return false;
243 }
244
245 // open device
246 param.fd = v4l2_open(param.deviceId.c_str(), O_RDWR /* required */ | O_NONBLOCK, 0);
247
248 // check if opening was successfull
249 if (-1 == param.fd) {
250 yCError(USBCAMERA, "Cannot open '%s': %d, %s", param.deviceId.c_str(), errno, strerror(errno));
251 return false;
252 }
253
254 // if previous instance crashed, maybe will help (?)
255 captureStop();
256 deviceUninit();
257 v4l2_close(param.fd);
258
260 // re-open device
261 param.fd = v4l2_open(param.deviceId.c_str(), O_RDWR /* required */ | O_NONBLOCK, 0);
262
263 // check if opening was successfull
264 if (-1 == param.fd) {
265 yCError(USBCAMERA, "Cannot open '%s': %d, %s", param.deviceId.c_str(), errno, strerror(errno));
266 return false;
267 }
268
269
270 // Initting video device
271 deviceInit();
272 if (verbose) {
273 enumerate_controls();
274 }
275 if (!check_V4L2_control(V4L2_CID_EXPOSURE)) {
276 use_exposure_absolute = check_V4L2_control(V4L2_CID_EXPOSURE_ABSOLUTE);
277 }
278 captureStart();
280 start();
281
282 populateConfigurations();
283
284 // Configure the device settings from input file
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"));
289 yarp::os::Bottle& white_balance = config.findGroup("white_balance");
290 if (!white_balance.isNull()) {
291 setFeature(cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE, white_balance.get(2).asFloat64(), white_balance.get(1).asFloat64());
292 }
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"));
298
299 return true;
300}
301
303{
304 return height();
305}
306
308{
309 return width();
310}
311
312ReturnValue V4L_camera::getRgbSupportedConfigurations(std::vector<CameraConfig>& configurations)
313{
314 configurations = param.configurations;
315 return ReturnValue_ok;
316}
318{
319 width = param.user_width;
320 height = param.user_height;
321 return ReturnValue_ok;
322}
323
325{
326 mutex.wait();
327 captureStop();
328 deviceUninit();
329 param.user_width = width;
330 param.user_height = height;
331 bool res = deviceInit();
332 captureStart();
333 mutex.post();
334 if (res) return ReturnValue_ok;
335 return ReturnValue::return_code::return_value_error_method_failed;
336}
337
338ReturnValue V4L_camera::getRgbFOV(double& horizontalFov, double& verticalFov)
339{
340 horizontalFov = param.horizontalFov;
341 verticalFov = param.verticalFov;
342 bool b = configFx && configFy;
343 if (b) return ReturnValue_ok;
344 return ReturnValue::return_code::return_value_error_method_failed;
345}
346
347ReturnValue V4L_camera::setRgbFOV(double horizontalFov, double verticalFov)
348{
349 yCError(USBCAMERA) << "cannot set fov";
350 return ReturnValue::return_code::return_value_error_not_implemented_by_device;
351}
352
358
360{
361
362 mirror = (ioctl(param.fd, V4L2_CID_HFLIP) != 0);
363 return ReturnValue_ok;
364}
365
367{
368 int ret = ioctl(param.fd, V4L2_CID_HFLIP, &mirror);
369 if (ret < 0) {
370 yCError(USBCAMERA) << "V4L2_CID_HFLIP - Unable to mirror image-" << strerror(errno);
371 return ReturnValue::return_code::return_value_error_method_failed;
372 }
373 return ReturnValue_ok;
374}
375
376bool V4L_camera::fromConfig(yarp::os::Searchable& config)
377{
378 if (config.check("verbose")) {
379 verbose = true;
380 }
381
382 if (!config.check("width")) {
383 yCDebug(USBCAMERA) << "width parameter not found, using default value of " << DEFAULT_WIDTH;
385 } else {
386 param.user_width = config.find("width").asInt32();
387 }
388
389 if (!config.check("height")) {
390 yCDebug(USBCAMERA) << "height parameter not found, using default value of " << DEFAULT_HEIGHT;
392 } else {
393 param.user_height = config.find("height").asInt32();
394 }
395
396 if (!config.check("framerate")) {
397 yCDebug(USBCAMERA) << "framerate parameter not found, using default value of " << DEFAULT_FRAMERATE;
398 param.fps = DEFAULT_FRAMERATE;
399 } else {
400 param.fps = config.find("framerate").asInt32();
401 }
402
403 if (!config.check("d")) {
404 yCError(USBCAMERA) << "No camera identifier was specified! (e.g. '--d /dev/video0' on Linux OS)";
405 return false;
406 }
407
408 param.deviceId = config.find("d").asString();
409 param.flip = config.check("flip", Value("false")).asBool();
410
411 if (!config.check("camModel")) {
412 yCInfo(USBCAMERA) << "No 'camModel' was specified, working with 'standard' uvc";
413 param.camModel = STANDARD_UVC;
414 } else {
415 std::map<std::string, supported_cams>::iterator it = camMap.find(config.find("camModel").asString());
416 if (it != camMap.end()) {
417 param.camModel = it->second;
418 yCDebug(USBCAMERA) << "cam model name : " << config.find("camModel").asString() << " -- number : " << it->second;
419 } else {
420 yCError(USBCAMERA) << "Unknown camera model <" << config.find("camModel").asString() << ">";
421 yCInfo(USBCAMERA) << "Supported models are: ";
422 for (it = camMap.begin(); it != camMap.end(); it++) {
423 yCInfo(USBCAMERA, " <%s>", it->first.c_str());
424 }
425 return false;
426 }
427 }
428
429 // Check for addictional leopard parameter for debugging purpose
430 if (param.camModel == LEOPARD_PYTHON) {
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();
434 switch (bit_bayer) {
435 case 8:
436 pixel_fmt_leo = V4L2_PIX_FMT_SGRBG8;
437 break;
438
439 case 10:
440 pixel_fmt_leo = V4L2_PIX_FMT_SGRBG10;
441 break;
442
443 case 12:
444 pixel_fmt_leo = V4L2_PIX_FMT_SGRBG12;
445 break;
446
447 default:
448 yCError(USBCAMERA) << "bayer conversion with " << bit_bayer << "not supported";
449 return false;
450 }
451
452 yCDebug(USBCAMERA) << "--------------------------------";
453 yCDebug(USBCAMERA) << bit_shift << "bits of right shift applied to raw data";
454 yCDebug(USBCAMERA) << "Bits used for de-bayer " << bit_bayer;
455 }
456
457 //crop is used to pass from 16:9 to 4:3
458 if (config.check("crop")) {
459 doCropping = true;
460 yCInfo(USBCAMERA, "Cropping enabled.");
461 } else {
462 doCropping = false;
463 }
464
465 Value isDual = config.check("dual", Value(0), "Is this a dual camera? Two cameras merged into a single frame");
466
467 if (config.find("dual").asBool()) {
468 param.dual = true;
469 yCInfo(USBCAMERA, "Using dual input camera.");
470 } else {
471 param.dual = false;
472 }
473
474 int type = 0;
475 if (!config.check("pixelType")) {
476 yCError(USBCAMERA) << "No 'pixelType' was specified!";
477 return false;
478 }
479 {
480 type = config.find("pixelType").asInt32();
481 }
482
483 switch (type) {
484 case VOCAB_PIXEL_MONO:
485 // Pixel type raw is the native one from the camera
486 param.pixelType = convertV4L_to_YARP_format(param.src_fmt.fmt.pix.pixelformat);
487 break;
488
489 case VOCAB_PIXEL_RGB:
490 // is variable param.pixelType really required??
492 break;
493
494 default:
495 yCError(USBCAMERA, "no valid pixel format found!! This should not happen!!");
496 return false;
497 break;
498 }
499 Value* retM;
500 retM = Value::makeList("1.0 0.0 0.0 0.0 1.0 0.0 0.0 0.0 1.0");
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");
507 Bottle bt;
508 bt = config.findGroup("cameraDistortionModelGroup");
509 if (!bt.isNull()) {
510 if (bt.find("name").isNull() || bt.find("k1").isNull()
511 || bt.find("k2").isNull() || bt.find("k3").isNull()
512 || bt.find("t1").isNull() || bt.find("t2").isNull()) {
513 yCError(USBCAMERA) << "group cameraDistortionModelGroup incomplete, "
514 "fields k1, k2, k3, t1, t2, name are required when using cameraDistortionModelGroup";
515 configIntrins = false;
516 return false;
517 }
518 {
519 configIntrins = true;
520 }
521 } else {
522 configIntrins = false;
523 }
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")) {
527 if (!setRgbMirroring(config.check("mirror",
528 Value(0),
529 "mirroring disabled by default")
530 .asBool())) {
531 yCError(USBCAMERA, "cannot set mirroring option");
532 return false;
533 }
534 }
535
536 param.intrinsic.focalLengthX = config.check("focalLengthX", Value(0.0), "Horizontal component of the focal lenght").asFloat64();
537 param.intrinsic.focalLengthY = config.check("focalLengthY", Value(0.0), "Vertical component of the focal lenght").asFloat64();
538 param.intrinsic.principalPointX = config.check("principalPointX", Value(0.0), "X coordinate of the principal point").asFloat64();
539 param.intrinsic.principalPointY = config.check("principalPointY", Value(0.0), "Y coordinate of the principal point").asFloat64();
540 *retM = config.check("rectificationMatrix", *retM, "Matrix that describes the lens' distortion");
541
542 for (size_t i = 0; i< retM->asList()->size(); i++)
543 {
544 param.intrinsic.rectificationMatrix3X3[i] = retM->asList()->get(i).asFloat64();
545 }
546 if (bt.isNull()) {
547 param.intrinsic.distortionModel.type = CameraDistortionType::YARP_DISTORTION_NONE;
548 param.intrinsic.distortionModel.k1 = 0.0;
549 param.intrinsic.distortionModel.k2 = 0.0;
550 param.intrinsic.distortionModel.k3 = 0.0;
551 param.intrinsic.distortionModel.t1 = 0.0;
552 param.intrinsic.distortionModel.t2 = 0.0;
553 } else {
554 param.intrinsic.distortionModel.type = (CameraDistortionType)CameraDistortionTypeConverter::fromString(bt.check("name", Value(""), "Name of the distortion model").asString());
555 param.intrinsic.distortionModel.k1 = bt.check("k1", Value(0.0), "Radial distortion coefficient of the lens").asFloat64();
556 param.intrinsic.distortionModel.k2 = bt.check("k2", Value(0.0), "Radial distortion coefficient of the lens").asFloat64();
557 param.intrinsic.distortionModel.k3 = bt.check("k3", Value(0.0), "Radial distortion coefficient of the lens").asFloat64();
558 param.intrinsic.distortionModel.t1 = bt.check("t1", Value(0.0), "Tangential distortion of the lens").asFloat64();
559 param.intrinsic.distortionModel.t2 = bt.check("t2", Value(0.0), "Tangential distortion of the lens").asFloat64();
560 }
561 delete retM;
562
563 yCDebug(USBCAMERA) << "using following device " << param.deviceId << "with the configuration: " << param.user_width << "x" << param.user_height << "; camModel is " << param.camModel;
564 return true;
565}
566
567int V4L_camera::getfd()
568{
569 return param.fd;
570}
571
572bool V4L_camera::threadInit()
573{
575
576 timeStart = timeNow = timeElapsed = yarp::os::Time::now();
577 frameCounter = 0;
578 return true;
579}
580
581void V4L_camera::run()
582{
583 if (full_FrameRead()) {
584 frameCounter++;
585 } else {
586 yCError(USBCAMERA) << "Failed acquiring new frame";
587 }
588
589 timeNow = yarp::os::Time::now();
590 if ((timeElapsed = timeNow - timeStart) > 1.0f) {
591 yCInfo(USBCAMERA, "frames acquired %d in %f sec", frameCounter, timeElapsed);
592 frameCounter = 0;
593 timeStart = timeNow;
594 }
595}
596
597void V4L_camera::threadRelease()
598{
600}
601
602
606bool V4L_camera::deviceInit()
607{
608 struct v4l2_capability cap;
609 struct v4l2_cropcap cropcap;
610 struct v4l2_crop crop;
612 configured = false;
613
614 if (-1 == xioctl(param.fd, VIDIOC_QUERYCAP, &cap)) {
615 if (EINVAL == errno) {
616 yCError(USBCAMERA, "%s is no V4L2 device", param.deviceId.c_str());
617 }
618 return false;
619 }
620
621 if (verbose) {
622 list_cap_v4l2(param.fd);
623 }
624
625 if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
626 yCError(USBCAMERA, "%s is no video capture device", param.deviceId.c_str());
627 return false;
628 }
629
630 yCInfo(USBCAMERA, "%s is good V4L2_CAP_VIDEO_CAPTURE", param.deviceId.c_str());
631
632 switch (param.io) {
633 case IO_METHOD_READ:
634 if (!(cap.capabilities & V4L2_CAP_READWRITE)) {
635 yCError(USBCAMERA, "%s does not support read i/o", param.deviceId.c_str());
636 return false;
637 }
638 break;
639
640 case IO_METHOD_MMAP:
642 if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
643 yCError(USBCAMERA, "%s does not support streaming i/o", param.deviceId.c_str());
644 return false;
645 }
646 break;
647
648 default:
649 yCError(USBCAMERA, "Unknown io method for device %s", param.deviceId.c_str());
650 return false;
651 break;
652 }
653
654 CLEAR(cropcap);
656
657 if (0 == xioctl(param.fd, VIDIOC_CROPCAP, &cropcap)) {
658 crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
659 crop.c = cropcap.defrect; /* reset to default */
660
661 /* Reset cropping to default if possible.
662 * Don't care about errors
663 */
664 xioctl(param.fd, VIDIOC_S_CROP, &crop);
665 }
666
667 CLEAR(param.src_fmt);
668 CLEAR(param.dst_fmt);
669
670 _v4lconvert_data = v4lconvert_create(param.fd);
671 if (_v4lconvert_data == nullptr) {
672 yCError(USBCAMERA) << "Failed to initialize v4lconvert. Conversion to required format may not work";
673 }
674
675 /*
676 * dst_fmt is the image format the user require.
677 * With try_format, V4l does an handshake with the camera and the best match from
678 * the available formats provided by the camera is selected.
679 * src_fmt will contain the source format, i.e. the configuration to be sent to the
680 * camera to optimize the conversion which will be done afterwards.
681 *
682 * VERY IMPORTANT NOTE:
683 *
684 * In case no match is found for the user input provided in dst_fmt, than dst_fmt
685 * itself may be changed to provide the best conversion possible similar to user
686 * input. In particular, pixel format conversion together with rescaling may not
687 * be possible to achieve. In this case only pixel format conversion will be done
688 * and we need to take care of the rescaling.
689 */
690
692 param.dst_fmt.fmt.pix.width = param.user_width;
693 param.dst_fmt.fmt.pix.height = param.user_height;
694 param.dst_fmt.fmt.pix.field = V4L2_FIELD_NONE;
695 param.dst_fmt.fmt.pix.pixelformat = param.pixelType;
696
697 if (v4lconvert_try_format(_v4lconvert_data, &(param.dst_fmt), &(param.src_fmt)) != 0) {
698 yCError(USBCAMERA, "v4lconvert_try_format -> Error is: %s", v4lconvert_get_error_message(_v4lconvert_data));
699 return false;
700 }
701
702 // Check if dst_fmt has been changed by the v4lconvert_try_format
703 if (param.dst_fmt.fmt.pix.width != param.user_width ||
704 param.dst_fmt.fmt.pix.height != param.user_height ||
705 param.dst_fmt.fmt.pix.pixelformat != param.pixelType) {
706 yCWarning(USBCAMERA) << "Conversion from HW supported configuration into user requested format will require addictional step.\n"
707 << "Performance issue may arise.";
708
709 param.addictionalResize = true;
710
711 // Compute offsets for cropping image in case the source image and the one
712 // required by the user have different form factors, i.e 16/9 vs 4/3
713 double inputFF = (double)param.dst_fmt.fmt.pix.width / (double)param.dst_fmt.fmt.pix.height;
714 double outputFF = (double)param.user_width / (double)param.user_height;
715
716 if (outputFF < inputFF) {
717 // Use all vertical pixels, crop lateral pixels to get the central portion of the image
718 param.resizeOffset_y = 0;
719 param.resizeHeight = param.dst_fmt.fmt.pix.height;
720
721 if (!param.dual) {
722 param.resizeOffset_x = (param.dst_fmt.fmt.pix.width - (param.dst_fmt.fmt.pix.height * outputFF)) / 2;
723 param.resizeWidth = param.dst_fmt.fmt.pix.width - param.resizeOffset_x * 2;
724 } else {
725 param.resizeOffset_x = (param.dst_fmt.fmt.pix.width - (param.dst_fmt.fmt.pix.height * outputFF)) / 4; // "/4" is "/2" 2 times because there are 2 images
726 param.resizeWidth = param.dst_fmt.fmt.pix.width / 2 - param.resizeOffset_x * 2;
727 }
728 } else {
729 // Use all horizontal pixels, crop top/bottom pixels to get the central portion of the image
730 param.resizeOffset_x = 0;
731
732 if (!param.dual) {
733 param.resizeWidth = param.dst_fmt.fmt.pix.width;
734 param.resizeOffset_y = (param.dst_fmt.fmt.pix.height - (param.dst_fmt.fmt.pix.width / outputFF)) / 2;
735 param.resizeHeight = param.dst_fmt.fmt.pix.height - param.resizeOffset_y * 2;
736 } else {
737 param.resizeWidth = param.dst_fmt.fmt.pix.width / 2;
738 param.resizeOffset_y = (param.dst_fmt.fmt.pix.height - (param.dst_fmt.fmt.pix.width / outputFF)) / 2;
739 param.resizeHeight = param.dst_fmt.fmt.pix.height - param.resizeOffset_y * 2;
740 }
741 }
742 } else {
743 param.addictionalResize = false;
744 param.resizeOffset_x = 0;
745 param.resizeWidth = param.user_width / 2;
746 param.resizeOffset_y = 0;
747 param.resizeHeight = param.user_height;
748 }
749
750 if (-1 == xioctl(param.fd, VIDIOC_S_FMT, &param.src_fmt)) {
751 yCError(USBCAMERA) << "xioctl error VIDIOC_S_FMT" << strerror(errno);
752 return false;
753 }
754
755 /* If the user has set the fps to -1, don't try to set the frame interval */
756 if (param.fps != -1) {
758
759 /* Attempt to set the frame interval. */
761 frameint.parm.capture.timeperframe.numerator = 1;
762 frameint.parm.capture.timeperframe.denominator = param.fps;
763 if (-1 == xioctl(param.fd, VIDIOC_S_PARM, &frameint)) {
764 yCError(USBCAMERA, "Unable to set frame interval.");
765 }
766 }
767
768 param.src_image_size = param.src_fmt.fmt.pix.sizeimage;
769 param.src_image = new unsigned char[param.src_image_size];
770
771 param.dst_image_size_rgb = param.dst_fmt.fmt.pix.width * param.dst_fmt.fmt.pix.height * 3;
772 param.dst_image_rgb = new unsigned char[param.dst_image_size_rgb];
773
774 // raw image is for non-standard type only, for example leopard_python
775 if (param.camModel == LEOPARD_PYTHON) {
776 /* This camera sends bayer 10bit over 2bytes for each piece of information,
777 * therefore the total size of the image is 2 times the number of pixels.
778 */
779 param.raw_image_size = param.src_fmt.fmt.pix.width * param.src_fmt.fmt.pix.height * 2;
780 param.raw_image = new unsigned char[param.raw_image_size];
781 param.read_image = param.raw_image; // store the image read in the raw_image buffer
782 } else // This buffer should not be used for STANDARD_UVC cameras
783 {
784 param.read_image = param.src_image; // store the image read in the src_image buffer
785 param.raw_image_size = 0;
786 param.raw_image = YARP_NULLPTR;
787 }
788
789 switch (param.io) {
790 case IO_METHOD_READ:
791 readInit(param.src_fmt.fmt.pix.sizeimage);
792 break;
793
794 case IO_METHOD_MMAP:
795 mmapInit();
796 break;
797
799 userptrInit(param.src_fmt.fmt.pix.sizeimage);
800 break;
801 }
802
803 if (verbose) {
805 }
806 configured = true;
807
808 return true;
809}
810
811bool V4L_camera::deviceUninit()
812{
813 unsigned int i;
814 bool ret = true;
815 configured = false;
816
817 switch (param.io) {
818 case IO_METHOD_READ:
819 free(param.buffers[0].start);
820 break;
821
822 case IO_METHOD_MMAP:
823 for (i = 0; i < param.n_buffers; ++i) {
824 if (-1 == v4l2_munmap(param.buffers[i].start, param.buffers[i].length)) {
825 ret = false;
826 }
827 }
828
829 CLEAR(param.req);
830 // memset(param.req, 0, sizeof(struct v4l2_requestbuffers));
831 param.req.count = 0;
832 param.req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
833 param.req.memory = V4L2_MEMORY_MMAP;
834 if (xioctl(param.fd, VIDIOC_REQBUFS, &param.req) < 0) {
835 yCError(USBCAMERA, "VIDIOC_REQBUFS - Failed to delete buffers: %s (errno %d)", strerror(errno), errno);
836 return false;
837 }
838
839 break;
840
842 for (i = 0; i < param.n_buffers; ++i) {
843 free(param.buffers[i].start);
844 }
845 break;
846 }
847
848 if (param.buffers != nullptr) {
849 free(param.buffers);
850 }
851
852 if (param.raw_image != YARP_NULLPTR) {
853 delete[] param.raw_image;
854 param.raw_image = YARP_NULLPTR;
855 }
856
857 if (param.src_image != YARP_NULLPTR) {
858 delete[] param.src_image;
859 param.src_image = YARP_NULLPTR;
860 }
861
862 if (param.dst_image_rgb != YARP_NULLPTR) {
863 delete[] param.dst_image_rgb;
865 }
866
867 if (_v4lconvert_data != YARP_NULLPTR) {
868 v4lconvert_destroy(_v4lconvert_data);
869 _v4lconvert_data = YARP_NULLPTR;
870 }
871
872 return ret;
873}
874
879{
881
882 stop(); // stop yarp thread acquiring images
883
884 if (param.fd != -1) {
885 captureStop();
886 deviceUninit();
887
888 if (-1 == v4l2_close(param.fd)) {
889 yCError(USBCAMERA) << "Error closing V4l2 device";
890 }
891 return false;
892 }
893 param.fd = -1;
894 return true;
895}
896
898{
899 image.resize(width(), height());
900
901 bool res = false;
902 mutex.wait();
903 if (configured) {
904 imagePreProcess();
905 imageProcess();
906
907 if (!param.addictionalResize) {
908 memcpy(image.getRawImage(), param.dst_image_rgb, param.dst_image_size_rgb);
909 } else {
910 memcpy(image.getRawImage(), param.outMat.data, param.outMat.total() * 3);
911 }
912 mutex.post();
913 res = true;
914 } else {
915 yCError(USBCAMERA) << "unable to get the buffer, device uninitialized";
916 mutex.post();
917 res = false;
918 }
919 if (res) return ReturnValue_ok;
920 return ReturnValue::return_code::return_value_error_method_failed;
921}
922
924{
925 image.resize(width(), height());
926
927 bool res = false;
928 mutex.wait();
929 if (configured) {
930 imagePreProcess();
931 memcpy(image.getRawImage(), param.src_image, param.src_image_size);
932 res = true;
933 } else {
934 yCError(USBCAMERA) << "unable to get the buffer, device uninitialized";
935 res = false;
936 }
937 mutex.post();
938 if (res) return ReturnValue_ok;
939 return ReturnValue::return_code::return_value_error_method_failed;
940}
941
947{
948 /*
949 * return user setting because at the end of the day, this is what
950 * the image must look like
951 */
952 return param.user_height;
953}
954
960{
961 /*
962 * return user setting because at the end of the day, this is what
963 * the image must look like
964 */
965 return param.user_width;
966}
967
976int V4L_camera::xioctl(int fd, int request, void* argp)
977{
978 int r;
979
980 do {
981 r = v4l2_ioctl(fd, request, argp);
982 } while (-1 == r && EINTR == errno);
983
984 return r;
985}
986
987
989
990
993
994void V4L_camera::enumerate_menu()
995{
996 yCInfo(USBCAMERA, "Menu items:");
997
998 memset(&querymenu, 0, sizeof(querymenu));
999 querymenu.id = queryctrl.id;
1000
1001 for (querymenu.index = (__u32)queryctrl.minimum; querymenu.index <= (__u32)queryctrl.maximum; querymenu.index++) {
1002 if (0 == ioctl(param.fd, VIDIOC_QUERYMENU, &querymenu)) {
1003 yCInfo(USBCAMERA, " %s", querymenu.name);
1004 } else {
1005 yCError(USBCAMERA, "VIDIOC_QUERYMENU: %d, %s", errno, strerror(errno));
1006 return;
1007 }
1008 }
1009}
1010
1011
1012bool V4L_camera::enumerate_controls()
1013{
1014 memset(&queryctrl, 0, sizeof(queryctrl));
1015
1017 if (0 == ioctl(param.fd, VIDIOC_QUERYCTRL, &queryctrl)) {
1018 if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
1019 continue;
1020 }
1021
1022 yCInfo(USBCAMERA, "Control %s (id %d)", queryctrl.name, queryctrl.id);
1023
1024 if (queryctrl.type == V4L2_CTRL_TYPE_MENU) {
1025 enumerate_menu();
1026 }
1027 } else {
1028 if (errno == EINVAL) {
1029 continue;
1030 }
1031
1032 yCError(USBCAMERA, "VIDIOC_QUERYCTRL: %d, %s", errno, strerror(errno));
1033 return false;
1034 }
1035 }
1036
1037 for (queryctrl.id = V4L2_CID_PRIVATE_BASE;; queryctrl.id++) {
1038 if (0 == ioctl(param.fd, VIDIOC_QUERYCTRL, &queryctrl)) {
1039 if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
1040 continue;
1041 }
1042
1043 yCInfo(USBCAMERA, "Control %s", queryctrl.name);
1044
1045 if (queryctrl.type == V4L2_CTRL_TYPE_MENU) {
1046 enumerate_menu();
1047 }
1048 } else {
1049 if (errno == EINVAL) {
1050 break;
1051 }
1052
1053 yCError(USBCAMERA, "VIDIOC_QUERYCTRL: %d, %s", errno, strerror(errno));
1054 return false;
1055 }
1056 }
1057 return true;
1058}
1059
1063bool V4L_camera::full_FrameRead()
1064{
1065 bool got_it = false;
1066 void* image_ret = nullptr;
1067 unsigned int count;
1068 unsigned int numberOfTimeouts;
1069
1070 fd_set fds;
1071 struct timeval tv;
1072 int r;
1073
1074 numberOfTimeouts = 0;
1075 count = 10; //trials
1076
1077
1078 for (unsigned int i = 0; i < count; i++) {
1079 FD_ZERO(&fds);
1080 FD_SET(param.fd, &fds);
1081
1082 /* Timeout. */
1083 tv.tv_sec = 1;
1084 tv.tv_usec = 0;
1085
1086 r = select(param.fd + 1, &fds, nullptr, nullptr, &tv);
1087
1088 if (r < 0) {
1089 if (EINTR == errno) {
1090 continue;
1091 }
1092
1093 return image_ret != nullptr;
1094 }
1095 if (0 == r) {
1097 {
1098 yCWarning(USBCAMERA, "timeout while reading image [%d/%d]", numberOfTimeouts, count);
1099 got_it = false;
1100 }
1101 } else if ((r > 0) && (FD_ISSET(param.fd, &fds))) {
1102 if (frameRead()) {
1103 //yCTrace(USBCAMERA, "got an image");
1104 got_it = true;
1105 break;
1106 }
1107 yCWarning(USBCAMERA, "trial %d failed", i);
1108
1109 } else {
1110 yCWarning(USBCAMERA, "select woke up for something else");
1111 }
1112
1113 /* EAGAIN - continue select loop. */
1114 }
1115 return got_it;
1116}
1117
1121bool V4L_camera::frameRead()
1122{
1123 unsigned int i;
1124 struct v4l2_buffer buf;
1125 mutex.wait();
1126
1127 switch (param.io) {
1128 case IO_METHOD_READ:
1129 if (-1 == v4l2_read(param.fd, param.buffers[0].start, param.buffers[0].length)) {
1130 mutex.post();
1131 return false;
1132 }
1133
1134 timeStamp.update(toEpochOffset + buf.timestamp.tv_sec + buf.timestamp.tv_usec / 1000000.0);
1135 // imageProcess(param.buffers[0].start);
1136 break;
1137
1138
1139 case IO_METHOD_MMAP:
1140 CLEAR(buf);
1141
1142 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1143 buf.memory = V4L2_MEMORY_MMAP;
1144
1145 if (-1 == xioctl(param.fd, VIDIOC_DQBUF, &buf)) {
1146 yCError(USBCAMERA, "usbCamera VIDIOC_DQBUF");
1147 mutex.post();
1148 return false;
1149 }
1150
1151 if (!(buf.index < param.n_buffers)) {
1152 mutex.post();
1153 return false;
1154 }
1155
1156 memcpy(param.read_image, param.buffers[buf.index].start, param.buffers[0].length);
1157 // imageProcess(param.raw_image);
1158 timeStamp.update(toEpochOffset + buf.timestamp.tv_sec + buf.timestamp.tv_usec / 1000000.0);
1159
1160 if (-1 == xioctl(param.fd, VIDIOC_QBUF, &buf)) {
1161 yCError(USBCAMERA, "VIDIOC_QBUF");
1162 mutex.post();
1163 return false;
1164 }
1165
1166 break;
1167
1168 case IO_METHOD_USERPTR:
1169 CLEAR(buf);
1170
1171 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1172 buf.memory = V4L2_MEMORY_USERPTR;
1173
1174 if (-1 == xioctl(param.fd, VIDIOC_DQBUF, &buf)) {
1175 yCError(USBCAMERA, "VIDIOC_DQBUF");
1176 mutex.post();
1177 return false;
1178 }
1179
1180 for (i = 0; i < param.n_buffers; ++i) {
1181 if (buf.m.userptr == (unsigned long)param.buffers[i].start && buf.length == param.buffers[i].length) {
1182 break;
1183 }
1184 }
1185
1186 if (!(i < param.n_buffers)) {
1187 mutex.post();
1188 return false;
1189 }
1190
1191 memcpy(param.read_image, param.buffers[buf.index].start, param.buffers[0].length);
1192 timeStamp.update(toEpochOffset + buf.timestamp.tv_sec + buf.timestamp.tv_usec / 1000000.0);
1193
1194
1195 if (-1 == xioctl(param.fd, VIDIOC_QBUF, &buf)) {
1196 yCError(USBCAMERA, "VIDIOC_QBUF");
1197 }
1198 break;
1199
1200 default:
1201 yCError(USBCAMERA, "frameRead no read method configured");
1202 }
1203 mutex.post();
1204 return true;
1205}
1206
1207/*
1208 * This function is intended to perform custom code to adapt
1209 * non standard pixel types to a standard one, in order to
1210 * use standard conversion libraries afterward.
1211 */
1212void V4L_camera::imagePreProcess()
1213{
1214 switch (param.camModel) {
1215 case LEOPARD_PYTHON:
1216 {
1217 // Here we are resizing the byte information from 10 to 8 bits.
1218 // Width and Height are not modified by this operation.
1219 const uint _pixelNum = param.src_fmt.fmt.pix.width * param.src_fmt.fmt.pix.height;
1220
1221 uint16_t* raw_p = (uint16_t*)param.raw_image;
1222 for (uint i = 0; i < _pixelNum; i++) {
1223 param.src_image[i] = (unsigned char)(raw_p[i] >> bit_shift);
1224 }
1225
1226 // Set the correct pixel type fot the v4l_convert to work on.
1227 param.src_fmt.fmt.pix.bytesperline = param.src_fmt.fmt.pix.width;
1228 param.src_fmt.fmt.pix.pixelformat = pixel_fmt_leo;
1229 break;
1230 }
1231 case STANDARD_UVC:
1232 default:
1233 // Nothing to do here
1234 break;
1235 }
1236}
1237
1241void V4L_camera::imageProcess()
1242{
1243 static bool initted = false;
1244 static int err = 0;
1245
1246 timeStart = yarp::os::Time::now();
1247
1248 // imagePreProcess() should already be called before entering here!!
1249 // src_fmt and dst_fmt must be alredy fixed up if needed!!
1250
1251 // Convert from src type to RGB
1252 if (v4lconvert_convert((v4lconvert_data*)_v4lconvert_data,
1253 &param.src_fmt,
1254 &param.dst_fmt,
1255 param.src_image,
1256 param.src_image_size,
1257 param.dst_image_rgb,
1258 param.dst_image_size_rgb)
1259 < 0) {
1260 if ((err % 20) == 0) {
1261 yCError(USBCAMERA, "error converting \n\t Error message is: %s", v4lconvert_get_error_message(_v4lconvert_data));
1262 err = 0;
1263 }
1264 err++;
1265 return;
1266 }
1267
1268 if (param.addictionalResize) {
1269 if (!param.dual) {
1270 cv::Mat img(cv::Size(param.dst_fmt.fmt.pix.width, param.dst_fmt.fmt.pix.height), CV_8UC3, param.dst_image_rgb);
1271 cv::Rect crop(param.resizeOffset_x, param.resizeOffset_y, param.resizeWidth, param.resizeHeight);
1272 cv::resize(img(crop), param.outMat, cvSize(param.user_width, param.user_height), 0, 0, cv::INTER_CUBIC);
1273 } else {
1274 // Load whole image in a cv::Mat
1275 cv::Mat img(cv::Size(param.dst_fmt.fmt.pix.width, param.dst_fmt.fmt.pix.height), CV_8UC3, param.dst_image_rgb);
1276 cv::Mat img_right;
1277 cv::Rect crop(param.resizeOffset_x, param.resizeOffset_y, param.resizeWidth, param.resizeHeight);
1278
1279 cv::resize(img(crop), param.outMat, cvSize(param.user_width / 2, param.user_height), 0, 0, cv::INTER_CUBIC);
1280 cv::Rect crop2(param.resizeWidth + param.resizeOffset_x * 2, param.resizeOffset_y, param.resizeWidth, param.resizeHeight);
1281 cv::resize(img(crop2), img_right, cvSize(param.user_width / 2, param.user_height), 0, 0, cv::INTER_CUBIC);
1282 cv::hconcat(param.outMat, img_right, param.outMat);
1283 }
1284 if (param.flip) {
1285 cv::flip(param.outMat, param.outMat, 1);
1286 }
1287 } else {
1288 if (param.flip) {
1289 cv::Mat img(cv::Size(param.dst_fmt.fmt.pix.width, param.dst_fmt.fmt.pix.height), CV_8UC3, param.dst_image_rgb);
1290 param.outMat = img;
1291 cv::flip(param.outMat, param.outMat, 1);
1292 }
1293 }
1294
1295 timeElapsed = yarp::os::Time::now() - timeStart;
1296 myCounter++;
1297 timeTot += timeElapsed;
1298
1299 if ((myCounter % 60) == 0) {
1300 if (!initted) {
1301 timeTot = 0;
1302 myCounter = 0;
1303 initted = true;
1304 }
1305 }
1306}
1307
1311void V4L_camera::captureStop()
1312{
1313 int ret = 0;
1314 int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1315 switch (param.io) {
1316 case IO_METHOD_READ:
1317 //do nothing
1318 break;
1319
1320 case IO_METHOD_MMAP:
1321 default:
1322 ret = xioctl(param.fd, VIDIOC_STREAMOFF, &type);
1323 if (ret < 0) {
1324 if (errno != 9) { /* errno = 9 means the capture was allready stoped*/
1325 yCError(USBCAMERA, "VIDIOC_STREAMOFF - Unable to stop capture: %d, %s", errno, strerror(errno));
1326 }
1327 }
1328 break;
1329 }
1330}
1331
1335void V4L_camera::captureStart()
1336{
1337 unsigned int i;
1338 enum v4l2_buf_type type;
1339
1340 switch (param.io) {
1341 case IO_METHOD_READ:
1342 /* Nothing to do. */
1343 break;
1344
1345 case IO_METHOD_MMAP:
1346 for (i = 0; i < param.n_buffers; ++i) {
1347 struct v4l2_buffer buf;
1348 CLEAR(buf);
1349
1350 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1351 buf.memory = V4L2_MEMORY_MMAP;
1352 buf.index = i;
1353
1354 if (-1 == xioctl(param.fd, VIDIOC_QBUF, &buf)) {
1355 yCError(USBCAMERA, "VIDIOC_QBUF");
1356 }
1357 }
1358
1360
1361 if (-1 == xioctl(param.fd, VIDIOC_STREAMON, &type)) {
1362 yCError(USBCAMERA, "VIDIOC_STREAMON");
1363 }
1364
1365 break;
1366
1367 case IO_METHOD_USERPTR:
1368 for (i = 0; i < param.n_buffers; ++i) {
1369 struct v4l2_buffer buf;
1370
1371 CLEAR(buf);
1372
1373 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1374 buf.memory = V4L2_MEMORY_USERPTR;
1375 buf.index = i;
1376 buf.m.userptr = (unsigned long)param.buffers[i].start;
1377 buf.length = param.buffers[i].length;
1378
1379 if (-1 == xioctl(param.fd, VIDIOC_QBUF, &buf)) {
1380 yCError(USBCAMERA, "VIDIOC_QBUF");
1381 }
1382 }
1383
1385
1386 if (-1 == xioctl(param.fd, VIDIOC_STREAMON, &type)) {
1387 yCError(USBCAMERA, "VIDIOC_STREAMON");
1388 }
1389
1390 break;
1391 }
1392}
1393
1394
1395bool V4L_camera::readInit(unsigned int buffer_size)
1396{
1397 param.buffers = (struct buffer*)calloc(1, sizeof(*(param.buffers)));
1398
1399 if (param.buffers == nullptr) {
1400 yCError(USBCAMERA, "cannot allocate buffer, out of memory");
1401 return false;
1402 }
1403
1404 param.buffers[0].length = buffer_size;
1405 param.buffers[0].start = malloc(buffer_size);
1406
1407 if (param.buffers[0].start == nullptr) {
1408 yCError(USBCAMERA, "cannot allocate buffer, out of memory");
1409 return false;
1410 }
1411 return true;
1412}
1413
1414bool V4L_camera::mmapInit()
1415{
1416 CLEAR(param.req);
1417
1419 param.req.count = param.n_buffers;
1420 param.req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1421 param.req.memory = V4L2_MEMORY_MMAP;
1422
1423 if (-1 == xioctl(param.fd, VIDIOC_REQBUFS, &param.req)) {
1424 if (EINVAL == errno) {
1425 yCError(USBCAMERA, "%s does not support memory mapping", param.deviceId.c_str());
1426 return false;
1427 }
1428 yCError(USBCAMERA, "Error on device %s requesting memory mapping (VIDIOC_REQBUFS)", param.deviceId.c_str());
1429 return false;
1430 }
1431
1432 if (param.req.count < 1) {
1433 yCError(USBCAMERA, "Insufficient buffer memory on %s", param.deviceId.c_str());
1434 return false;
1435 }
1436
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());
1439 }
1440
1441 param.buffers = (struct buffer*)calloc(param.req.count, sizeof(*(param.buffers)));
1442
1443 if (param.buffers == nullptr) {
1444 yCError(USBCAMERA, "Out of memory");
1445 return false;
1446 }
1447
1448 struct v4l2_buffer buf;
1449
1450 for (param.n_buffers = 0; param.n_buffers < param.req.count; param.n_buffers++) {
1451 CLEAR(buf);
1452
1453 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1454 buf.memory = V4L2_MEMORY_MMAP;
1455 buf.index = param.n_buffers;
1456
1457 if (-1 == xioctl(param.fd, VIDIOC_QUERYBUF, &buf)) {
1458 yCError(USBCAMERA, "VIDIOC_QUERYBUF");
1459 }
1460
1461 param.buffers[param.n_buffers].length = buf.length;
1462 param.buffers[param.n_buffers].start = v4l2_mmap(nullptr, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, param.fd, buf.m.offset);
1463
1464 if (MAP_FAILED == param.buffers[param.n_buffers].start) {
1465 yCError(USBCAMERA, "mmap");
1466 }
1467 }
1468 return true;
1469}
1470
1471bool V4L_camera::userptrInit(unsigned int buffer_size)
1472{
1473 unsigned int page_size;
1474
1476 buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);
1477
1478 CLEAR(param.req);
1479
1480 param.req.count = VIDIOC_REQBUFS_COUNT;
1481 param.req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1482 param.req.memory = V4L2_MEMORY_USERPTR;
1483
1484 if (-1 == xioctl(param.fd, VIDIOC_REQBUFS, &param.req)) {
1485 if (EINVAL == errno) {
1486 yCError(USBCAMERA, "%s does not support user pointer i/o", param.deviceId.c_str());
1487 return false;
1488 }
1489 yCError(USBCAMERA, "Error requesting VIDIOC_REQBUFS for device %s", param.deviceId.c_str());
1490 return false;
1491 }
1492
1493 param.buffers = (struct buffer*)calloc(4, sizeof(*(param.buffers)));
1494
1495 if (param.buffers == nullptr) {
1496 yCError(USBCAMERA, "cannot allocate buffer, out of memory");
1497 return false;
1498 }
1499
1500 for (param.n_buffers = 0; param.n_buffers < 4; ++param.n_buffers) {
1501 param.buffers[param.n_buffers].length = buffer_size;
1502 param.buffers[param.n_buffers].start = memalign(/* boundary */ page_size, buffer_size);
1503
1504 if (param.buffers[param.n_buffers].start == nullptr) {
1505 yCError(USBCAMERA, "cannot allocate buffer, out of memory");
1506 return false;
1507 }
1508 }
1509 return true;
1510}
1511
1512bool V4L_camera::set_V4L2_control(uint32_t id, double value, bool verbatim)
1513{
1514 if (value < 0) {
1515 return false;
1516 }
1517
1519 struct v4l2_control control;
1520
1521 memset(&queryctrl, 0, sizeof(queryctrl));
1522 queryctrl.id = id;
1523
1524 if (-1 == ioctl(param.fd, VIDIOC_QUERYCTRL, &queryctrl)) {
1525 if (errno != EINVAL) {
1526 yCError(USBCAMERA, "VIDIOC_QUERYCTRL: %d, %s", errno, strerror(errno));
1527 } else {
1528 yCError(USBCAMERA, "Cannot set control <%s> (id 0x%0X) is not supported", queryctrl.name, queryctrl.id);
1529 }
1530 return false;
1531 }
1532
1533 if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
1534 yCError(USBCAMERA, "Control %s is disabled", queryctrl.name);
1535 return false;
1536 }
1537 memset(&control, 0, sizeof(control));
1538 control.id = id;
1539 if (verbatim) {
1540 control.value = value;
1541 } else {
1542 if (param.camModel == LEOPARD_PYTHON) {
1543 if ((V4L2_CID_EXPOSURE == id) || (V4L2_CID_EXPOSURE_ABSOLUTE == id) || (V4L2_CID_EXPOSURE_AUTO == id)) {
1544 queryctrl.maximum = 8000;
1545 queryctrl.minimum = 0;
1546 }
1547 }
1548 control.value = (int32_t)(value * (queryctrl.maximum - queryctrl.minimum) + queryctrl.minimum);
1549 }
1550 if (-1 == ioctl(param.fd, VIDIOC_S_CTRL, &control)) {
1551 yCError(USBCAMERA, "VIDIOC_S_CTRL: %d, %s", errno, strerror(errno));
1552 if (errno == ERANGE) {
1553 yCError(USBCAMERA, "Normalized input value %f ( equivalent to raw value of %d) was out of range for control %s: Min and Max are: %d - %d", value, control.value, queryctrl.name, queryctrl.minimum, queryctrl.maximum);
1554 }
1555 return false;
1556 }
1557 if (verbose) {
1558 yCInfo(USBCAMERA, "set control %s to %d done!", queryctrl.name, control.value);
1559 }
1560
1561 return true;
1562}
1563
1564bool V4L_camera::check_V4L2_control(uint32_t id)
1565{
1566 // yCTrace(USBCAMERA);
1568 struct v4l2_control control;
1569
1570 memset(&control, 0, sizeof(control));
1571 memset(&queryctrl, 0, sizeof(queryctrl));
1572
1573 control.id = id;
1574 queryctrl.id = id;
1575
1576 if (-1 == ioctl(param.fd, VIDIOC_QUERYCTRL, &queryctrl)) {
1577 if (errno != EINVAL) {
1578 yCError(USBCAMERA, "VIDIOC_QUERYCTRL: %d, %s", errno, strerror(errno));
1579 }
1580 return false;
1581 }
1582 return true;
1583}
1584
1585double V4L_camera::get_V4L2_control(uint32_t id, bool verbatim)
1586{
1588 struct v4l2_control control;
1589
1590 memset(&control, 0, sizeof(control));
1591 memset(&queryctrl, 0, sizeof(queryctrl));
1592
1593 control.id = id;
1594 queryctrl.id = id;
1595
1596 if (-1 == ioctl(param.fd, VIDIOC_QUERYCTRL, &queryctrl)) {
1597 if (errno != EINVAL) {
1598 yCError(USBCAMERA, "VIDIOC_QUERYCTRL: %d, %s", errno, strerror(errno));
1599 }
1600
1601 return -1.0;
1602 }
1603
1604 if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
1605 yCError(USBCAMERA, "Control %s is disabled", queryctrl.name);
1606 } else {
1607 if (-1 == ioctl(param.fd, VIDIOC_G_CTRL, &control)) {
1608 yCError(USBCAMERA, "VIDIOC_G_CTRL: %d, %s", errno, strerror(errno));
1609 return -1.0;
1610 }
1611 }
1612 if (verbatim) {
1613 return control.value;
1614 }
1615
1616 if (param.camModel == LEOPARD_PYTHON) {
1617 if ((V4L2_CID_EXPOSURE == id) || (V4L2_CID_EXPOSURE_ABSOLUTE == id) || (V4L2_CID_EXPOSURE_AUTO == id)) {
1618 queryctrl.maximum = 8000;
1619 queryctrl.minimum = 0;
1620 }
1621 }
1622 return (double)(control.value - queryctrl.minimum) / (queryctrl.maximum - queryctrl.minimum);
1623}
1624
1631
1633{
1634 bool tmpMan(false);
1635 bool tmpAuto(false);
1636 bool tmpOnce(false);
1637
1638 switch (feature) {
1639 case cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE:
1640 tmpMan = check_V4L2_control(V4L2_CID_RED_BALANCE) && check_V4L2_control(V4L2_CID_BLUE_BALANCE);
1641 tmpOnce = check_V4L2_control(V4L2_CID_DO_WHITE_BALANCE);
1642 tmpAuto = check_V4L2_control(V4L2_CID_AUTO_WHITE_BALANCE);
1643 break;
1644
1645 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
1646 tmpMan = check_V4L2_control(V4L2_CID_EXPOSURE) || check_V4L2_control(V4L2_CID_EXPOSURE_ABSOLUTE);
1647 tmpAuto = check_V4L2_control(V4L2_CID_EXPOSURE_AUTO);
1648 break;
1649
1650 default:
1651 tmpMan = check_V4L2_control(convertYARP_to_V4L(feature));
1652 break;
1653 }
1654
1656 return ReturnValue_ok;
1657}
1658
1660{
1661 bool ret = false;
1662 switch (feature) {
1663 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
1664 if (use_exposure_absolute) {
1665 ret = set_V4L2_control(V4L2_CID_EXPOSURE_ABSOLUTE, value);
1666 } else {
1667 ret = set_V4L2_control(V4L2_CID_EXPOSURE, value);
1668 }
1669 break;
1670
1671 default:
1672 ret = set_V4L2_control(convertYARP_to_V4L(feature), value);
1673 break;
1674 }
1675 if (ret) return ReturnValue_ok;
1676 return ReturnValue::return_code::return_value_error_method_failed;
1677}
1678
1680{
1681 double tmp = 0.0;
1682 switch (feature) {
1683 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
1684 if (use_exposure_absolute) {
1685 tmp = get_V4L2_control(V4L2_CID_EXPOSURE_ABSOLUTE);
1686 } else {
1687 tmp = get_V4L2_control(V4L2_CID_EXPOSURE);
1688 }
1689 break;
1690
1691 default:
1692 tmp = get_V4L2_control(convertYARP_to_V4L(feature));
1693 break;
1694 }
1695
1696 if (tmp == -1) {
1697 return ReturnValue::return_code::return_value_error_method_failed;
1698 }
1699
1700 value = tmp;
1701 return ReturnValue_ok;
1702}
1703
1705{
1706 if (feature == cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE) {
1707 bool ret = true;
1708 ret &= set_V4L2_control(V4L2_CID_AUTO_WHITE_BALANCE, false);
1710 ret &= set_V4L2_control(V4L2_CID_RED_BALANCE, value1);
1711 ret &= set_V4L2_control(V4L2_CID_BLUE_BALANCE, value2);
1712 if (ret) return ReturnValue_ok;
1713 else return ReturnValue::return_code::return_value_error_method_failed;
1714 }
1715 return ReturnValue::return_code::return_value_error_method_failed;
1716}
1717
1719{
1720 if (feature == cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE) {
1721 value1 = get_V4L2_control(V4L2_CID_RED_BALANCE);
1722 value2 = get_V4L2_control(V4L2_CID_BLUE_BALANCE);
1723 bool b = !((value1 == -1) || (value2 == -1));
1724 if (b) return ReturnValue_ok;
1725 else return ReturnValue::return_code::return_value_error_method_failed;
1726 }
1727 return ReturnValue::return_code::return_value_error_method_failed;
1728}
1729
1731{
1732 bool _hasAuto;
1733 // I can't find any meaning of setting a feature to off on V4l ... what it is supposed to do????
1734 switch (feature) {
1735 // The following do have a way to set them auto/manual
1736 case cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE:
1737 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
1738 if (hasAuto(feature, _hasAuto)) {
1739 _hasOnOff = true;
1740 } else {
1741 _hasOnOff = false;
1742 }
1743 break;
1744
1745 // try it out
1746 default:
1748 if (_hasAuto) {
1749 _hasOnOff = true;
1750 } else {
1751 _hasOnOff = false;
1752 }
1753 break;
1754 }
1755 return ReturnValue_ok;
1756}
1757
1759{
1760 // I can't find any meaning of setting a feature to off on V4l ... what it is supposed to do????
1761 bool tmp;
1762 switch (feature) {
1763 case cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE:
1764 tmp = set_V4L2_control(V4L2_CID_AUTO_WHITE_BALANCE, onoff);
1765 if (tmp) {
1766 isActive_vector[(int)feature] = onoff;
1767 }
1768 break;
1769
1770 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
1771 if (onoff) {
1772 set_V4L2_control(V4L2_LOCK_EXPOSURE, false);
1773
1775 if (tmp) {
1776 tmp = set_V4L2_control(V4L2_CID_EXPOSURE_AUTO, V4L2_EXPOSURE_AUTO);
1777 } else {
1778 tmp = set_V4L2_control(V4L2_CID_EXPOSURE_AUTO, V4L2_EXPOSURE_MANUAL);
1779 }
1780
1781 if (tmp) {
1782 isActive_vector[(int)feature] = onoff;
1783 }
1784 } else {
1785 bool man = set_V4L2_control(V4L2_CID_EXPOSURE_AUTO, V4L2_EXPOSURE_MANUAL);
1786 if (!man) {
1787 man = set_V4L2_control(V4L2_CID_EXPOSURE_AUTO, V4L2_EXPOSURE_SHUTTER_PRIORITY, true);
1788 if (!man) {
1789 yCError(USBCAMERA) << "Cannot set manual exposure";
1790 }
1791 }
1792 set_V4L2_control(V4L2_LOCK_EXPOSURE, true);
1793 isActive_vector[(int)feature] = onoff;
1794 }
1795 break;
1796
1797 default: // what to do in each case?
1798 if (onoff) {
1799 isActive_vector[(int)feature] = true;
1800 return ReturnValue_ok;
1801 }
1802 isActive_vector[(int)feature] = false;
1803 return ReturnValue::return_code::return_value_error_method_failed;
1804 }
1805 return ReturnValue_ok;
1806}
1807
1809{
1810 switch (feature) {
1811 case cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE:
1812 {
1813 double tmp = get_V4L2_control(V4L2_CID_AUTO_WHITE_BALANCE);
1814 if (tmp == 1) {
1815 _isActive = true;
1816 } else {
1817 _isActive = false;
1818 }
1819 break;
1820 }
1821
1822 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
1823 {
1824 bool _hasMan = check_V4L2_control(V4L2_CID_EXPOSURE); // check manual version (normal and asbolute)
1825 bool _hasMan2 = check_V4L2_control(V4L2_CID_EXPOSURE_ABSOLUTE);
1826 double _hasAuto = get_V4L2_control(V4L2_CID_EXPOSURE_AUTO, true); // check auto version
1827
1829 break;
1830 }
1831
1832 default:
1833 _isActive = true;
1834 break;
1835 }
1836
1837 return ReturnValue_ok;
1838}
1839
1841{
1842 switch (feature) {
1843 case cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE:
1844 _hasAuto = check_V4L2_control(V4L2_CID_AUTO_WHITE_BALANCE);
1845 break;
1846
1847 case cameraFeature_id_t::YARP_FEATURE_BRIGHTNESS:
1848 _hasAuto = check_V4L2_control(V4L2_CID_AUTOBRIGHTNESS);
1849 break;
1850
1851 case cameraFeature_id_t::YARP_FEATURE_GAIN:
1852 _hasAuto = check_V4L2_control(V4L2_CID_AUTOGAIN);
1853 break;
1854
1855 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
1856 _hasAuto = check_V4L2_control(V4L2_CID_EXPOSURE_AUTO);
1857 break;
1858
1859 case cameraFeature_id_t::YARP_FEATURE_HUE:
1860 _hasAuto = check_V4L2_control(V4L2_CID_HUE_AUTO);
1861 break;
1862
1863 default:
1864 _hasAuto = false;
1865 break;
1866 }
1867 return ReturnValue_ok;
1868}
1869
1871{
1872 if (feature == cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE) {
1873 _hasManual = check_V4L2_control(V4L2_CID_RED_BALANCE) && check_V4L2_control(V4L2_CID_BLUE_BALANCE);
1874 return ReturnValue_ok;
1875 }
1876
1877 if (feature == cameraFeature_id_t::YARP_FEATURE_EXPOSURE) {
1878 _hasManual = check_V4L2_control(V4L2_CID_EXPOSURE) || check_V4L2_control(V4L2_CID_EXPOSURE_ABSOLUTE);
1879 return ReturnValue_ok;
1880 }
1881 return hasFeature(feature, _hasManual);
1882}
1883
1885{
1886 // I'm not able to map a 'onePush' request on V4L api
1887 switch (feature) {
1888 case cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE:
1889 _hasOnePush = check_V4L2_control((int)V4L2_CID_DO_WHITE_BALANCE);
1890 return ReturnValue_ok;
1891
1892 default:
1893 _hasOnePush = false;
1894 break;
1895 }
1896 return ReturnValue_ok;
1897}
1898
1900{
1901 bool ret = false;
1902 switch (feature) {
1903 case cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE:
1904 if (mode == FeatureMode::MODE_AUTO) {
1905 ret = set_V4L2_control(V4L2_CID_AUTO_WHITE_BALANCE, true);
1906 } else {
1907 ret = set_V4L2_control(V4L2_CID_AUTO_WHITE_BALANCE, false);
1908 }
1909 break;
1910
1911 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
1912 bool _tmpAuto;
1913 hasAuto(cameraFeature_id_t::YARP_FEATURE_EXPOSURE, _tmpAuto);
1914
1915 if (_tmpAuto) {
1916 if (mode == FeatureMode::MODE_AUTO) {
1917 ret = set_V4L2_control(V4L2_CID_EXPOSURE_AUTO, true);
1918 } else {
1919 ret = set_V4L2_control(V4L2_CID_EXPOSURE_AUTO, false);
1920 }
1921 } else {
1922 ret = mode != FeatureMode::MODE_AUTO;
1923 }
1924 break;
1925
1926 case cameraFeature_id_t::YARP_FEATURE_GAIN:
1927 if (mode == FeatureMode::MODE_AUTO) {
1928 yCInfo(USBCAMERA) << "GAIN: set mode auto";
1929 ret = set_V4L2_control(V4L2_CID_AUTOGAIN, true);
1930 } else {
1931 yCInfo(USBCAMERA) << "GAIN: set mode manual";
1932 ret = set_V4L2_control(V4L2_CID_AUTOGAIN, false);
1933 }
1934 break;
1935
1936 case cameraFeature_id_t::YARP_FEATURE_BRIGHTNESS:
1937 {
1938 bool _tmpAuto;
1939 hasAuto(cameraFeature_id_t::YARP_FEATURE_BRIGHTNESS, _tmpAuto);
1940
1941 if (_tmpAuto) {
1942 if (mode == FeatureMode::MODE_AUTO) {
1943 ret = set_V4L2_control(V4L2_CID_AUTOBRIGHTNESS, true);
1944 } else {
1945 ret = set_V4L2_control(V4L2_CID_AUTOBRIGHTNESS, false);
1946 }
1947 } else {
1948 ret = mode != FeatureMode::MODE_AUTO;
1949 }
1950 break;
1951 }
1952
1953 case cameraFeature_id_t::YARP_FEATURE_HUE:
1954 if (mode == FeatureMode::MODE_AUTO) {
1955 ret = set_V4L2_control(V4L2_CID_HUE_AUTO, true);
1956 } else {
1957 ret = set_V4L2_control(V4L2_CID_HUE_AUTO, false);
1958 }
1959 break;
1960
1961 default:
1962 yCError(USBCAMERA) << "Feature " << (int)feature << " does not support auto mode";
1963 break;
1964 }
1965
1966 if (ret) return ReturnValue_ok;
1967 return ReturnValue::return_code::return_value_error_method_failed;
1968}
1969
1971{
1972 bool _tmpAuto;
1973 switch (feature) {
1974 case cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE:
1975 {
1976 double ret = get_V4L2_control(V4L2_CID_AUTO_WHITE_BALANCE);
1977 mode = toFeatureMode(ret != 0.0);
1978 break;
1979 }
1980
1981 case cameraFeature_id_t::YARP_FEATURE_EXPOSURE:
1982 {
1983 double ret = get_V4L2_control(V4L2_CID_EXPOSURE_AUTO);
1984 if (ret == -1.0) {
1985 mode = FeatureMode::MODE_MANUAL;
1986 break;
1987 }
1988
1989 if (ret == V4L2_EXPOSURE_MANUAL) {
1990 mode = FeatureMode::MODE_MANUAL;
1991 } else {
1992 mode = FeatureMode::MODE_AUTO;
1993 }
1994 break;
1995 }
1996
1997 case cameraFeature_id_t::YARP_FEATURE_BRIGHTNESS:
1998 hasAuto(cameraFeature_id_t::YARP_FEATURE_BRIGHTNESS, _tmpAuto);
2000 if (!_tmpAuto) {
2001 mode = FeatureMode::MODE_MANUAL;
2002 } else {
2003 double ret = get_V4L2_control(V4L2_CID_AUTOBRIGHTNESS);
2004 mode = toFeatureMode(ret != 0.0);
2005 }
2006 break;
2007
2008 case cameraFeature_id_t::YARP_FEATURE_GAIN:
2009 hasAuto(cameraFeature_id_t::YARP_FEATURE_GAIN, _tmpAuto);
2011 if (!_tmpAuto) {
2012 mode = FeatureMode::MODE_MANUAL;
2013 } else {
2014 double ret = get_V4L2_control(V4L2_CID_AUTOGAIN);
2015 mode = toFeatureMode(ret != 0.0);
2016 }
2017 break;
2018
2019 case cameraFeature_id_t::YARP_FEATURE_HUE:
2020 hasAuto(cameraFeature_id_t::YARP_FEATURE_HUE, _tmpAuto);
2022 if (!_tmpAuto) {
2023 mode = FeatureMode::MODE_MANUAL;
2024 } else {
2025 double ret = get_V4L2_control(V4L2_CID_HUE_AUTO);
2026 mode = toFeatureMode(ret != 0.0);
2027 }
2028 break;
2029
2030 default:
2031 mode = FeatureMode::MODE_MANUAL;
2032 break;
2033 }
2034 return ReturnValue_ok;
2035}
2036
2038{
2039 // I'm not able to map a 'onePush' request on each V4L api
2040 if (feature == cameraFeature_id_t::YARP_FEATURE_WHITE_BALANCE) {
2041 if (set_V4L2_control(V4L2_CID_DO_WHITE_BALANCE, true))
2042 {
2043 return ReturnValue_ok;
2044 }
2045 else
2046 {
2047 return ReturnValue::return_code::return_value_error_method_failed;
2048 }
2049
2050 }
2051 return ReturnValue::return_code::return_value_error_method_failed;
2052}
CameraDescriptor camera
FeatureMode mode
YarpVocabPixelTypesEnum
Definition Image.h:40
@ VOCAB_PIXEL_ENCODING_BAYER_BGGR16
Definition Image.h:59
@ VOCAB_PIXEL_YUV_420
Definition Image.h:64
@ VOCAB_PIXEL_MONO16
Definition Image.h:43
@ VOCAB_PIXEL_ENCODING_BAYER_BGGR8
Definition Image.h:58
@ VOCAB_PIXEL_YUV_444
Definition Image.h:65
@ VOCAB_PIXEL_BGR
Definition Image.h:49
@ VOCAB_PIXEL_ENCODING_BAYER_RGGB8
Definition Image.h:62
@ VOCAB_PIXEL_ENCODING_BAYER_GRBG8
Definition Image.h:56
@ VOCAB_PIXEL_YUV_422
Definition Image.h:66
@ VOCAB_PIXEL_ENCODING_BAYER_GBRG8
Definition Image.h:60
@ VOCAB_PIXEL_MONO
Definition Image.h:42
@ VOCAB_PIXEL_YUV_411
Definition Image.h:67
@ VOCAB_PIXEL_RGB
Definition Image.h:44
#define ReturnValue_ok
Definition ReturnValue.h:82
const yarp::os::LogComponent & USBCAMERA()
struct v4l2_queryctrl queryctrl
static double getEpochTimeShift()
struct v4l2_querymenu querymenu
#define NOT_PRESENT
#define DEFAULT_HEIGHT
Definition V4L_camera.h:44
#define DEFAULT_FRAMERATE
Definition V4L_camera.h:45
#define VIDIOC_REQBUFS_COUNT
Definition V4L_camera.h:46
#define DEFAULT_WIDTH
Definition V4L_camera.h:43
@ STANDARD_UVC
Definition V4L_camera.h:57
@ LEOPARD_PYTHON
Definition V4L_camera.h:58
#define CLEAR(x)
Definition V4L_camera.h:40
@ IO_METHOD_MMAP
Definition V4L_camera.h:51
@ IO_METHOD_READ
Definition V4L_camera.h:50
@ IO_METHOD_USERPTR
Definition V4L_camera.h:52
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.
A simple collection of objects that can be described and transmitted in a portable way.
Definition Bottle.h:64
bool check(const std::string &key) const override
Check if there exists a property of the given name.
Definition Bottle.cpp:278
bool isNull() const override
Checks if the object is invalid.
Definition Bottle.cpp:344
Value & find(const std::string &key) const override
Gets a value corresponding to a given keyword.
Definition Bottle.cpp:288
A mini-server for performing network communication in the background.
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.
Definition Searchable.h:31
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.
Definition Semaphore.cpp:96
void post()
Increment the counter.
An abstraction for a time stamp and/or sequence number.
Definition Stamp.h:18
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Stamp.cpp:49
A single value (typically within a Bottle).
Definition Value.h:44
static Value * makeList()
Create a list Value.
Definition Value.cpp:431
bool isNull() const override
Checks if the object is invalid.
Definition Value.cpp:371
static int32_t fromString(const std::string &input)
Typed image class.
Definition Image.h:603
yarp::sig::DistortionModelData distortionModel
#define YARP_NULLPTR
Expands to either the standard nullptr or to 0 elsewhere.
Definition compiler.h:2937
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCWarning(component,...)
#define yCDebug(component,...)
void list_cap_v4l2(int fd)
Definition list.cpp:278
void query_current_image_fmt_v4l2(int fd)
Definition list.cpp:44
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.
Definition Time.cpp:121
void delay(double seconds)
Wait for a certain number of seconds.
Definition Time.cpp:111
An interface to the operating system, including Port based communication.
int stat(const char *path)
Portable wrapper for the stat() function.
Definition Os.cpp:78
std::string deviceId
Definition V4L_camera.h:72
struct v4l2_format dst_fmt
Definition V4L_camera.h:118
yarp::sig::IntrinsicParams intrinsic
Definition V4L_camera.h:83
unsigned char * read_image
Definition V4L_camera.h:97
unsigned int n_buffers
Definition V4L_camera.h:115
size_t pixelType
Definition V4L_camera.h:120
struct buffer * buffers
Definition V4L_camera.h:116
__u32 user_height
Definition V4L_camera.h:79
struct v4l2_format src_fmt
Definition V4L_camera.h:117
unsigned char * raw_image
Definition V4L_camera.h:92
io_method io
Definition V4L_camera.h:86
unsigned char * dst_image_rgb
Definition V4L_camera.h:106
std::vector< yarp::dev::CameraConfig > configurations
Definition V4L_camera.h:112
int resizeOffset_x
Definition V4L_camera.h:75
int resizeOffset_y
Definition V4L_camera.h:75
struct v4l2_requestbuffers req
Definition V4L_camera.h:119
__u32 user_width
Definition V4L_camera.h:78
double horizontalFov
Definition V4L_camera.h:81
unsigned char * src_image
Definition V4L_camera.h:101
bool addictionalResize
Definition V4L_camera.h:74
cv::Mat outMat
Definition V4L_camera.h:110
supported_cams camModel
Definition V4L_camera.h:121
double verticalFov
Definition V4L_camera.h:82
unsigned int dst_image_size_rgb
Definition V4L_camera.h:107
unsigned int src_image_size
Definition V4L_camera.h:102
unsigned int raw_image_size
Definition V4L_camera.h:93
The IntrinsicParams struct to handle the intrinsic parameter of cameras(RGB and RGBD either).