Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vp1394CMUGrabber.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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*****************************************************************************/
35
36#include <visp3/core/vpConfig.h>
37
38#ifdef VISP_HAVE_CMU1394
39
40#include <iostream>
41
42#include <visp3/core/vpImageConvert.h>
43#include <visp3/sensor/vp1394CMUGrabber.h>
44
49 : index(0), // If a camera was not selected the first one (index = 0) will
50 // be used
51 _format(-1), _mode(-1), _fps(-1), _modeauto(true), _gain(0), _shutter(0), _color(vp1394CMUGrabber::UNKNOWN)
52{
53 // public members
54 init = false;
55
56 // protected members
57 width = height = -1;
58
59 // private members
60 camera = new C1394Camera;
61}
62
67{
68 close();
69 // delete camera instance
70 if (camera) {
71 delete camera;
72 camera = NULL;
73 }
74}
75
81{
82 int camerror;
83
84 index = cam_id;
85
86 camerror = camera->SelectCamera(index);
87 if (camerror != CAM_SUCCESS) {
88 switch (camerror) {
89 case CAM_ERROR_PARAM_OUT_OF_RANGE:
90 vpERROR_TRACE("vp1394CMUGrabber error: Found no camera number %i", index);
91 throw(
92 vpFrameGrabberException(vpFrameGrabberException::initializationError, "The required camera is not present"));
93 break;
94 case CAM_ERROR_BUSY:
95 vpERROR_TRACE("vp1394CMUGrabber error: The camera %i is busy", index);
97 "The required camera is in use by other application"));
98 break;
99 case CAM_ERROR:
100 vpERROR_TRACE("vp1394CMUGrabber error: General I/O error when "
101 "selecting camera number %i",
102 index);
103 throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Resolve camera can not be used"));
104 break;
105 }
106 close();
107 }
108} // end camera select
109
113void vp1394CMUGrabber::initCamera()
114{
115 if (init == false) {
116 int camerror;
117
118 if (camera->CheckLink() != CAM_SUCCESS) {
119 vpERROR_TRACE("C1394Camera error: Found no cameras on the 1394 bus");
121 }
122
123 camerror = camera->InitCamera();
124 if (camerror != CAM_SUCCESS) {
125 switch (camerror) {
126 case CAM_ERROR_NOT_INITIALIZED:
127 vpERROR_TRACE("vp1394CMUGrabber error: No camera selected", index);
129 break;
130 case CAM_ERROR_BUSY:
131 vpERROR_TRACE("vp1394CMUGrabber error: The camera %i is busy", index);
133 "The required camera is in use by other application"));
134 break;
135 case CAM_ERROR:
136 vpERROR_TRACE("vp1394CMUGrabber error: General I/O error when "
137 "selecting camera number %i",
138 index);
139 throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Resolve camera can not be used"));
140 break;
141 }
142 close();
143 }
144
145 if (camera->Has1394b())
146 camera->Set1394b(TRUE);
147
148 // Get the current settings
149 _format = camera->GetVideoFormat();
150 _mode = camera->GetVideoMode();
151 _color = getVideoColorCoding();
152 // std::cout << "format: " << _format << std::endl;
153 // std::cout << "mode: " << _mode << std::endl;
154 // std::cout << "color coding: " << _color << std::endl;
155
156 // Set trigger off
157 camera->GetCameraControlTrigger()->SetOnOff(false);
158
159 unsigned long w, h;
160 camera->GetVideoFrameDimensions(&w, &h);
161 this->width = w;
162 this->height = h;
163
164 // start acquisition
165 if (camera->StartImageAcquisition() != CAM_SUCCESS) {
166 close();
167 vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition "
168 "from IEEE 1394 camera number %i",
169 index);
170 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while starting image acquisition"));
171 }
172
173 init = true;
174 }
175
176} // end camera init
177
183{
184 initCamera();
185 I.resize(this->height, this->width);
186}
187
193{
194 initCamera();
195 I.resize(this->height, this->width);
196}
197
206{
207 // get image data
208 unsigned long length;
209 unsigned char *rawdata = NULL;
210 int dropped;
211 unsigned int size;
212
213 open(I);
214
215 camera->AcquireImageEx(TRUE, &dropped);
216 rawdata = camera->GetRawData(&length);
217
218 size = I.getSize();
219 switch (_color) {
221 memcpy(I.bitmap, (unsigned char *)rawdata, size);
222 break;
224 vpImageConvert::MONO16ToGrey(rawdata, I.bitmap, size);
225 break;
226
228 vpImageConvert::YUV411ToGrey(rawdata, I.bitmap, size);
229 break;
230
232 vpImageConvert::YUV422ToGrey(rawdata, I.bitmap, size);
233 break;
234
236 vpImageConvert::YUV444ToGrey(rawdata, I.bitmap, size);
237 break;
238
240 vpImageConvert::RGBToGrey(rawdata, I.bitmap, size);
241 break;
242
243 default:
244 close();
245 vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
246 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Format conversion not implemented. "
247 "Acquisition failed."));
248 break;
249 };
250
251 // unsigned short depth = 0;
252 // camera->GetVideoDataDepth(&depth);
253 // std::cout << "depth: " << depth << " computed: " <<
254 // (float)(length/(I.getHeight() * I.getWidth())) << std::endl;
255
256 // memcpy(I.bitmap,rawdata,length);
257}
258
268{
269 // get image data
270 unsigned long length;
271 unsigned char *rawdata = NULL;
272 int dropped;
273 unsigned int size;
274
275 open(I);
276
277 camera->AcquireImageEx(TRUE, &dropped);
278 rawdata = camera->GetRawData(&length);
279 size = I.getWidth() * I.getHeight();
280
281 switch (_color) {
283 vpImageConvert::GreyToRGBa(rawdata, (unsigned char *)I.bitmap, size);
284 break;
285
287 vpImageConvert::MONO16ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
288 break;
289
291 vpImageConvert::YUV411ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
292 break;
293
295 vpImageConvert::YUV422ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
296 break;
297
299 vpImageConvert::YUV444ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
300 break;
301
303 size = length / 3;
304 vpImageConvert::RGBToRGBa(rawdata, (unsigned char *)I.bitmap, size);
305 break;
306
307 default:
308 close();
309 vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
310 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Format conversion not implemented. "
311 "Acquisition failed."));
312 break;
313 };
314}
315
320{
321 // stop acquisition
322 if (camera->IsAcquiring()) {
323 // stop acquisition
324 if (camera->StopImageAcquisition() != CAM_SUCCESS) {
325 close();
326 vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition "
327 "from IEEE 1394 camera number %i",
328 index);
329 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while stopping image acquisition"));
330 }
331 }
332
333 init = false;
334}
335
340void vp1394CMUGrabber::setControl(unsigned short gain, unsigned short shutter)
341{
342 setShutter(shutter);
343 setGain(gain);
344}
345
350{
351 int n_cam = camera->RefreshCameraList();
352
353 return n_cam;
354}
355
361void vp1394CMUGrabber::getGainMinMax(unsigned short &min, unsigned short &max)
362{
363 initCamera();
364
365 C1394CameraControl *Control;
366 Control = camera->GetCameraControl(FEATURE_GAIN);
367 Control->Inquire();
368 Control->GetRange(&min, &max);
369}
376{
377 initCamera();
378 camera->GetCameraControl(FEATURE_GAIN)->SetAutoMode(true);
379}
385void vp1394CMUGrabber::setGain(unsigned short gain)
386{
387 initCamera();
388 _gain = gain;
389
390 unsigned short min, max;
391 C1394CameraControl *Control;
392
393 Control = camera->GetCameraControl(FEATURE_GAIN);
394 Control->Inquire();
395 Control->GetRange(&min, &max);
396
397 if (_gain < min) {
398 _gain = min;
399 std::cout << "vp1394CMUGrabber warning: Desired gain register value of "
400 "IEEE 1394 camera number "
401 << index << " can't be less than " << _gain << std::endl;
402 } else if (_gain > max) {
403 _gain = max;
404 std::cout << "vp1394CMUGrabber warning: Desired gain register value of "
405 "IEEE 1394 camera number "
406 << index << " can't be greater than " << _gain << std::endl;
407 }
408
409 Control->SetAutoMode(false);
410 if (Control->SetValue(_gain) != CAM_SUCCESS) {
411 std::cout << "vp1394CMUGrabber warning: Can't set gain register value of "
412 "IEEE 1394 camera number "
413 << index << std::endl;
414 }
415}
416
422void vp1394CMUGrabber::getShutterMinMax(unsigned short &min, unsigned short &max)
423{
424 initCamera();
425
426 C1394CameraControl *Control;
427 Control = camera->GetCameraControl(FEATURE_SHUTTER);
428 Control->Inquire();
429 Control->GetRange(&min, &max);
430}
431
438{
439 initCamera();
440 camera->GetCameraControl(FEATURE_SHUTTER)->SetAutoMode(true);
441}
447void vp1394CMUGrabber::setShutter(unsigned short shutter)
448{
449 initCamera();
450
451 _shutter = shutter;
452
453 unsigned short min, max;
454 C1394CameraControl *Control;
455
456 Control = camera->GetCameraControl(FEATURE_SHUTTER);
457 Control->Inquire();
458 Control->GetRange(&min, &max);
459
460 if (_shutter < min) {
461 _shutter = min;
462 std::cout << "vp1394CMUGrabber warning: Desired exposure time register "
463 "value of IEEE 1394 camera number "
464 << index << " can't be less than " << _shutter << std::endl;
465 } else if (_shutter > max) {
466 _shutter = max;
467 std::cout << "vp1394CMUGrabber warning: Desired exposure time register "
468 "value of IEEE 1394 camera number "
469 << index << " can't be greater than " << _shutter << std::endl;
470 }
471 Control->SetAutoMode(false);
472 if (Control->SetValue(_shutter) != CAM_SUCCESS) {
473 std::cout << "vp1394CMUGrabber warning: Can't set exposure time register "
474 "value of IEEE 1394 camera number "
475 << index << std::endl;
476 }
477}
478
483{
484 if (camera->GetNumberCameras() > cam_id) {
485 char buf[512];
486 camera->GetNodeDescription(cam_id, buf, 512);
487 std::cout << "Camera " << cam_id << ": " << buf << std::endl;
488
489 } else {
490 std::cout << "Camera " << cam_id << ": camera not found" << std::endl;
491 }
492}
493
498{
499 char vendor[256], model[256], buf[256];
500 LARGE_INTEGER ID;
501
502 camera->GetCameraName(model, sizeof(model));
503 camera->GetCameraVendor(vendor, sizeof(vendor));
504 camera->GetCameraUniqueID(&ID);
505
506 std::cout << "Vendor: " << vendor << std::endl;
507 std::cout << "Model: " << model << std::endl;
508
509 snprintf(buf, 256, "%08X%08X", ID.HighPart, ID.LowPart);
510 std::cout << "UniqueID: " << buf << std::endl;
511}
512
552void vp1394CMUGrabber::setVideoMode(unsigned long format, unsigned long mode)
553{
554 initCamera();
555
556 _format = format;
557 _mode = mode;
558
559 // Set format and mode
560 if ((_format != -1) && (_mode != -1)) {
561 if (!camera->HasVideoMode(_format, _mode)) {
562 close();
563 vpERROR_TRACE("vp1394CMUGrabber error: The image format is not "
564 "supported by the IEEE 1394 camera number %i",
565 index);
566 throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Video mode not supported"));
567 }
568
569 if (camera->IsAcquiring()) {
570 // stop acquisition
571 if (camera->StopImageAcquisition() != CAM_SUCCESS) {
572 close();
573 vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition "
574 "from IEEE 1394 camera number %i",
575 index);
576 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while stopping image acquisition"));
577 }
578 }
579
580 if (camera->SetVideoFormat(_format) != CAM_SUCCESS) {
581 close();
582 vpERROR_TRACE("vp1394CMUGrabber error: Can't set video format of IEEE "
583 "1394 camera number %i",
584 index);
585 throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't set video format"));
586 }
587
588 if (camera->SetVideoMode(_mode) != CAM_SUCCESS) {
589 close();
590 vpERROR_TRACE("vp1394CMUGrabber error: Can't set video mode of IEEE "
591 "1394 camera number %i",
592 index);
594 }
595
596 // start acquisition
597 if (camera->StartImageAcquisition() != CAM_SUCCESS) {
598 close();
599 vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition "
600 "from IEEE 1394 camera number %i",
601 index);
602 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while starting image acquisition"));
603 }
604
605 // Update Image dimension
606 unsigned long w, h;
607 camera->GetVideoFrameDimensions(&w, &h);
608 this->width = w;
609 this->height = h;
610
611 // Update the color coding
612 _color = getVideoColorCoding();
613 }
614}
615
637void vp1394CMUGrabber::setFramerate(unsigned long fps)
638{
639 initCamera();
640
641 _fps = fps;
642
643 // Set fps
644 if (_fps != -1) {
645 if (!camera->HasVideoFrameRate(_format, _mode, _fps)) {
646 close();
647 vpERROR_TRACE("vp1394CMUGrabber error: The frame rate is not supported "
648 "by the IEEE 1394 camera number %i for the selected "
649 "image format",
650 index);
651 throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "The frame rate is not supported"));
652 }
653
654 if (camera->IsAcquiring()) {
655 // stop acquisition
656 if (camera->StopImageAcquisition() != CAM_SUCCESS) {
657 close();
658 vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition "
659 "from IEEE 1394 camera number %i",
660 index);
661 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while stopping image acquisition"));
662 }
663 }
664 if (camera->SetVideoFrameRate(_fps) != CAM_SUCCESS) {
665 close();
666 vpERROR_TRACE("vp1394CMUGrabber error: Can't set video frame rate of "
667 "IEEE 1394 camera number %i",
668 index);
669 throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't set video frame rate"));
670 }
671 // start acquisition
672 if (camera->StartImageAcquisition() != CAM_SUCCESS) {
673 close();
674 vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition "
675 "from IEEE 1394 camera number %i",
676 index);
677 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while starting image acquisition"));
678 }
679 }
680}
703{
704 initCamera();
705 int fps = camera->GetVideoFrameRate();
706 return fps;
707}
708
730
748{
749 this->acquire(I);
750 return *this;
751}
752
753#elif !defined(VISP_BUILD_SHARED_LIBS)
754// Work around to avoid warning: libvisp_sensor.a(vp1394CMUGrabber.cpp.o) has
755// no symbols
756void dummy_vp1394CMUGrabber(){};
757#endif
Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
void setControl(unsigned short gain, unsigned short shutter)
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 emitted by the vpFrameGrabber class and its derivates.
@ settingError
Grabber settings error.
@ initializationError
Grabber initialization error.
@ otherError
Grabber returned an other error.
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)
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition vpImage.h:795
unsigned int getSize() const
Definition vpImage.h:223
Type * bitmap
points toward the bitmap
Definition vpImage.h:139
unsigned int getHeight() const
Definition vpImage.h:184
#define vpERROR_TRACE
Definition vpDebug.h:388