Visual Servoing Platform  version 3.5.1 under development (2022-12-02)
vp1394CMUGrabber.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
33  *
34  * Authors:
35  * Lucas Lopes Lemos FEMTO-ST, AS2M departement, Besancon
36  * Guillaume Laurent FEMTO-ST, AS2M departement, Besancon
37  * Fabien Spindler
38  *
39  *****************************************************************************/
40 
41 #include <visp3/core/vpConfig.h>
42 
43 #ifdef VISP_HAVE_CMU1394
44 
45 #include <iostream>
46 
47 #include <visp3/core/vpImageConvert.h>
48 #include <visp3/sensor/vp1394CMUGrabber.h>
49 
54  : index(0), // If a camera was not selected the first one (index = 0) will
55  // be used
56  _format(-1), _mode(-1), _fps(-1), _modeauto(true), _gain(0), _shutter(0), _color(vp1394CMUGrabber::UNKNOWN)
57 {
58  // public members
59  init = false;
60 
61  // protected members
62  width = height = -1;
63 
64  // private members
65  camera = new C1394Camera;
66 }
67 
72 {
73  close();
74  // delete camera instance
75  if (camera) {
76  delete camera;
77  camera = NULL;
78  }
79 }
80 
86 {
87  int camerror;
88 
89  index = cam_id;
90 
91  camerror = camera->SelectCamera(index);
92  if (camerror != CAM_SUCCESS) {
93  switch (camerror) {
94  case CAM_ERROR_PARAM_OUT_OF_RANGE:
95  vpERROR_TRACE("vp1394CMUGrabber error: Found no camera number %i", index);
96  throw(
97  vpFrameGrabberException(vpFrameGrabberException::initializationError, "The required camera is not present"));
98  break;
99  case CAM_ERROR_BUSY:
100  vpERROR_TRACE("vp1394CMUGrabber error: The camera %i is busy", index);
102  "The required camera is in use by other application"));
103  break;
104  case CAM_ERROR:
105  vpERROR_TRACE("vp1394CMUGrabber error: General I/O error when "
106  "selecting camera number %i",
107  index);
108  throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Resolve camera can not be used"));
109  break;
110  }
111  close();
112  }
113 } // end camera select
114 
118 void vp1394CMUGrabber::initCamera()
119 {
120  if (init == false) {
121  int camerror;
122 
123  if (camera->CheckLink() != CAM_SUCCESS) {
124  vpERROR_TRACE("C1394Camera error: Found no cameras on the 1394 bus");
125  throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "The is no detected camera"));
126  }
127 
128  camerror = camera->InitCamera();
129  if (camerror != CAM_SUCCESS) {
130  switch (camerror) {
131  case CAM_ERROR_NOT_INITIALIZED:
132  vpERROR_TRACE("vp1394CMUGrabber error: No camera selected", index);
133  throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "The is no selected camera"));
134  break;
135  case CAM_ERROR_BUSY:
136  vpERROR_TRACE("vp1394CMUGrabber error: The camera %i is busy", index);
138  "The required camera is in use by other application"));
139  break;
140  case CAM_ERROR:
141  vpERROR_TRACE("vp1394CMUGrabber error: General I/O error when "
142  "selecting camera number %i",
143  index);
144  throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Resolve camera can not be used"));
145  break;
146  }
147  close();
148  }
149 
150  if (camera->Has1394b())
151  camera->Set1394b(TRUE);
152 
153  // Get the current settings
154  _format = camera->GetVideoFormat();
155  _mode = camera->GetVideoMode();
156  _color = getVideoColorCoding();
157  // std::cout << "format: " << _format << std::endl;
158  // std::cout << "mode: " << _mode << std::endl;
159  // std::cout << "color coding: " << _color << std::endl;
160 
161  // Set trigger off
162  camera->GetCameraControlTrigger()->SetOnOff(false);
163 
164  unsigned long w, h;
165  camera->GetVideoFrameDimensions(&w, &h);
166  this->width = w;
167  this->height = h;
168 
169  // start acquisition
170  if (camera->StartImageAcquisition() != CAM_SUCCESS) {
171  close();
172  vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition "
173  "from IEEE 1394 camera number %i",
174  index);
175  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while starting image acquisition"));
176  }
177 
178  init = true;
179  }
180 
181 } // end camera init
182 
188 {
189  initCamera();
190  I.resize(this->height, this->width);
191 }
192 
198 {
199  initCamera();
200  I.resize(this->height, this->width);
201 }
202 
211 {
212  // get image data
213  unsigned long length;
214  unsigned char *rawdata = NULL;
215  int dropped;
216  unsigned int size;
217 
218  open(I);
219 
220  camera->AcquireImageEx(TRUE, &dropped);
221  rawdata = camera->GetRawData(&length);
222 
223  size = I.getSize();
224  switch (_color) {
226  memcpy(I.bitmap, (unsigned char *)rawdata, size);
227  break;
229  vpImageConvert::MONO16ToGrey(rawdata, I.bitmap, size);
230  break;
231 
233  vpImageConvert::YUV411ToGrey(rawdata, I.bitmap, size);
234  break;
235 
237  vpImageConvert::YUV422ToGrey(rawdata, I.bitmap, size);
238  break;
239 
241  vpImageConvert::YUV444ToGrey(rawdata, I.bitmap, size);
242  break;
243 
245  vpImageConvert::RGBToGrey(rawdata, I.bitmap, size);
246  break;
247 
248  default:
249  close();
250  vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
251  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Format conversion not implemented. "
252  "Acquisition failed."));
253  break;
254  };
255 
256  // unsigned short depth = 0;
257  // camera->GetVideoDataDepth(&depth);
258  // std::cout << "depth: " << depth << " computed: " <<
259  // (float)(length/(I.getHeight() * I.getWidth())) << std::endl;
260 
261  // memcpy(I.bitmap,rawdata,length);
262 }
263 
273 {
274  // get image data
275  unsigned long length;
276  unsigned char *rawdata = NULL;
277  int dropped;
278  unsigned int size;
279 
280  open(I);
281 
282  camera->AcquireImageEx(TRUE, &dropped);
283  rawdata = camera->GetRawData(&length);
284  size = I.getWidth() * I.getHeight();
285 
286  switch (_color) {
288  vpImageConvert::GreyToRGBa(rawdata, (unsigned char *)I.bitmap, size);
289  break;
290 
292  vpImageConvert::MONO16ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
293  break;
294 
296  vpImageConvert::YUV411ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
297  break;
298 
300  vpImageConvert::YUV422ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
301  break;
302 
304  vpImageConvert::YUV444ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
305  break;
306 
308  size = length / 3;
309  vpImageConvert::RGBToRGBa(rawdata, (unsigned char *)I.bitmap, size);
310  break;
311 
312  default:
313  close();
314  vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
315  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Format conversion not implemented. "
316  "Acquisition failed."));
317  break;
318  };
319 }
320 
325 {
326  // stop acquisition
327  if (camera->IsAcquiring()) {
328  // stop acquisition
329  if (camera->StopImageAcquisition() != CAM_SUCCESS) {
330  close();
331  vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition "
332  "from IEEE 1394 camera number %i",
333  index);
334  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while stopping image acquisition"));
335  }
336  }
337 
338  init = false;
339 }
340 
345 void vp1394CMUGrabber::setControl(unsigned short gain, unsigned short shutter)
346 {
347  setShutter(shutter);
348  setGain(gain);
349 }
350 
355 {
356  int n_cam = camera->RefreshCameraList();
357 
358  return n_cam;
359 }
360 
366 void vp1394CMUGrabber::getGainMinMax(unsigned short &min, unsigned short &max)
367 {
368  initCamera();
369 
370  C1394CameraControl *Control;
371  Control = camera->GetCameraControl(FEATURE_GAIN);
372  Control->Inquire();
373  Control->GetRange(&min, &max);
374 }
381 {
382  initCamera();
383  camera->GetCameraControl(FEATURE_GAIN)->SetAutoMode(true);
384 }
390 void vp1394CMUGrabber::setGain(unsigned short gain)
391 {
392  initCamera();
393  _gain = gain;
394 
395  unsigned short min, max;
396  C1394CameraControl *Control;
397 
398  Control = camera->GetCameraControl(FEATURE_GAIN);
399  Control->Inquire();
400  Control->GetRange(&min, &max);
401 
402  if (_gain < min) {
403  _gain = min;
404  std::cout << "vp1394CMUGrabber warning: Desired gain register value of "
405  "IEEE 1394 camera number "
406  << index << " can't be less than " << _gain << std::endl;
407  } else if (_gain > max) {
408  _gain = max;
409  std::cout << "vp1394CMUGrabber warning: Desired gain register value of "
410  "IEEE 1394 camera number "
411  << index << " can't be greater than " << _gain << std::endl;
412  }
413 
414  Control->SetAutoMode(false);
415  if (Control->SetValue(_gain) != CAM_SUCCESS) {
416  std::cout << "vp1394CMUGrabber warning: Can't set gain register value of "
417  "IEEE 1394 camera number "
418  << index << std::endl;
419  }
420 }
421 
427 void vp1394CMUGrabber::getShutterMinMax(unsigned short &min, unsigned short &max)
428 {
429  initCamera();
430 
431  C1394CameraControl *Control;
432  Control = camera->GetCameraControl(FEATURE_SHUTTER);
433  Control->Inquire();
434  Control->GetRange(&min, &max);
435 }
436 
443 {
444  initCamera();
445  camera->GetCameraControl(FEATURE_SHUTTER)->SetAutoMode(true);
446 }
452 void vp1394CMUGrabber::setShutter(unsigned short shutter)
453 {
454  initCamera();
455 
456  _shutter = shutter;
457 
458  unsigned short min, max;
459  C1394CameraControl *Control;
460 
461  Control = camera->GetCameraControl(FEATURE_SHUTTER);
462  Control->Inquire();
463  Control->GetRange(&min, &max);
464 
465  if (_shutter < min) {
466  _shutter = min;
467  std::cout << "vp1394CMUGrabber warning: Desired exposure time register "
468  "value of IEEE 1394 camera number "
469  << index << " can't be less than " << _shutter << std::endl;
470  } else if (_shutter > max) {
471  _shutter = max;
472  std::cout << "vp1394CMUGrabber warning: Desired exposure time register "
473  "value of IEEE 1394 camera number "
474  << index << " can't be greater than " << _shutter << std::endl;
475  }
476  Control->SetAutoMode(false);
477  if (Control->SetValue(_shutter) != CAM_SUCCESS) {
478  std::cout << "vp1394CMUGrabber warning: Can't set exposure time register "
479  "value of IEEE 1394 camera number "
480  << index << std::endl;
481  }
482 }
483 
488 {
489  if (camera->GetNumberCameras() > cam_id) {
490  char buf[512];
491  camera->GetNodeDescription(cam_id, buf, 512);
492  std::cout << "Camera " << cam_id << ": " << buf << std::endl;
493 
494  } else {
495  std::cout << "Camera " << cam_id << ": camera not found" << std::endl;
496  }
497 }
498 
503 {
504  char vendor[256], model[256], buf[256];
505  LARGE_INTEGER ID;
506 
507  camera->GetCameraName(model, sizeof(model));
508  camera->GetCameraVendor(vendor, sizeof(vendor));
509  camera->GetCameraUniqueID(&ID);
510 
511  std::cout << "Vendor: " << vendor << std::endl;
512  std::cout << "Model: " << model << std::endl;
513 
514  sprintf(buf, "%08X%08X", ID.HighPart, ID.LowPart);
515  std::cout << "UniqueID: " << buf << std::endl;
516 }
517 
557 void vp1394CMUGrabber::setVideoMode(unsigned long format, unsigned long mode)
558 {
559  initCamera();
560 
561  _format = format;
562  _mode = mode;
563 
564  // Set format and mode
565  if ((_format != -1) && (_mode != -1)) {
566  if (!camera->HasVideoMode(_format, _mode)) {
567  close();
568  vpERROR_TRACE("vp1394CMUGrabber error: The image format is not "
569  "supported by the IEEE 1394 camera number %i",
570  index);
571  throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Video mode not supported"));
572  }
573 
574  if (camera->IsAcquiring()) {
575  // stop acquisition
576  if (camera->StopImageAcquisition() != CAM_SUCCESS) {
577  close();
578  vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition "
579  "from IEEE 1394 camera number %i",
580  index);
581  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while stopping image acquisition"));
582  }
583  }
584 
585  if (camera->SetVideoFormat(_format) != CAM_SUCCESS) {
586  close();
587  vpERROR_TRACE("vp1394CMUGrabber error: Can't set video format of IEEE "
588  "1394 camera number %i",
589  index);
590  throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't set video format"));
591  }
592 
593  if (camera->SetVideoMode(_mode) != CAM_SUCCESS) {
594  close();
595  vpERROR_TRACE("vp1394CMUGrabber error: Can't set video mode of IEEE "
596  "1394 camera number %i",
597  index);
598  throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't set video mode"));
599  }
600 
601  // start acquisition
602  if (camera->StartImageAcquisition() != CAM_SUCCESS) {
603  close();
604  vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition "
605  "from IEEE 1394 camera number %i",
606  index);
607  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while starting image acquisition"));
608  }
609 
610  // Update Image dimension
611  unsigned long w, h;
612  camera->GetVideoFrameDimensions(&w, &h);
613  this->width = w;
614  this->height = h;
615 
616  // Update the color coding
617  _color = getVideoColorCoding();
618  }
619 }
620 
642 void vp1394CMUGrabber::setFramerate(unsigned long fps)
643 {
644  initCamera();
645 
646  _fps = fps;
647 
648  // Set fps
649  if (_fps != -1) {
650  if (!camera->HasVideoFrameRate(_format, _mode, _fps)) {
651  close();
652  vpERROR_TRACE("vp1394CMUGrabber error: The frame rate is not supported "
653  "by the IEEE 1394 camera number %i for the selected "
654  "image format",
655  index);
656  throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "The frame rate is not supported"));
657  }
658 
659  if (camera->IsAcquiring()) {
660  // stop acquisition
661  if (camera->StopImageAcquisition() != CAM_SUCCESS) {
662  close();
663  vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition "
664  "from IEEE 1394 camera number %i",
665  index);
666  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while stopping image acquisition"));
667  }
668  }
669  if (camera->SetVideoFrameRate(_fps) != CAM_SUCCESS) {
670  close();
671  vpERROR_TRACE("vp1394CMUGrabber error: Can't set video frame rate of "
672  "IEEE 1394 camera number %i",
673  index);
674  throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't set video frame rate"));
675  }
676  // start acquisition
677  if (camera->StartImageAcquisition() != CAM_SUCCESS) {
678  close();
679  vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition "
680  "from IEEE 1394 camera number %i",
681  index);
682  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while starting image acquisition"));
683  }
684  }
685 }
708 {
709  initCamera();
710  int fps = camera->GetVideoFrameRate();
711  return fps;
712 }
713 
731 {
732  this->acquire(I);
733  return *this;
734 }
735 
753 {
754  this->acquire(I);
755  return *this;
756 }
757 
758 #elif !defined(VISP_BUILD_SHARED_LIBS)
759 // Work around to avoid warning: libvisp_sensor.a(vp1394CMUGrabber.cpp.o) has
760 // no symbols
761 void dummy_vp1394CMUGrabber(){};
762 #endif
Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
void setControl(unsigned short gain, unsigned short shutter)
virtual ~vp1394CMUGrabber()
void displayCameraDescription(int cam_id)
vpColorCodingType getVideoColorCoding() const
Get the video color coding format.
void getGainMinMax(unsigned short &min, unsigned short &max)
void setVideoMode(unsigned long format, unsigned long mode)
vp1394CMUGrabber & operator>>(vpImage< unsigned char > &I)
void acquire(vpImage< unsigned char > &I)
void setFramerate(unsigned long fps)
void getShutterMinMax(unsigned short &min, unsigned short &max)
int getNumberOfConnectedCameras() const
void setGain(unsigned short gain)
void open(vpImage< unsigned char > &I)
void selectCamera(int cam_id)
void setShutter(unsigned short shutter)
Error that can be emited by the vpFrameGrabber class and its derivates.
unsigned int height
Number of rows in the image.
bool init
Set to true if the frame grabber has been initialized.
unsigned int width
Number of columns in the image.
static void YUV411ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size)
static void MONO16ToGrey(unsigned char *grey16, unsigned char *grey, unsigned int size)
static void YUV422ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size)
static void YUV411ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size)
static void GreyToRGBa(unsigned char *grey, unsigned char *rgba, unsigned int width, unsigned int height)
static void MONO16ToRGBa(unsigned char *grey16, unsigned char *rgba, unsigned int size)
static void RGBToGrey(unsigned char *rgb, unsigned char *grey, unsigned int width, unsigned int height, bool flip=false)
static void RGBToRGBa(unsigned char *rgb, unsigned char *rgba, unsigned int size)
static void YUV422ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size)
static void YUV444ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size)
static void YUV444ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size)
unsigned int getWidth() const
Definition: vpImage.h:246
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:799
unsigned int getSize() const
Definition: vpImage.h:227
Type * bitmap
points toward the bitmap
Definition: vpImage.h:143
unsigned int getHeight() const
Definition: vpImage.h:188
#define vpERROR_TRACE
Definition: vpDebug.h:393