YARP
Yet Another Robot Platform
V4L_camera.h
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 #ifndef YARP_DEVICE_USBCAMERA_LINUX_V4L_CAMERA_H
8 #define YARP_DEVICE_USBCAMERA_LINUX_V4L_CAMERA_H
9 
10 #include <yarp/os/PeriodicThread.h>
11 #include <yarp/os/Semaphore.h>
12 
13 #include <yarp/dev/DeviceDriver.h>
18 
19 #include <asm/types.h>
20 #include <opencv2/opencv.hpp>
21 #include <cerrno>
22 #include <fcntl.h>
23 #include <getopt.h>
24 #include <iostream>
25 #include <jpeglib.h>
26 #include <libv4l2.h>
27 #include <libv4lconvert.h>
28 #include <linux/videodev2.h>
29 #include <malloc.h>
30 #include <map>
31 #include <cstdlib>
32 #include <cstring>
33 #include <sys/ioctl.h>
34 #include <sys/mman.h>
35 #include <sys/stat.h>
36 #include <sys/time.h>
37 #include <sys/types.h>
38 #include <unistd.h>
39 
40 #define CLEAR(x) memset(&(x), 0, sizeof(x))
41 
42 // minimum number of buffers to request in VIDIOC_REQBUFS call
43 #define DEFAULT_WIDTH 640
44 #define DEFAULT_HEIGHT 480
45 #define DEFAULT_FRAMERATE 30
46 #define VIDIOC_REQBUFS_COUNT 2
47 
48 typedef enum
49 {
53 } io_method;
54 
55 typedef enum
56 {
60 
61 
62 struct buffer
63 {
64  void* start;
65  size_t length;
66 };
67 
68 
69 typedef struct
70 {
71  int fd;
72  std::string deviceId;
73 
75  int resizeOffset_x, resizeOffset_y;
76  int resizeWidth, resizeHeight;
77 
78  __u32 user_width;
79  __u32 user_height;
80 
81  double horizontalFov;
82  double verticalFov;
84  bool dual;
85 
87  int fps;
88 
89  // Temporary step required for leopard python camera only
90  // The image has to be converted into standard bayer format
91  // in order to be correctly converted into rgb
92  unsigned char* raw_image;
93  unsigned int raw_image_size;
94 
95  // this is a helper pointer set either to raw_image or src_image,
96  // depending if the source is custom or standard
97  unsigned char* read_image;
98 
99  // src image: standard image type read from the camera sensor
100  // used as input for color conversion
101  unsigned char* src_image;
102  unsigned int src_image_size;
103 
104  // RGB image after color conversion. The size may not be the one
105  // requested by the user and a rescaling may be required afterwards
106  unsigned char* dst_image_rgb;
107  unsigned int dst_image_size_rgb;
108 
109  // OpenCV object to perform the final rescaling of the image
110  cv::Mat outMat; // OpenCV output
111 
113  bool flip;
114 
115  unsigned int n_buffers;
116  struct buffer* buffers;
117  struct v4l2_format src_fmt;
118  struct v4l2_format dst_fmt;
119  struct v4l2_requestbuffers req;
120  size_t pixelType;
121  supported_cams camModel; // In case some camera requires custom procedure
122 } Video_params;
123 
124 
125 /*
126  * Device handling
127  */
128 
129 class V4L_camera :
137 {
138 public:
139  V4L_camera();
140 
141  // DeviceDriver Interface
142  bool open(yarp::os::Searchable& config) override;
143  bool close() override;
144 
146 
147  /*Implementation of IFrameGrabberImage and IFrameGrabberImageRaw interfaces*/
150  int height() const override;
151  int width() const override;
152 
153  /*Implementation of IRgbVisualParams interface*/
154  int getRgbHeight() override;
155  int getRgbWidth() override;
157  bool getRgbResolution(int& width, int& height) override;
158  bool setRgbResolution(int width, int height) override;
159  bool getRgbFOV(double& horizontalFov, double& verticalFov) override;
160  bool setRgbFOV(double horizontalFov, double verticalFov) override;
161  bool getRgbIntrinsicParam(yarp::os::Property& intrinsic) override;
162  bool getRgbMirroring(bool& mirror) override;
163  bool setRgbMirroring(bool mirror) override;
164 
165 
166  /* Implementation of IFrameGrabberControls interface */
167  bool getCameraDescription(CameraDescriptor* camera) override;
168  bool hasFeature(int feature, bool* hasFeature) override;
169  bool setFeature(int feature, double value) override;
170  bool getFeature(int feature, double* value) override;
171  bool setFeature(int feature, double value1, double value2) override;
172  bool getFeature(int feature, double* value1, double* value2) override;
173  bool hasOnOff(int feature, bool* _hasOnOff) override;
174  bool setActive(int feature, bool onoff) override;
175  bool getActive(int feature, bool* _isActive) override;
176  bool hasAuto(int feature, bool* _hasAuto) override;
177  bool hasManual(int feature, bool* _hasManual) override;
178  bool hasOnePush(int feature, bool* _hasOnePush) override;
179  bool setMode(int feature, FeatureMode mode) override;
180  bool getMode(int feature, FeatureMode* mode) override;
181  bool setOnePush(int feature) override;
182 
183 private:
184  bool verbose;
185  v4lconvert_data* _v4lconvert_data;
186  bool use_exposure_absolute;
187 
188  yarp::os::Stamp timeStamp;
189  Video_params param;
190  yarp::os::Semaphore mutex;
191  bool configFx, configFy;
192  bool configPPx, configPPy;
193  bool configRet, configDistM;
194  bool configIntrins;
195  bool configured;
196  bool doCropping;
197  bool isActive_vector[YARP_FEATURE_NUMBER_OF];
198  double timeStart, timeTot, timeNow, timeElapsed;
199  int myCounter;
200  int frameCounter;
201 
202  std::map<std::string, supported_cams> camMap;
203 
204  bool fromConfig(yarp::os::Searchable& config);
205 
206  void populateConfigurations();
207 
208  int convertV4L_to_YARP_format(int format);
209 
210  double checkDouble(yarp::os::Searchable& config, const char* key);
211 
212  // initialize device
213  bool deviceInit();
214 
215  // de-initialize device
216  bool deviceUninit();
217 
218  void captureStart();
219  void captureStop();
220 
221  bool threadInit() override;
222  void run() override;
223  void threadRelease() override;
224 
225 
226  /*
227  * Inintialize different types of reading frame
228  */
229 
230  // some description
231  bool readInit(unsigned int buffer_size);
232 
233  // some description
234  bool mmapInit();
235 
236  // some description
237  bool userptrInit(unsigned int buffer_size);
238 
239 
240  // use the device for something
244  bool frameRead();
245 
246  bool full_FrameRead();
247 
248  /*
249  * This function is intended to perform custom code to adapt
250  * non standard pixel types to a standard one, in order to
251  * use standard conversion libraries afterward.
252  */
253  void imagePreProcess();
254 
255  /*
256  * This function is intended to perform all the required conversions
257  * from the camera pixel type to the RGB one and eventually rescaling
258  * to size requested by the user.
259  */
260  void imageProcess();
261 
262  int getfd();
263 
264 private:
265  // low level stuff - all functions here uses the Linux V4L specific definitions
274  int xioctl(int fd, int request, void* argp);
275 
276  int convertYARP_to_V4L(int feature);
277  void enumerate_menu();
278  bool enumerate_controls();
279  bool check_V4L2_control(uint32_t id);
280  bool set_V4L2_control(u_int32_t id, double value, bool verbatim = false);
281  double get_V4L2_control(uint32_t id, bool verbatim = false); // verbatim = do not convert value, for enum types
282 
283  double toEpochOffset;
284 
285  // leopard de-bayer test
286  int bit_shift;
287  int bit_bayer;
288  int pixel_fmt_leo;
289 };
290 
291 #endif // YARP_DEVICE_USBCAMERA_LINUX_V4L_CAMERA_H
@ YARP_FEATURE_NUMBER_OF
supported_cams
Definition: V4L_camera.h:56
@ STANDARD_UVC
Definition: V4L_camera.h:57
@ LEOPARD_PYTHON
Definition: V4L_camera.h:58
io_method
Definition: V4L_camera.h:49
@ 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.
Interface implemented by all device drivers.
Definition: DeviceDriver.h:35
Control interface for frame grabber devices.
An interface for retrieving intrinsic parameter from a rgb camera.
An abstraction for a periodic thread.
A class for storing options and configuration information.
Definition: Property.h:34
A base class for nested structures that can be searched.
Definition: Searchable.h:66
A class for thread synchronization and mutual exclusion.
Definition: Semaphore.h:26
An abstraction for a time stamp and/or sequence number.
Definition: Stamp.h:22
std::string deviceId
Definition: V4L_camera.h:72
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
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
__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