Fawkes API  Fawkes Development Version
swissranger.cpp
1 
2 /***************************************************************************
3  * swissranger.cpp - Implementation to access SwissRanger SR4000 camera
4  *
5  * Created: Wed Jan 13 17:02:39 2010
6  * Copyright 2005-2010 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22  */
23 
24 #include <core/exception.h>
25 #include <core/exceptions/software.h>
26 #include <utils/system/console_colors.h>
27 
28 #include <fvcams/swissranger.h>
29 #include <fvcams/cam_exceptions.h>
30 #include <fvutils/system/camargp.h>
31 
32 #include <cstdlib>
33 #include <unistd.h>
34 #include <climits>
35 #include <string>
36 #include <cstring>
37 #include <cstdio>
38 #include <regex.h>
39 
40 #include <usb.h>
41 #include <libMesaSR.h>
42 
43 using namespace std;
44 using namespace fawkes;
45 
46 namespace firevision {
47 #if 0 /* just to make Emacs auto-indent happy */
48 }
49 #endif
50 
51 /** @class SwissRangerCamera <fvcams/swissranger.h>
52  * SwissRanger camera.
53  * This camera implementation provides access to the SwissRanger SR4000 camera
54  * (and probably other models supported by libmesasr, but cannot test).
55  * @author Tim Niemueller
56  */
57 
58 
59 /** Empty destructor. */
60 SwissRangerCamera::~SwissRangerCamera()
61 {
62  close();
63 
64  if ( __model != NULL ) {
65  free(__model);
66  }
67 }
68 
69 
70 void
71 SwissRangerCamera::open()
72 {
73  if (_opened) return;
74 
75  usb_set_debug(0);
76 
77  int num_cams = SR_OpenUSB(&__cam, 0);
78  if (num_cams <= 0) {
79  throw Exception("Cannot find SwissRanger camera");
80  }
81 
82  // turn off debugging after open, it already sucks during open...
83  usb_set_debug(0);
84 
85  char devstr[1024];
86  SR_GetDeviceString(__cam, devstr, 1024);
87 
88  regmatch_t m[5];
89  regex_t re;
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) ) {
94  SR_Close(__cam);
95  throw Exception("Could not parse device string");
96  }
97  char *tmp;
98  tmp = strndup(&(devstr[m[1].rm_so]), m[1].rm_eo - m[3].rm_so);
99  __vendor_id = strtol(tmp, NULL, 16);
100  free(tmp);
101  tmp = strndup(&(devstr[m[2].rm_so]), m[2].rm_eo - m[3].rm_so);
102  __product_id = strtol(tmp, NULL, 16);
103  free(tmp);
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);
106  regfree(&re);
107 
108  __serial = SR_ReadSerial(__cam);
109 
110  __width = SR_GetCols(__cam);
111  __height = SR_GetRows(__cam);
112 
113 
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) {
118  acqm |= AM_CONF_MAP;
119  }
120  if (__use_median) {
121  acqm |= AM_MEDIAN;
122  }
123  if (__use_denoise) {
124  acqm |= AM_DENOISE_ANF;
125  }
126  SR_SetMode(__cam, acqm);
127 
128  if (__integration_time > 0) {
129  SR_SetIntegrationTime(__cam, __integration_time);
130  }
131 
132  SR_SetAmplitudeThreshold(__cam, __amplitude_threshold);
133 
134  if (__set_modfreq) {
135  SR_SetModulationFrequency(__cam, __modulation_freq);
136  }
137 
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;
167  }
168 
169  _opened = true;
170 }
171 
172 
173 void
174 SwissRangerCamera::start()
175 {
176  if (_started) return;
177 
178  if (! _opened) {
179  throw Exception("SwissRangerCamera: Cannot start closed camera");
180  }
181 
182  _started = true;
183 }
184 
185 
186 void
187 SwissRangerCamera::stop()
188 {
189  _started = false;
190 }
191 
192 
193 void
194 SwissRangerCamera::print_info()
195 {
196  printf("Vendor: %-20s (0x%04x)\n"
197  "Model: %-20s (0x%04x)\n"
198  "Serial: %x\n",
199  __vendor, __vendor_id, __model, __product_id, __serial);
200 }
201 
202 
203 void
204 SwissRangerCamera::close()
205 {
206  if ( _started ) stop();
207  if ( _opened ) {
208  SR_Close(__cam);
209  if (__gray_buffer) {
210  free(__gray_buffer);
211  __gray_buffer = NULL;
212  }
213  if (__coord_uint16_buf) {
214  free(__coord_uint16_buf);
215  __coord_uint16_buf = NULL;
216  }
217  if (__coord_float_buf) {
218  free(__coord_float_buf);
219  __coord_float_buf = NULL;
220  }
221  if (__coord_double_buf) {
222  free(__coord_double_buf);
223  __coord_double_buf = NULL;
224  }
225  _opened = false;
226  }
227 }
228 
229 
230 /** Get camera model.
231  * @return string with the camera model name
232  */
233 const char *
234 SwissRangerCamera::model() const
235 {
236  if ( ! _opened ) {
237  throw Exception("Camera not opened");
238  }
239 
240  return __model;
241 }
242 
243 
244 void
245 SwissRangerCamera::capture()
246 {
247 
248  if (! _opened) {
249  throw CaptureException("SwissRangerCamera(%s): cannot capture on closed camera", __model);
250  }
251  if (! _started) {
252  throw CaptureException("SwissRangerCamera(%s): cannot capture on stopped camera", __model);
253  }
254 
255  _valid_frame_received = (SR_Acquire(__cam) > 0);
256  if (!_valid_frame_received) {
257  throw CaptureException("SwissRangerCamera(%s): failed to acquire image", __model);
258  }
259 
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);
267  // convert image
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;
271  }
272  }
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));
283  }
284 }
285 
286 
287 void
288 SwissRangerCamera::flush()
289 {
290 }
291 
292 
293 unsigned char*
294 SwissRangerCamera::buffer()
295 {
296  if ( _valid_frame_received ) {
297  return __buffer;
298  } else {
299  return NULL;
300  }
301 }
302 
303 
304 unsigned int
305 SwissRangerCamera::buffer_size()
306 {
307  if ( _valid_frame_received ) {
308  return __buffer_size;
309  } else {
310  return 0;
311  }
312 }
313 
314 void
315 SwissRangerCamera::dispose_buffer()
316 {
317  _valid_frame_received = false;
318 }
319 
320 
321 unsigned int
322 SwissRangerCamera::pixel_width()
323 {
324  if (_opened) {
325  return __width;
326  } else {
327  throw Exception("Camera not opened");
328  }
329 }
330 
331 
332 unsigned int
333 SwissRangerCamera::pixel_height()
334 {
335  if (_opened) {
336  return __height;
337  } else {
338  throw Exception("Camera not opened");
339  }
340 }
341 
342 
343 colorspace_t
344 SwissRangerCamera::colorspace()
345 {
346  switch (__mode) {
347  case DISTANCE:
348  case AMPLITUDE:
349  case CONF_MAP:
350  case CARTESIAN_UINT16:
351  return RAW16;
352  case AMPLITUDE_GRAY:
353  return MONO16;
354  case DISTANCE_GRAY_8:
355  case AMPLITUDE_GRAY_8:
356  return GRAY8;
357  case CARTESIAN_FLOAT:
358  return CARTESIAN_3D_FLOAT;
359  case CARTESIAN_DOUBLE:
360  return CARTESIAN_3D_DOUBLE;
361  }
362 
363  return RAW16;
364 }
365 
366 
367 bool
368 SwissRangerCamera::ready()
369 {
370  return _started;
371 }
372 
373 
374 void
375 SwissRangerCamera::set_image_number(unsigned int n)
376 {
377 }
378 
379 /** Constructor.
380  * Initialize and take parameters from camera argument parser. The following
381  * arguments are supported:
382  * - mode=MODE where MODE is one of
383  * - DISTANCE
384  * - DISTANCE_GRAY_8
385  * - AMPLITUDE
386  * - AMPLITUDE_GRAY
387  * - AMPLITUDE_GRAY_8
388  * - CONF_MAP
389  * - CARTESIAN_UINT16
390  * - CARTESIAN_FLOAT
391  * - CARTESIAN_DOUBLE
392  * - median=on (enable median filter)
393  * - denoise=on (enable denoise filter)
394  * - modfreq=MODFREQ where MODFREQ (modulation frequency) is one of
395  * - 40MHz
396  * - 30MHz
397  * - 21MHz
398  * - 20MHz
399  * - 19MHz
400  * - 60MHz
401  * - 15MHz
402  * - 10MHz
403  * - 29MHz
404  * - 31MHz
405  * - 14.5MHz
406  * - 15.5MHz
407  * - integration_time=NUM
408  * integration time, confer camera's API documentation
409  * - amplitude_threshold=NUM
410  * amplitude threshold, must be unsigned 16 bit value
411  * @param cap camera argument parser
412  */
413 SwissRangerCamera::SwissRangerCamera(const CameraArgumentParser *cap)
414 {
415  _started = _opened = false;
416  _valid_frame_received = false;
417 
418  __model = __vendor = NULL;
419  __vendor_id = __product_id = 0;
420 
421  __buffer = NULL;
422 
423  __mode = AMPLITUDE_GRAY_8;
424  if (cap->has("mode")) {
425  string m = cap->get("mode");
426  if (m == "DISTANCE") {
427  __mode = DISTANCE;
428  } else if (m == "DISTANCE_GRAY_8") {
429  __mode = DISTANCE_GRAY_8;
430  } else if (m == "AMPLITUDE") {
431  __mode = 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") {
437  __mode = 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;
444  } else {
445  throw Exception("Unknown mode %s given", m.c_str());
446  }
447  }
448 
449  __use_median = false;
450  if (cap->get("median") == "on") {
451  __use_median=true;
452  }
453 
454  __use_denoise = false;
455  if (cap->get("denoise") == "on") {
456  __use_denoise=true;
457  }
458 
459  __integration_time = 0; // do not set
460  if (cap->has("integration_time")) {
461  __integration_time = cap->get_int("integration_time");
462  }
463 
464  __amplitude_threshold = 0;
465  if (cap->has("amplitude_threshold")) {
466  __amplitude_threshold = cap->get_int("amplitude_threshold");
467  }
468 
469  __set_modfreq = false;
470  __modulation_freq = MF_40MHz;
471  if (cap->has("modfreq")) {
472  string m = cap->get("modfreq");
473  __set_modfreq = true;
474  if (m == "40MHz") {
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;
498  } else {
499  throw Exception("Unknown modulation frequency %s given", m.c_str());
500  }
501  }
502 
503 }
504 
505 
506 /** Print list of cameras.
507  * Prints a list of available cameras to stdout.
508  */
509 void
510 SwissRangerCamera::print_available_cams()
511 {
512  SRCAM cams[16];
513  //SR_SetCallback(sr_callback);
514  int num_cams = SR_OpenAll(cams, 16, 0, 0xFFFFFFFF);
515  if (num_cams < 0) {
516  printf("Error opening SwissRanger cameras\n");
517  } else if (num_cams == 0) {
518  printf("No SwissRanger camera found\n");
519  } else {
520  for (int i = 0; i < 1; ++i) {
521  char devstr[1024];
522  SR_GetDeviceString(cams[i], devstr, 1024);
523  unsigned int serial = SR_ReadSerial(cams[i]);
524  printf("%s, Serial:'%x'\n", devstr, serial);
525  SR_Close(cams[i]);
526  }
527  }
528 }
529 
530 } // end namespace firevision
531 
Fawkes library namespace.
Capturing a frame failed.
STL namespace.
Camera argument parser.
Definition: camargp.h:38
bool has(std::string s) const
Check if an parameter was given.
Definition: camargp.cpp:152
Base class for exceptions in Fawkes.
Definition: exception.h:36
long int get_int(std::string s) const
Get the value of the given parameter as integer.
Definition: camargp.cpp:184
std::string get(std::string s) const
Get the value of the given parameter.
Definition: camargp.cpp:164