forked from fixstars/libSGM
-
Notifications
You must be signed in to change notification settings - Fork 0
/
withrobot_camera.hpp
346 lines (283 loc) · 11.8 KB
/
withrobot_camera.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
/*******************************************************************************#
# #
# Withrobot Camera API #
# #
# Copyright (C) 2016 Withrobot. Inc. #
# #
# This program is free software: you can redistribute it and/or modify #
# it under the terms of the GNU General Public License as published by #
# the Free Software Foundation, either version 3 of the License, or #
# (at your option) any later version. #
# #
# This program is distributed in the hope that it will be useful, #
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the #
# GNU General Public License for more details. #
# You should have received a copy of the GNU General Public License #
# along with this program. If not, see <http://www.gnu.org/licenses/> #
# #
********************************************************************************/
#ifndef WITHROBOT_CAMERA_HPP_
#define WITHROBOT_CAMERA_HPP_
#include <linux/videodev2.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <libv4l2.h>
#include <fcntl.h>
#include <string.h>
#include <stdlib.h>
#include <errno.h>
#include <assert.h>
#include <memory.h>
#include <vector>
#include <string>
#include <cstring>
#include <map>
#include <sstream>
#include <stdio.h>
#include "withrobot_utility.hpp"
#define WITHROBOT_CAMERA_DEFAULT_WIDTH 640
#define WITHROBOT_CAMERA_DEFAULT_HEIGHT 480
#define WITHROBOT_CAMERA_DEFAULT_FIXFORMAT V4L2_PIX_FMT_YUYV
/*
* DBG print
*/
#define PRINT_DEBUG_MSG /* When you want to show the debug messages remove the comment here. */
#ifdef PRINT_DEBUG_MSG
#define DBG_PRINTF(...) {\
fprintf(stdout, "DBG: [%s, %d, %s] ", __FILE__, __LINE__, __FUNCTION__); \
fprintf(stdout, __VA_ARGS__); fprintf(stdout, "\n"); fflush(stdout); \
}
#define DBG_PRINTF_MSG(...) { fprintf(stdout, __VA_ARGS__); fflush(stdout); }
#define DBG_PERROR(...) { fflush(stdout); printf("DBG_ERR: [%s, %d, %s] ", __FILE__, __LINE__, __func__); fflush(stdout); perror(__VA_ARGS__); fflush(stdout); }
#else
#define DBG_PRINTF(...)
#define DBG_PRINTF_MSG(...)
#define DBG_PERROR(...)
#endif /* PRINT_DEBUG_MSG */
/**
* @defgroup Withrobot Camera API
* @brief Withrobot inc. UVC type camera control API
*
* Withrobot UVC type camera control API
*
* @{
*/
namespace Withrobot {
/**
* FOURCC 문자를 pixel format 코드로 변환하는 함수
*
* e.g. fourcc_to_pixformat('Y', 'U', 'Y', 'V')
*
* @param a [입력] first character
* @param b [입력] second character
* @param c [입력] third character
* @param d [입력] forth character
* @return pixel_format 코드
*/
inline static unsigned int fourcc_to_pixformat(const char a, const char b, const char c, const char d) {
return (((unsigned int)a << 0)|((unsigned int)b << 8)|((unsigned int) c << 16)|((unsigned int)d << 24));
}
/**
* Camera control types
*/
enum camera_control_type {
CAM_CTRL_TYPE_INTEGER = 1,//!< 정수
CAM_CTRL_TYPE_BOOLEAN = 2,//!< 불리언
CAM_CTRL_TYPE_MENU = 3,//!< 하위 메뉴
CAM_CTRL_TYPE_BUTTON = 4,//!< 버튼
CAM_CTRL_TYPE_INTEGER64 = 5,//!< 64비트 정수
CAM_CTRL_TYPE_CTRL_CLASS = 6,//!< 컨트롤 클래스
CAM_CTRL_TYPE_STRING = 7,//!< 문자열
CAM_CTRL_TYPE_BITMASK = 8,//!< 비트 마스크
CAM_CTRL_TYPE_INTEGER_MENU = 9 //!< 정부 메뉴
};
/**
* Camera control flags
*/
enum camera_control_flags {
CAM_CTRL_FLAG_DISABLED = 1, //!< Disabled
CAM_CTRL_FLAG_GRABBED = 2, //!< Grabbed
CAM_CTRL_FLAG_READ_ONLY = 4, //!< Read only
CAM_CTRL_FLAG_UPDATE = 8, //!< Update
CAM_CTRL_FLAG_INACTIVE = 16,//!< Inactive
CAM_CTRL_FLAG_SLIDER = 32,//!< Slider
CAM_CTRL_FLAG_WRITE_ONLY = 64 //!< Wirte only
};
/**
* Camera image format structure
*/
struct camera_format {
unsigned int pixformat;
unsigned int width;
unsigned int height;
unsigned int image_size;
unsigned int rate_numerator;
unsigned int rate_denominator;
double frame_rate;
camera_format() { clear(); }
void clear() { memset(this, 0, sizeof(*this)); }
void print() {
printf("Pixel format: %c, %c, %c, %c\n", (pixformat >> 0) & 0xFF, (pixformat >> 8) & 0xFF, (pixformat >> 16) & 0xff, (pixformat >> 24) & 0xFF);
printf("Width: %d, Height: %d, Image size: %d\n", width, height, image_size);
printf("Frame Rate : %d / %d (%.2f fps)\n", rate_numerator, rate_denominator, frame_rate);
}
void dbg_print() {
DBG_PRINTF("Pixel format: %c, %c, %c, %c", (pixformat >> 0) & 0xFF, (pixformat >> 8) & 0xFF, (pixformat >> 16) & 0xff, (pixformat >> 24) & 0xFF);
DBG_PRINTF("Width: %d, Height: %d, Image size: %d", width, height, image_size);
DBG_PRINTF("Frame Rate : %d / %d (%.2f fps)", rate_numerator, rate_denominator, frame_rate);
}
};
/**
* Camera control menu structure
*/
struct camera_control_menu {
unsigned int index;
char name[256];
int value;
camera_control_menu() { clear(); }
void clear() { memset(this, 0, sizeof(*this)); }
void dbg_print() {
DBG_PRINTF("Index: %d, Name: %s, Value: %d", index, name, value);
}
};
/**
* Camera control structure
*/
struct camera_control {
unsigned int id;
//char name[256];
std::string name;
int value;
int default_value;
unsigned int type;
unsigned int flags;
int minimum;
int maximum;
int step;
std::vector<camera_control_menu> menu_list;
camera_control() { clear(); }
void clear() { memset(this, 0, sizeof(*this)); }
void dbg_print() {
DBG_PRINTF("Id: %u, Name: %s, Value(default [min, step, max]): %d ( %d [%d, %d, %d] ), Type: %d, Flag: %d",
id, name.c_str(), value, default_value, minimum, step, maximum, type, flags);
for (unsigned int i=0; i < menu_list.size(); i++) {
DBG_PRINTF("Menu %d", i);
menu_list[i].dbg_print();
}
}
};
/**
* Camera Class
*/
class Camera
{
public:
/*
* oCam-1MGN
*
* [ supported image formats ]
*
* USB 3.0
* [1] "8-bit Greyscale 1280 x 720 60 fps"
* [2] "8-bit Greyscale 1280 x 960 45 fps"
* [3] "8-bit Greyscale 320 x 240 160 fps"
* [4] "8-bit Greyscale 640 x 480 80 fps"
*
* USB 2.0
* [1] "8-bit Greyscale 1280 x 720 30 fps"
* [2] "8-bit Greyscale 1280 x 960 22.5 fps"
* [3] "8-bit Greyscale 320 x 240 160 fps"
* [4] "8-bit Greyscale 640 x 480 80 fps"
*
*
* [ supported camera controls; The double quotes are the 'get_control' and the 'set_control' function string argument values. ]
*
* [1] "Exposure (Absolute)", Value(default [min, step, max]): 39 ( 39 [1, 1, 625] )
* [2] "Brightness", Value(default [min, step, max]): 64 ( 64 [0, 1, 127] ) // gain
*
*/
Camera(const char* dev_name, struct camera_format* conf=0, const char* format_string=0, const unsigned char disable_libv4l2=0);
~Camera();
bool start();
bool stop();
bool start_out();
bool stop_out();
void get_configurations(std::vector<std::string>& formats, std::vector<std::string>& controls);
bool get_current_format(camera_format& fmt);
int get_frame(unsigned char* out_buffer, const unsigned int size, unsigned int timeout_sec=1);
int set_frame(unsigned char* in_buffer, const unsigned int size, unsigned int timeout_sec=1);
bool set_format(const char* format_description);
bool set_format(unsigned int width, unsigned int height, unsigned int pixelformat, unsigned int rate_numerator=0, unsigned int rate_denomonator=0);
bool get_control(camera_control& ctrl);
int get_control(const char* name);
bool set_control(const char* name, const int value);
int valid_controls(std::vector<std::pair<const char*, unsigned int> >& list);
int get_valid_image_format_list(std::vector<const char*>& list);
int get_valid_resolution_list(const char* format_description, std::vector<const char*>& list);
int get_valid_ratio_list(const char* resolution_description, std::vector<const char*>& list);
/**
* 동작중(streaming) 여부 확인 함수
* @return streaming status
*/
inline bool is_running() { return streaming; }
/**
* 장치 이름 제공 함수
* @return device name
*/
inline std::string get_dev_name() { return std::string((const char*) v4l2_s.capability.card); }
std::string get_serial_number();
private:
struct _buffer {
unsigned char* buffer;
unsigned int length;
};
struct _v4l2 {
enum v4l2_buf_type buf_type;
struct v4l2_capability capability;
struct v4l2_format format;
struct v4l2_requestbuffers requestbuffers;
struct v4l2_buffer buffer;
struct v4l2_queryctrl queryctrl;
struct v4l2_querymenu querymenu;
struct v4l2_fmtdesc fmtdesc;
struct v4l2_frmsizeenum frmsizeenum;
struct v4l2_frmivalenum frmivalenum;
struct v4l2_streamparm streamparm;
};
private:
std::string dev_name;
int fd;
_v4l2 v4l2_s;
_buffer* buffers;
unsigned int buffer_count;
unsigned char disable_libv4l2; /* set to 1 to disable libv4l2 calls */
std::map<std::string, v4l2_queryctrl> valid_control_list;
std::map<std::string, v4l2_fmtdesc> valid_format_list;
std::map<std::string, v4l2_frmsizeenum> valid_resolution_list;
std::map<std::string, v4l2_frmivalenum> valid_ratio_list;
std::vector<usb_device_info> usb_device_list;
struct camera_format config;
bool streaming;
bool output;
Mutex mutex;
private:
bool get_capability();
bool get_current_format();
bool set_buffer();
bool set_outbuffer();
void check_essential_capability();
int enumerate_controls();
int enumerate_control_menu();
bool enumerate_image_formats(const enum v4l2_buf_type type);
bool enumerate_frame_sizes(const unsigned int pixelformat, std::string& description);
bool enumerate_frame_intervals(const unsigned int pixelformat, const unsigned int width, const unsigned int height, std::string& description);
int get_buffer(unsigned char* yuy2_buffer, const unsigned int size);
int write_buffer(unsigned char* yuy2_buffer, const unsigned int size);
bool remove_buffers();
int xioctl(int IOCTL_X, void *arg);
int query_ioctl(int current_ctrl, struct v4l2_queryctrl* ctrl);
};
} /* namespace Withrobot */
/** @} */ // end of Withrobot
#endif /* WITHROBOT_CAMERA_HPP_ */