24 #include <core/exception.h> 25 #include <core/exceptions/software.h> 26 #include <utils/system/console_colors.h> 28 #include <fvcams/swissranger.h> 29 #include <fvcams/cam_exceptions.h> 30 #include <fvutils/system/camargp.h> 41 #include <libMesaSR.h> 60 SwissRangerCamera::~SwissRangerCamera()
64 if ( __model != NULL ) {
71 SwissRangerCamera::open()
77 int num_cams = SR_OpenUSB(&__cam, 0);
79 throw Exception(
"Cannot find SwissRanger camera");
86 SR_GetDeviceString(__cam, devstr, 1024);
90 if ( (regcomp(&re,
"VendorID:0x([a-fA-F0-9]{4}), ProductID:0x([a-fA-F0-9]{4}), " 91 "Manufacturer:'([^' ]+) *', Product:'([^' ]+) *'",
92 REG_EXTENDED) != 0) ||
93 (regexec(&re, devstr, 5, m, 0) != 0) ) {
95 throw Exception(
"Could not parse device string");
98 tmp = strndup(&(devstr[m[1].rm_so]), m[1].rm_eo - m[3].rm_so);
99 __vendor_id = strtol(tmp, NULL, 16);
101 tmp = strndup(&(devstr[m[2].rm_so]), m[2].rm_eo - m[3].rm_so);
102 __product_id = strtol(tmp, NULL, 16);
104 __vendor = strndup(&(devstr[m[3].rm_so]), m[3].rm_eo - m[3].rm_so);
105 __model = strndup(&(devstr[m[4].rm_so]), m[4].rm_eo - m[4].rm_so);
108 __serial = SR_ReadSerial(__cam);
110 __width = SR_GetCols(__cam);
111 __height = SR_GetRows(__cam);
114 int acqm = AM_COR_FIX_PTRN;
115 if ( (__mode == AMPLITUDE_GRAY) || (__mode == AMPLITUDE_GRAY_8) ) {
116 acqm |= AM_CONV_GRAY;
117 }
else if (__mode == CONF_MAP) {
124 acqm |= AM_DENOISE_ANF;
126 SR_SetMode(__cam, acqm);
128 if (__integration_time > 0) {
129 SR_SetIntegrationTime(__cam, __integration_time);
132 SR_SetAmplitudeThreshold(__cam, __amplitude_threshold);
135 SR_SetModulationFrequency(__cam, __modulation_freq);
138 __buffer_size = __width * __height;
139 __gray_buffer = NULL;
140 __coord_uint16_buf = NULL;
141 __coord_float_buf = NULL;
142 __coord_double_buf = NULL;
143 if ( (__mode == AMPLITUDE_GRAY_8) || (__mode == DISTANCE_GRAY_8) ) {
144 __gray_buffer = (
unsigned char *)malloc(__width * __height);
145 __buffer = __gray_buffer;
146 }
else if (__mode == CARTESIAN_UINT16) {
147 __buffer_size = 3 * __width * __height *
sizeof(
unsigned short);
148 __coord_uint16_buf = (
unsigned short *)malloc(__buffer_size);
149 __xu = (
short *)__coord_uint16_buf;
150 __yu = &(__xu[__width * __height]);
151 __zu = (
unsigned short *)&(__yu[__width * __height]);
152 __buffer = (
unsigned char *)__coord_uint16_buf;
153 }
else if (__mode == CARTESIAN_FLOAT) {
154 __buffer_size = 3 * __width * __height *
sizeof(float);
155 __coord_float_buf = (
float *)malloc(__buffer_size);
156 __xf = __coord_float_buf;
157 __yf = &(__coord_float_buf[ __width * __height]);
158 __zf = &(__coord_float_buf[2 * __width * __height]);
159 __buffer = (
unsigned char *)__coord_float_buf;
160 }
else if (__mode == CARTESIAN_FLOAT) {
161 __buffer_size = 3 * __width * __height *
sizeof(double);
162 __coord_double_buf = (
double *)malloc(__buffer_size);
163 __xd = __coord_double_buf;
164 __yd = &(__coord_double_buf[ __width * __height]);
165 __zd = &(__coord_double_buf[2 * __width * __height]);
166 __buffer = (
unsigned char *)__coord_double_buf;
174 SwissRangerCamera::start()
176 if (_started)
return;
179 throw Exception(
"SwissRangerCamera: Cannot start closed camera");
187 SwissRangerCamera::stop()
194 SwissRangerCamera::print_info()
196 printf(
"Vendor: %-20s (0x%04x)\n" 197 "Model: %-20s (0x%04x)\n" 199 __vendor, __vendor_id, __model, __product_id, __serial);
204 SwissRangerCamera::close()
206 if ( _started ) stop();
211 __gray_buffer = NULL;
213 if (__coord_uint16_buf) {
214 free(__coord_uint16_buf);
215 __coord_uint16_buf = NULL;
217 if (__coord_float_buf) {
218 free(__coord_float_buf);
219 __coord_float_buf = NULL;
221 if (__coord_double_buf) {
222 free(__coord_double_buf);
223 __coord_double_buf = NULL;
234 SwissRangerCamera::model()
const 245 SwissRangerCamera::capture()
249 throw CaptureException(
"SwissRangerCamera(%s): cannot capture on closed camera", __model);
252 throw CaptureException(
"SwissRangerCamera(%s): cannot capture on stopped camera", __model);
255 _valid_frame_received = (SR_Acquire(__cam) > 0);
256 if (!_valid_frame_received) {
257 throw CaptureException(
"SwissRangerCamera(%s): failed to acquire image", __model);
260 if (__mode == DISTANCE) {
261 __buffer = (
unsigned char *)SR_GetImage(__cam, 0);
262 }
else if ( (__mode == AMPLITUDE) || (__mode == AMPLITUDE_GRAY) ) {
263 __buffer = (
unsigned char *)SR_GetImage(__cam, 1);
264 }
else if ( (__mode == DISTANCE_GRAY_8) || (__mode == AMPLITUDE_GRAY_8) ) {
265 unsigned int image_num = (__mode == DISTANCE_GRAY_8) ? 0 : 1;
266 unsigned short *buf = (
unsigned short *)SR_GetImage(__cam, image_num);
268 for (
unsigned int h = 0; h < __height; ++h) {
269 for (
unsigned int w = 0; w < __width; ++w) {
270 __gray_buffer[h * __width + w] = buf[h * __width + w] / 2;
273 }
else if (__mode == CONF_MAP) {
274 __buffer = (
unsigned char *)SR_GetImage(__cam, 2);
275 }
else if (__mode == CARTESIAN_UINT16) {
276 SR_CoordTrfUint16(__cam, __xu, __yu, __zu, 2, 2, 2);
277 }
else if (__mode == CARTESIAN_FLOAT) {
278 SR_CoordTrfFlt(__cam, __xf, __yf, __zf,
279 sizeof(
float),
sizeof(
float),
sizeof(
float));
280 }
else if (__mode == CARTESIAN_DOUBLE) {
281 SR_CoordTrfDbl(__cam, __xd, __yd, __zd,
282 sizeof(
double),
sizeof(
double),
sizeof(
double));
288 SwissRangerCamera::flush()
294 SwissRangerCamera::buffer()
296 if ( _valid_frame_received ) {
305 SwissRangerCamera::buffer_size()
307 if ( _valid_frame_received ) {
308 return __buffer_size;
315 SwissRangerCamera::dispose_buffer()
317 _valid_frame_received =
false;
322 SwissRangerCamera::pixel_width()
333 SwissRangerCamera::pixel_height()
344 SwissRangerCamera::colorspace()
350 case CARTESIAN_UINT16:
354 case DISTANCE_GRAY_8:
355 case AMPLITUDE_GRAY_8:
357 case CARTESIAN_FLOAT:
358 return CARTESIAN_3D_FLOAT;
359 case CARTESIAN_DOUBLE:
360 return CARTESIAN_3D_DOUBLE;
368 SwissRangerCamera::ready()
375 SwissRangerCamera::set_image_number(
unsigned int n)
415 _started = _opened =
false;
416 _valid_frame_received =
false;
418 __model = __vendor = NULL;
419 __vendor_id = __product_id = 0;
423 __mode = AMPLITUDE_GRAY_8;
424 if (cap->
has(
"mode")) {
425 string m = cap->
get(
"mode");
426 if (m ==
"DISTANCE") {
428 }
else if (m ==
"DISTANCE_GRAY_8") {
429 __mode = DISTANCE_GRAY_8;
430 }
else if (m ==
"AMPLITUDE") {
432 }
else if (m ==
"AMPLITUDE_GRAY") {
433 __mode = AMPLITUDE_GRAY;
434 }
else if (m ==
"AMPLITUDE_GRAY_8") {
435 __mode = AMPLITUDE_GRAY_8;
436 }
else if (m ==
"CONF_MAP") {
438 }
else if (m ==
"CARTESIAN_UINT16") {
439 __mode = CARTESIAN_UINT16;
440 }
else if (m ==
"CARTESIAN_FLOAT") {
441 __mode = CARTESIAN_FLOAT;
442 }
else if (m ==
"CARTESIAN_DOUBLE") {
443 __mode = CARTESIAN_DOUBLE;
445 throw Exception(
"Unknown mode %s given", m.c_str());
449 __use_median =
false;
450 if (cap->
get(
"median") ==
"on") {
454 __use_denoise =
false;
455 if (cap->
get(
"denoise") ==
"on") {
459 __integration_time = 0;
460 if (cap->
has(
"integration_time")) {
461 __integration_time = cap->
get_int(
"integration_time");
464 __amplitude_threshold = 0;
465 if (cap->
has(
"amplitude_threshold")) {
466 __amplitude_threshold = cap->
get_int(
"amplitude_threshold");
469 __set_modfreq =
false;
470 __modulation_freq = MF_40MHz;
471 if (cap->
has(
"modfreq")) {
472 string m = cap->
get(
"modfreq");
473 __set_modfreq =
true;
475 __modulation_freq = MF_40MHz;
476 }
else if (m ==
"30MHz") {
477 __modulation_freq = MF_30MHz;
478 }
else if (m ==
"21MHz") {
479 __modulation_freq = MF_21MHz;
480 }
else if (m ==
"20MHz") {
481 __modulation_freq = MF_20MHz;
482 }
else if (m ==
"19MHz") {
483 __modulation_freq = MF_19MHz;
484 }
else if (m ==
"60MHz") {
485 __modulation_freq = MF_60MHz;
486 }
else if (m ==
"15MHz") {
487 __modulation_freq = MF_15MHz;
488 }
else if (m ==
"10MHz") {
489 __modulation_freq = MF_10MHz;
490 }
else if (m ==
"29MHz") {
491 __modulation_freq = MF_29MHz;
492 }
else if (m ==
"31MHz") {
493 __modulation_freq = MF_31MHz;
494 }
else if (m ==
"14.5MHz") {
495 __modulation_freq = MF_14_5MHz;
496 }
else if (m ==
"15.5MHz") {
497 __modulation_freq = MF_15_5MHz;
499 throw Exception(
"Unknown modulation frequency %s given", m.c_str());
510 SwissRangerCamera::print_available_cams()
514 int num_cams = SR_OpenAll(cams, 16, 0, 0xFFFFFFFF);
516 printf(
"Error opening SwissRanger cameras\n");
517 }
else if (num_cams == 0) {
518 printf(
"No SwissRanger camera found\n");
520 for (
int i = 0; i < 1; ++i) {
522 SR_GetDeviceString(cams[i], devstr, 1024);
523 unsigned int serial = SR_ReadSerial(cams[i]);
524 printf(
"%s, Serial:'%x'\n", devstr, serial);
Fawkes library namespace.
Capturing a frame failed.
bool has(std::string s) const
Check if an parameter was given.
Base class for exceptions in Fawkes.
long int get_int(std::string s) const
Get the value of the given parameter as integer.
std::string get(std::string s) const
Get the value of the given parameter.