00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include <core/exception.h>
00025 #include <core/exceptions/software.h>
00026 #include <utils/system/console_colors.h>
00027
00028 #include <cams/swissranger.h>
00029 #include <cams/cam_exceptions.h>
00030 #include <fvutils/system/camargp.h>
00031
00032 #include <cstdlib>
00033 #include <unistd.h>
00034 #include <climits>
00035 #include <string>
00036 #include <cstring>
00037 #include <cstdio>
00038 #include <regex.h>
00039
00040 #include <usb.h>
00041 #include <libMesaSR.h>
00042
00043 using namespace std;
00044 using namespace fawkes;
00045
00046 namespace firevision {
00047 #if 0
00048 }
00049 #endif
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060 SwissRangerCamera::~SwissRangerCamera()
00061 {
00062 close();
00063
00064 if ( __model != NULL ) {
00065 free(__model);
00066 }
00067 }
00068
00069
00070 void
00071 SwissRangerCamera::open()
00072 {
00073 if (_opened) return;
00074
00075 usb_set_debug(0);
00076
00077 int num_cams = SR_OpenUSB(&__cam, 0);
00078 if (num_cams <= 0) {
00079 throw Exception("Cannot find SwissRanger camera");
00080 }
00081
00082
00083 usb_set_debug(0);
00084
00085 char devstr[1024];
00086 SR_GetDeviceString(__cam, devstr, 1024);
00087
00088 regmatch_t m[5];
00089 regex_t re;
00090 if ( (regcomp(&re, "VendorID:0x([a-fA-F0-9]{4}), ProductID:0x([a-fA-F0-9]{4}), "
00091 "Manufacturer:'([^' ]+) *', Product:'([^' ]+) *'",
00092 REG_EXTENDED) != 0) ||
00093 (regexec(&re, devstr, 5, m, 0) != 0) ) {
00094 SR_Close(__cam);
00095 throw Exception("Could not parse device string");
00096 }
00097 char *tmp;
00098 tmp = strndup(&(devstr[m[1].rm_so]), m[1].rm_eo - m[3].rm_so);
00099 __vendor_id = strtol(tmp, NULL, 16);
00100 free(tmp);
00101 tmp = strndup(&(devstr[m[2].rm_so]), m[2].rm_eo - m[3].rm_so);
00102 __product_id = strtol(tmp, NULL, 16);
00103 free(tmp);
00104 __vendor = strndup(&(devstr[m[3].rm_so]), m[3].rm_eo - m[3].rm_so);
00105 __model = strndup(&(devstr[m[4].rm_so]), m[4].rm_eo - m[4].rm_so);
00106 regfree(&re);
00107
00108 __serial = SR_ReadSerial(__cam);
00109
00110 __width = SR_GetCols(__cam);
00111 __height = SR_GetRows(__cam);
00112
00113
00114 int acqm = AM_COR_FIX_PTRN;
00115 if ( (__mode == AMPLITUDE_GRAY) || (__mode == AMPLITUDE_GRAY_8) ) {
00116 acqm |= AM_CONV_GRAY;
00117 } else if (__mode == CONF_MAP) {
00118 acqm |= AM_CONF_MAP;
00119 }
00120 if (__use_median) {
00121 acqm |= AM_MEDIAN;
00122 }
00123 if (__use_denoise) {
00124 acqm |= AM_DENOISE_ANF;
00125 }
00126 SR_SetMode(__cam, acqm);
00127
00128 if (__integration_time > 0) {
00129 SR_SetIntegrationTime(__cam, __integration_time);
00130 }
00131
00132 SR_SetAmplitudeThreshold(__cam, __amplitude_threshold);
00133
00134 if (__set_modfreq) {
00135 SR_SetModulationFrequency(__cam, __modulation_freq);
00136 }
00137
00138 __buffer_size = __width * __height;
00139 __gray_buffer = NULL;
00140 __coord_uint16_buf = NULL;
00141 __coord_float_buf = NULL;
00142 __coord_double_buf = NULL;
00143 if ( (__mode == AMPLITUDE_GRAY_8) || (__mode == DISTANCE_GRAY_8) ) {
00144 __gray_buffer = (unsigned char *)malloc(__width * __height);
00145 __buffer = __gray_buffer;
00146 } else if (__mode == CARTESIAN_UINT16) {
00147 __buffer_size = 3 * __width * __height * sizeof(unsigned short);
00148 __coord_uint16_buf = (unsigned short *)malloc(__buffer_size);
00149 __xu = (short *)__coord_uint16_buf;
00150 __yu = &(__xu[__width * __height]);
00151 __zu = (unsigned short *)&(__yu[__width * __height]);
00152 __buffer = (unsigned char *)__coord_uint16_buf;
00153 } else if (__mode == CARTESIAN_FLOAT) {
00154 __buffer_size = 3 * __width * __height * sizeof(float);
00155 __coord_float_buf = (float *)malloc(__buffer_size);
00156 __xf = __coord_float_buf;
00157 __yf = &(__coord_float_buf[ __width * __height]);
00158 __zf = &(__coord_float_buf[2 * __width * __height]);
00159 __buffer = (unsigned char *)__coord_float_buf;
00160 } else if (__mode == CARTESIAN_FLOAT) {
00161 __buffer_size = 3 * __width * __height * sizeof(double);
00162 __coord_double_buf = (double *)malloc(__buffer_size);
00163 __xd = __coord_double_buf;
00164 __yd = &(__coord_double_buf[ __width * __height]);
00165 __zd = &(__coord_double_buf[2 * __width * __height]);
00166 __buffer = (unsigned char *)__coord_double_buf;
00167 }
00168
00169 _opened = true;
00170 }
00171
00172
00173 void
00174 SwissRangerCamera::start()
00175 {
00176 if (_started) return;
00177
00178 if (! _opened) {
00179 throw Exception("SwissRangerCamera: Cannot start closed camera");
00180 }
00181
00182 _started = true;
00183 }
00184
00185
00186 void
00187 SwissRangerCamera::stop()
00188 {
00189 _started = false;
00190 }
00191
00192
00193 void
00194 SwissRangerCamera::print_info()
00195 {
00196 printf("Vendor: %-20s (0x%04x)\n"
00197 "Model: %-20s (0x%04x)\n"
00198 "Serial: %x\n",
00199 __vendor, __vendor_id, __model, __product_id, __serial);
00200 }
00201
00202
00203 void
00204 SwissRangerCamera::close()
00205 {
00206 if ( _started ) stop();
00207 if ( _opened ) {
00208 SR_Close(__cam);
00209 if (__gray_buffer) {
00210 free(__gray_buffer);
00211 __gray_buffer = NULL;
00212 }
00213 if (__coord_uint16_buf) {
00214 free(__coord_uint16_buf);
00215 __coord_uint16_buf = NULL;
00216 }
00217 if (__coord_float_buf) {
00218 free(__coord_float_buf);
00219 __coord_float_buf = NULL;
00220 }
00221 if (__coord_double_buf) {
00222 free(__coord_double_buf);
00223 __coord_double_buf = NULL;
00224 }
00225 _opened = false;
00226 }
00227 }
00228
00229
00230
00231
00232
00233 const char *
00234 SwissRangerCamera::model() const
00235 {
00236 if ( ! _opened ) {
00237 throw Exception("Camera not opened");
00238 }
00239
00240 return __model;
00241 }
00242
00243
00244 void
00245 SwissRangerCamera::capture()
00246 {
00247
00248 if (! _opened) {
00249 throw CaptureException("SwissRangerCamera(%s): cannot capture on closed camera", __model);
00250 }
00251 if (! _started) {
00252 throw CaptureException("SwissRangerCamera(%s): cannot capture on stopped camera", __model);
00253 }
00254
00255 _valid_frame_received = (SR_Acquire(__cam) > 0);
00256 if (!_valid_frame_received) {
00257 throw CaptureException("SwissRangerCamera(%s): failed to acquire image", __model);
00258 }
00259
00260 if (__mode == DISTANCE) {
00261 __buffer = (unsigned char *)SR_GetImage(__cam, 0);
00262 } else if ( (__mode == AMPLITUDE) || (__mode == AMPLITUDE_GRAY) ) {
00263 __buffer = (unsigned char *)SR_GetImage(__cam, 1);
00264 } else if ( (__mode == DISTANCE_GRAY_8) || (__mode == AMPLITUDE_GRAY_8) ) {
00265 unsigned int image_num = (__mode == DISTANCE_GRAY_8) ? 0 : 1;
00266 unsigned short *buf = (unsigned short *)SR_GetImage(__cam, image_num);
00267
00268 for (unsigned int h = 0; h < __height; ++h) {
00269 for (unsigned int w = 0; w < __width; ++w) {
00270 __gray_buffer[h * __width + w] = buf[h * __width + w] / 2;
00271 }
00272 }
00273 } else if (__mode == CONF_MAP) {
00274 __buffer = (unsigned char *)SR_GetImage(__cam, 2);
00275 } else if (__mode == CARTESIAN_UINT16) {
00276 SR_CoordTrfUint16(__cam, __xu, __yu, __zu, 2, 2, 2);
00277 } else if (__mode == CARTESIAN_FLOAT) {
00278 SR_CoordTrfFlt(__cam, __xf, __yf, __zf,
00279 sizeof(float), sizeof(float), sizeof(float));
00280 } else if (__mode == CARTESIAN_DOUBLE) {
00281 SR_CoordTrfDbl(__cam, __xd, __yd, __zd,
00282 sizeof(double), sizeof(double), sizeof(double));
00283 }
00284 }
00285
00286
00287 void
00288 SwissRangerCamera::flush()
00289 {
00290 }
00291
00292
00293 unsigned char*
00294 SwissRangerCamera::buffer()
00295 {
00296 if ( _valid_frame_received ) {
00297 return __buffer;
00298 } else {
00299 return NULL;
00300 }
00301 }
00302
00303
00304 unsigned int
00305 SwissRangerCamera::buffer_size()
00306 {
00307 if ( _valid_frame_received ) {
00308 return __buffer_size;
00309 } else {
00310 return 0;
00311 }
00312 }
00313
00314 void
00315 SwissRangerCamera::dispose_buffer()
00316 {
00317 _valid_frame_received = false;
00318 }
00319
00320
00321 unsigned int
00322 SwissRangerCamera::pixel_width()
00323 {
00324 if (_opened) {
00325 return __width;
00326 } else {
00327 throw Exception("Camera not opened");
00328 }
00329 }
00330
00331
00332 unsigned int
00333 SwissRangerCamera::pixel_height()
00334 {
00335 if (_opened) {
00336 return __height;
00337 } else {
00338 throw Exception("Camera not opened");
00339 }
00340 }
00341
00342
00343 colorspace_t
00344 SwissRangerCamera::colorspace()
00345 {
00346 switch (__mode) {
00347 case DISTANCE:
00348 case AMPLITUDE:
00349 case CONF_MAP:
00350 case CARTESIAN_UINT16:
00351 return RAW16;
00352 case AMPLITUDE_GRAY:
00353 return MONO16;
00354 case DISTANCE_GRAY_8:
00355 case AMPLITUDE_GRAY_8:
00356 return GRAY8;
00357 case CARTESIAN_FLOAT:
00358 return CARTESIAN_3D_FLOAT;
00359 case CARTESIAN_DOUBLE:
00360 return CARTESIAN_3D_DOUBLE;
00361 }
00362
00363 return RAW16;
00364 }
00365
00366
00367 bool
00368 SwissRangerCamera::ready()
00369 {
00370 return _started;
00371 }
00372
00373
00374 void
00375 SwissRangerCamera::set_image_number(unsigned int n)
00376 {
00377 }
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413 SwissRangerCamera::SwissRangerCamera(const CameraArgumentParser *cap)
00414 {
00415 _started = _opened = false;
00416 _valid_frame_received = false;
00417
00418 __model = __vendor = NULL;
00419 __vendor_id = __product_id = 0;
00420
00421 __buffer = NULL;
00422
00423 __mode = AMPLITUDE_GRAY_8;
00424 if (cap->has("mode")) {
00425 string m = cap->get("mode");
00426 if (m == "DISTANCE") {
00427 __mode = DISTANCE;
00428 } else if (m == "DISTANCE_GRAY_8") {
00429 __mode = DISTANCE_GRAY_8;
00430 } else if (m == "AMPLITUDE") {
00431 __mode = AMPLITUDE;
00432 } else if (m == "AMPLITUDE_GRAY") {
00433 __mode = AMPLITUDE_GRAY;
00434 } else if (m == "AMPLITUDE_GRAY_8") {
00435 __mode = AMPLITUDE_GRAY_8;
00436 } else if (m == "CONF_MAP") {
00437 __mode = CONF_MAP;
00438 } else if (m == "CARTESIAN_UINT16") {
00439 __mode = CARTESIAN_UINT16;
00440 } else if (m == "CARTESIAN_FLOAT") {
00441 __mode = CARTESIAN_FLOAT;
00442 } else if (m == "CARTESIAN_DOUBLE") {
00443 __mode = CARTESIAN_DOUBLE;
00444 } else {
00445 throw Exception("Unknown mode %s given", m.c_str());
00446 }
00447 }
00448
00449 __use_median = false;
00450 if (cap->get("median") == "on") {
00451 __use_median=true;
00452 }
00453
00454 __use_denoise = false;
00455 if (cap->get("denoise") == "on") {
00456 __use_denoise=true;
00457 }
00458
00459 __integration_time = 0;
00460 if (cap->has("integration_time")) {
00461 __integration_time = cap->get_int("integration_time");
00462 }
00463
00464 __amplitude_threshold = 0;
00465 if (cap->has("amplitude_threshold")) {
00466 __amplitude_threshold = cap->get_int("amplitude_threshold");
00467 }
00468
00469 __set_modfreq = false;
00470 __modulation_freq = MF_40MHz;
00471 if (cap->has("modfreq")) {
00472 string m = cap->get("modfreq");
00473 __set_modfreq = true;
00474 if (m == "40MHz") {
00475 __modulation_freq = MF_40MHz;
00476 } else if (m == "30MHz") {
00477 __modulation_freq = MF_30MHz;
00478 } else if (m == "21MHz") {
00479 __modulation_freq = MF_21MHz;
00480 } else if (m == "20MHz") {
00481 __modulation_freq = MF_20MHz;
00482 } else if (m == "19MHz") {
00483 __modulation_freq = MF_19MHz;
00484 } else if (m == "60MHz") {
00485 __modulation_freq = MF_60MHz;
00486 } else if (m == "15MHz") {
00487 __modulation_freq = MF_15MHz;
00488 } else if (m == "10MHz") {
00489 __modulation_freq = MF_10MHz;
00490 } else if (m == "29MHz") {
00491 __modulation_freq = MF_29MHz;
00492 } else if (m == "31MHz") {
00493 __modulation_freq = MF_31MHz;
00494 } else if (m == "14.5MHz") {
00495 __modulation_freq = MF_14_5MHz;
00496 } else if (m == "15.5MHz") {
00497 __modulation_freq = MF_15_5MHz;
00498 } else {
00499 throw Exception("Unknown modulation frequency %s given", m.c_str());
00500 }
00501 }
00502
00503 }
00504
00505
00506
00507
00508
00509 void
00510 SwissRangerCamera::print_available_cams()
00511 {
00512 SRCAM cams[16];
00513
00514 int num_cams = SR_OpenAll(cams, 16, 0, 0xFFFFFFFF);
00515 if (num_cams < 0) {
00516 printf("Error opening SwissRanger cameras\n");
00517 } else if (num_cams == 0) {
00518 printf("No SwissRanger camera found\n");
00519 } else {
00520 for (int i = 0; i < 1; ++i) {
00521 char devstr[1024];
00522 SR_GetDeviceString(cams[i], devstr, 1024);
00523 unsigned int serial = SR_ReadSerial(cams[i]);
00524 printf("%s, Serial:'%x'\n", devstr, serial);
00525 SR_Close(cams[i]);
00526 }
00527 }
00528 }
00529
00530 }
00531