swissranger.cpp

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 

Generated on Tue Feb 22 13:32:14 2011 for Fawkes API by  doxygen 1.4.7