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