Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * swissranger.cpp - Implementation to access SwissRanger SR4000 camera 00004 * 00005 * Created: Wed Jan 13 17:02:39 2010 00006 * Copyright 2005-2010 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. A runtime exception applies to 00014 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 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 /* just to make Emacs auto-indent happy */ 00048 } 00049 #endif 00050 00051 /** @class SwissRangerCamera <cams/swissranger.h> 00052 * SwissRanger camera. 00053 * This camera implementation provides access to the SwissRanger SR4000 camera 00054 * (and probably other models supported by libmesasr, but cannot test). 00055 * @author Tim Niemueller 00056 */ 00057 00058 00059 /** Empty destructor. */ 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 // turn off debugging after open, it already sucks during open... 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 /** Get camera model. 00231 * @return string with the camera model name 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 // convert image 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 /** Constructor. 00380 * Initialize and take parameters from camera argument parser. The following 00381 * arguments are supported: 00382 * - mode=MODE where MODE is one of 00383 * - DISTANCE 00384 * - DISTANCE_GRAY_8 00385 * - AMPLITUDE 00386 * - AMPLITUDE_GRAY 00387 * - AMPLITUDE_GRAY_8 00388 * - CONF_MAP 00389 * - CARTESIAN_UINT16 00390 * - CARTESIAN_FLOAT 00391 * - CARTESIAN_DOUBLE 00392 * - median=on (enable median filter) 00393 * - denoise=on (enable denoise filter) 00394 * - modfreq=MODFREQ where MODFREQ (modulation frequency) is one of 00395 * - 40MHz 00396 * - 30MHz 00397 * - 21MHz 00398 * - 20MHz 00399 * - 19MHz 00400 * - 60MHz 00401 * - 15MHz 00402 * - 10MHz 00403 * - 29MHz 00404 * - 31MHz 00405 * - 14.5MHz 00406 * - 15.5MHz 00407 * - integration_time=NUM 00408 * integration time, confer camera's API documentation 00409 * - amplitude_threshold=NUM 00410 * amplitude threshold, must be unsigned 16 bit value 00411 * @param cap camera argument parser 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; // do not set 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 /** Print list of cameras. 00507 * Prints a list of available cameras to stdout. 00508 */ 00509 void 00510 SwissRangerCamera::print_available_cams() 00511 { 00512 SRCAM cams[16]; 00513 //SR_SetCallback(sr_callback); 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 } // end namespace firevision 00531