/*! Grabs a grayscale image from the selected camera. If the camera color coding differs from vp1394CMUGrabber::MONO8, the acquired image is converted in a gray level image to match the requested format. \param I : Acquired gray level image. */ void vp1394CMUGrabber::acquire(vpImage<unsigned char> &I) { // get image data unsigned long length; unsigned char *rawdata = NULL ; int dropped; unsigned int size; if(init == false){ close(); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done") ); } camera->AcquireImageEx(TRUE,&dropped); rawdata = camera->GetRawData(&length); size = I.getWidth() * I.getHeight(); switch(_color) { case vp1394CMUGrabber::MONO8: memcpy(I.bitmap, (unsigned char *) rawdata, size); break; case vp1394CMUGrabber::MONO16: vpImageConvert::MONO16ToGrey(rawdata, I.bitmap, size); break; case vp1394CMUGrabber::YUV411: vpImageConvert::YUV411ToGrey(rawdata, I.bitmap, size); break; case vp1394CMUGrabber::YUV422: vpImageConvert::YUV422ToGrey(rawdata, I.bitmap, size); break; case vp1394CMUGrabber::YUV444: vpImageConvert::YUV444ToGrey(rawdata, I.bitmap, size); break; case vp1394CMUGrabber::RGB8: vpImageConvert::RGBToGrey(rawdata, I.bitmap, size); break; default: close(); vpERROR_TRACE("Format conversion not implemented. Acquisition failed."); throw (vpFrameGrabberException(vpFrameGrabberException::otherError, "Format conversion not implemented. " "Acquisition failed.") ); break; }; //unsigned short depth = 0; //camera->GetVideoDataDepth(&depth); //std::cout << "depth: " << depth << " computed: " << (float)(length/(I.getHeight() * I.getWidth())) << std::endl; //memcpy(I.bitmap,rawdata,length); }
/*! Select the camera on the bus from its index. The first camera found on the bus has index 0. \param cam_id : Camera index. */ void vp1394CMUGrabber::selectCamera(int cam_id) { int camerror; index = cam_id ; camerror = camera->SelectCamera(index); if ( camerror!= CAM_SUCCESS) { switch (camerror) { case CAM_ERROR_PARAM_OUT_OF_RANGE: vpERROR_TRACE("vp1394CMUGrabber error: Found no camera number %i",index); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The required camera is not present") ); break; case CAM_ERROR_BUSY: vpERROR_TRACE("vp1394CMUGrabber error: The camera %i is busy",index); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The required camera is in use by other application") ); break; case CAM_ERROR: vpERROR_TRACE("vp1394CMUGrabber error: General I/O error when selecting camera number %i",index); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"Resolve camera can not be used") ); break; } close(); } } // end camera select
/*! Grabs a color image from the selected camera. Since the cameras are not able to grab RGBa color coding format, the acquired image is converted in a RGBa to match the requested format. This transformation could be time consuming. \param I : Acquired color image in RGBa format. */ void vp1394CMUGrabber::acquire(vpImage<vpRGBa> &I) { // get image data unsigned long length; unsigned char *rawdata = NULL; int dropped; unsigned int size; if(init == false){ close(); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done") ); } camera->AcquireImageEx(TRUE,&dropped); rawdata = camera->GetRawData(&length); size = I.getWidth() * I.getHeight(); switch (_color) { case vp1394CMUGrabber::MONO8: vpImageConvert::GreyToRGBa(rawdata, (unsigned char *)I.bitmap, size); break; case vp1394CMUGrabber::MONO16: vpImageConvert::MONO16ToRGBa(rawdata, (unsigned char *)I.bitmap, size); break; case vp1394CMUGrabber::YUV411: vpImageConvert::YUV411ToRGBa(rawdata, (unsigned char *)I.bitmap, size); break; case vp1394CMUGrabber::YUV422: vpImageConvert::YUV422ToRGBa(rawdata, (unsigned char *)I.bitmap, size); break; case vp1394CMUGrabber::YUV444: vpImageConvert::YUV444ToRGBa(rawdata, (unsigned char *)I.bitmap, size); break; case vp1394CMUGrabber::RGB8: size = length / 3; vpImageConvert::RGBToRGBa(rawdata, (unsigned char *)I.bitmap, size); break; default: close(); vpERROR_TRACE("Format conversion not implemented. Acquisition failed."); throw (vpFrameGrabberException(vpFrameGrabberException::otherError, "Format conversion not implemented. " "Acquisition failed.") ); break; }; }
void vpROSGrabber::open(){ if(ros::isInitialized() && ros::master::getURI() != _master_uri){ close(); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "ROS grabber already initialised with a different master_URI (" + ros::master::getURI() +" != " + _master_uri + ")") ); } if(!isInitialized){ int argc = 2; char *argv[2]; argv[0] = new char [255]; argv[1] = new char [255]; std::string exe = "ros.exe", arg1 = "__master:="; strcpy(argv[0], exe.c_str()); arg1.append(_master_uri); strcpy(argv[1], arg1.c_str()); open(argc, argv); // Wait for a first image while( !first_img_received) vpTime::wait(40); delete [] argv[0]; delete [] argv[1]; } }
/*! Set camera framerate rate. This method has to be called before open(). \param fps : Value between 0 to 7 used to select a specific camera framerate. See the following table for the correspondances between the input value and the framerate. <TABLE BORDER="1"> <TR><TH> Value </TH> <TH> Frame rate </TH></TR> <TR><TD> 0 </TD> <TD> 1.875 fps </TD></TR> <TR><TD> 1 </TD> <TD> 3.75 fps </TD></TR> <TR><TD> 2 </TD> <TD> 7.5 fps </TD></TR> <TR><TD> 3 </TD> <TD> 15 fps </TD></TR> <TR><TD> 4 </TD> <TD> 30 fps </TD></TR> <TR><TD> 5 </TD> <TD> 60 fps </TD></TR> <TR><TD> 6 </TD> <TD> 120 fps </TD></TR> <TR><TD> 7 </TD> <TD> 240 fps </TD></TR> </TABLE> \sa getFramerate() */ void vp1394CMUGrabber::setFramerate(unsigned long fps) { initCamera(); _fps = fps; // Set fps if (_fps!=-1) { if (!camera->HasVideoFrameRate(_format,_mode,_fps)) { close(); vpERROR_TRACE("vp1394CMUGrabber error: The frame rate is not supported by the IEEE 1394 camera number %i for the selected image format",index); throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"The frame rate is not supported") ); } if (camera->IsAcquiring()) { // stop acquisition if (camera->StopImageAcquisition() != CAM_SUCCESS) { close(); vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition from IEEE 1394 camera number %i",index); throw (vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while stopping image acquisition") ); } } if (camera->SetVideoFrameRate(_fps) != CAM_SUCCESS) { close(); vpERROR_TRACE("vp1394CMUGrabber error: Can't set video frame rate of IEEE 1394 camera number %i",index); throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Can't set video frame rate") ); } // start acquisition if (camera->StartImageAcquisition() != CAM_SUCCESS) { close(); vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition from IEEE 1394 camera number %i",index); throw (vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while starting image acquisition") ); } } }
/*! Sets the captured image height. \warning This function must be called after open() method. \param h : The requested value of the captured image height. \exception vpFrameGrabberException::initializationError If no camera was found. */ void vpOpenCVGrabber::setHeight(const unsigned int h) { if ( cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, h)) { close(); vpERROR_TRACE("Impossible to set the size of the grabber"); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Impossible to set the size of the grabber") ); } this->height = h; }
/*! Sets the captured image width. \warning This function must be called after open() method. \param w : The requested value of the captured image width. \exception vpFrameGrabberException::initializationError If no camera was found. */ void vpOpenCVGrabber::setWidth(const unsigned int w) { if ( cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, w)) { close(); vpERROR_TRACE("Impossible to set the size of the grabber"); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Impossible to set the size of the grabber") ); } this->width = w; }
/*! Grab a color image with timestamp \param I : Acquired color image. \param timestamp : timestamp of the acquired image. \exception vpFrameGrabberException::initializationError If the initialization of the grabber was not done previously. */ void vpROSGrabber::acquire(vpImage<vpRGBa> &I, struct timespec ×tamp) { if (isInitialized==false) { close(); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") ); } while(!mutex_image || !first_img_received); mutex_image = false; timestamp . tv_sec = _sec; timestamp . tv_nsec = _nsec; vpImageConvert::convert(data, I, flip); first_img_received = false; mutex_image = true; }
/*! Grab a color image. \param I : Acquired color image. \exception vpFrameGrabberException::initializationError If the initialization of the grabber was not done previously. */ void vpOpenCVGrabber::acquire(vpImage<vpRGBa> &I) { IplImage *im; if (init==false) { close(); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done") ); } cvGrabFrame(capture); im = cvRetrieveFrame(capture); vpImageConvert::convert(im, I, flip); }
/*! Initialization of the grabber using a color image. \param I : color image. */ void vp1394CMUGrabber::open(vpImage<vpRGBa> &I) { initCamera(); I.init(this->height,this->width); // start acquisition if (camera->StartImageAcquisition() != CAM_SUCCESS) { close(); vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition from IEEE 1394 camera number %i",index); throw (vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while strating image acquisition") ); } }
/*! Grab an image direclty in the OpenCV format. \return Pointer to the image (must not be freed). \exception vpFrameGrabberException::initializationError If the initialization of the grabber was not done previously. */ IplImage* vpOpenCVGrabber::acquire() { IplImage *im; if (init==false) { close(); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done") ); } cvGrabFrame(capture); im = cvRetrieveFrame(capture); return im; }
/*! Stop the acquisition of images and free the camera. */ void vp1394CMUGrabber::close() { // stop acquisition camera->StopImageAcquisition(); if (camera->StopImageAcquisition() != CAM_SUCCESS) { throw (vpFrameGrabberException(vpFrameGrabberException::otherError, "vp1394CMUGrabber error: Can't stop image acquisition from IEEE 1394 camera") ); } init = false; }
/*! Grab an image direclty in the OpenCV format. \param timestamp : timestamp of the acquired image. \return Acquired image. \exception vpFrameGrabberException::initializationError If the initialization of the grabber was not done previously. */ cv::Mat vpROSGrabber::acquire(struct timespec ×tamp) { cv::Mat retour; if (isInitialized==false) { close(); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") ); } while(!mutex_image || !first_img_received); mutex_image = false; timestamp . tv_sec = _sec; timestamp . tv_nsec = _nsec; retour = data.clone(); first_img_received = false; mutex_image = true; return retour; }
bool vpROSGrabber::getCameraInfo(vpCameraParameters &cam){ if (! isInitialized) { close(); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") ); } // Test: if we get an image (first_img_received=true) we should have the camera parameters (first_param_received=true) if they are available if (first_img_received && !first_param_received) return false; while(!mutex_param || !first_param_received); mutex_param = false; cam = _cam; mutex_param = true; return true; }
/*! Generic initialization of the grabber. */ void vpOpenCVGrabber::open() { capture = cvCreateCameraCapture(DeviceType); if (capture != NULL) { init = true; } else { close(); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done : camera already used or no camera found") ); } }
/*! Stop the acquisition of images and free the camera. */ void vp1394CMUGrabber::close() { // stop acquisition if (camera->IsAcquiring()) { // stop acquisition if (camera->StopImageAcquisition() != CAM_SUCCESS) { close(); vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition from IEEE 1394 camera number %i",index); throw (vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while stopping image acquisition") ); } } init = false; }
/*! Grab a gray level image with timestamp without waiting. \param I : Acquired gray level image. \param timestamp : timestamp of the acquired image. \return true if a new image was acquired or false if the image is the same than the previous one. \exception vpFrameGrabberException::initializationError If the initialization of the grabber was not done previously. */ bool vpROSGrabber::acquireNoWait(vpImage<unsigned char> &I, struct timespec ×tamp) { bool new_image = false; if (isInitialized==false) { close(); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError, "Grabber not initialized.") ); } while(!mutex_image); mutex_image = false; timestamp . tv_sec = _sec; timestamp . tv_nsec = _nsec; vpImageConvert::convert(data, I, flip); new_image = first_img_received; first_img_received = false; mutex_image = true; return new_image; }
/*! Set the camera format and video mode. This method has to be called before open(). \param format : Camera video format. \param mode : Camera video mode. See the following table for the correspondances between the input format and mode and the resulting video color coding. <TABLE BORDER="1"> <TR><TH> Format </TH><TH> Mode </TH><TH> (H) x (W) </TH><TH> Color </TH></TR> <TR><TD> 0 </TD><TD> 0 </TD><TD> 160 x 120 </TD><TD> YUV444 </TD></TR> <TR><TD> 0 </TD><TD> 1 </TD><TD> 320 x 240 </TD><TD> YUV422 </TD></TR> <TR><TD> 0 </TD><TD> 2 </TD><TD> 640 x 480 </TD><TD> YUV411 </TD></TR> <TR><TD> 0 </TD><TD> 3 </TD><TD> 640 x 480 </TD><TD> YUV422 </TD></TR> <TR><TD> 0 </TD><TD> 4 </TD><TD> 640 x 480 </TD><TD> RGB8 </TD></TR> <TR><TD> 0 </TD><TD> 5 </TD><TD> 640 x 480 </TD><TD> MONO8 </TD></TR> <TR><TD> 0 </TD><TD> 6 </TD><TD> 640 x 480 </TD><TD> MONO16 </TD></TR> <TR><TD> 1 </TD><TD> 0 </TD><TD> 800 x 600 </TD><TD> YUV422 </TD></TR> <TR><TD> 1 </TD><TD> 1 </TD><TD> 800 x 600 </TD><TD> RGB8 </TD></TR> <TR><TD> 1 </TD><TD> 2 </TD><TD> 800 x 600 </TD><TD> MONO8 </TD></TR> <TR><TD> 1 </TD><TD> 3 </TD><TD>1024 x 768 </TD><TD> YUV422 </TD></TR> <TR><TD> 1 </TD><TD> 4 </TD><TD>1024 x 768 </TD><TD> RGB8 </TD></TR> <TR><TD> 1 </TD><TD> 5 </TD><TD>1024 x 768 </TD><TD> MONO8 </TD></TR> <TR><TD> 1 </TD><TD> 6 </TD><TD> 800 x 600 </TD><TD> MONO16 </TD></TR> <TR><TD> 1 </TD><TD> 7 </TD><TD>1024 x 768 </TD><TD> MONO16 </TD></TR> <TR><TD> 2 </TD><TD> 0 </TD><TD>1280 x 960 </TD><TD> YUV422 </TD></TR> <TR><TD> 2 </TD><TD> 1 </TD><TD>1280 x 960 </TD><TD> RGB8 </TD></TR> <TR><TD> 2 </TD><TD> 2 </TD><TD>1280 x 960 </TD><TD> MONO8 </TD></TR> <TR><TD> 2 </TD><TD> 3 </TD><TD>1600 x 1200</TD><TD> YUV422 </TD></TR> <TR><TD> 2 </TD><TD> 4 </TD><TD>1600 x 1200</TD><TD> RGB8 </TD></TR> <TR><TD> 2 </TD><TD> 5 </TD><TD>1600 x 1200</TD><TD> MONO8 </TD></TR> <TR><TD> 2 </TD><TD> 6 </TD><TD>1280 x 960 </TD><TD> MONO16 </TD></TR> <TR><TD> 2 </TD><TD> 7 </TD><TD>1600 x 1200</TD><TD> MONO16 </TD></TR> </TABLE> */ void vp1394CMUGrabber::setVideoMode( unsigned long format, unsigned long mode ) { initCamera(); _format = format ; _mode = mode ; // Set format and mode if ((_format != -1) && (_mode != -1)) { if (!camera->HasVideoMode(_format, _mode)) { close(); vpERROR_TRACE("vp1394CMUGrabber error: The image format is not supported by the IEEE 1394 camera number %i",index); throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Video mode not supported") ); } if (camera->IsAcquiring()) { // stop acquisition if (camera->StopImageAcquisition() != CAM_SUCCESS) { close(); vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition from IEEE 1394 camera number %i",index); throw (vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while stopping image acquisition") ); } } if (camera->SetVideoFormat(_format) != CAM_SUCCESS) { close(); vpERROR_TRACE("vp1394CMUGrabber error: Can't set video format of IEEE 1394 camera number %i",index); throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Can't set video format") ); } if (camera->SetVideoMode(_mode) != CAM_SUCCESS) { close(); vpERROR_TRACE("vp1394CMUGrabber error: Can't set video mode of IEEE 1394 camera number %i",index); throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Can't set video mode") ); } // start acquisition if (camera->StartImageAcquisition() != CAM_SUCCESS) { close(); vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition from IEEE 1394 camera number %i",index); throw (vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while starting image acquisition") ); } // Update Image dimension unsigned long w, h; camera->GetVideoFrameDimensions(&w, &h); this->width = w; this->height = h; // Update the color coding _color = getVideoColorCoding(); } }
/*! Init the selected camera. */ void vp1394CMUGrabber::initCamera() { if (init == false) { int camerror; if (camera->CheckLink() != CAM_SUCCESS) { vpERROR_TRACE("C1394Camera error: Found no cameras on the 1394 bus"); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The is no detected camera") ); } camerror = camera->InitCamera(); if ( camerror != CAM_SUCCESS ) { switch (camerror) { case CAM_ERROR_NOT_INITIALIZED: vpERROR_TRACE("vp1394CMUGrabber error: No camera selected",index); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The is no selected camera") ); break; case CAM_ERROR_BUSY: vpERROR_TRACE("vp1394CMUGrabber error: The camera %i is busy",index); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The required camera is in use by other application") ); break; case CAM_ERROR: vpERROR_TRACE("vp1394CMUGrabber error: General I/O error when selecting camera number %i",index); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"Resolve camera can not be used") ); break; } close(); } if (camera->Has1394b()) camera->Set1394b(TRUE); // Get the current settings _format = camera->GetVideoFormat(); _mode = camera->GetVideoMode(); _color = getVideoColorCoding(); //std::cout << "format: " << _format << std::endl; //std::cout << "mode: " << _mode << std::endl; //std::cout << "color coding: " << _color << std::endl; // Set trigger off camera->GetCameraControlTrigger()->SetOnOff(false); unsigned long w, h; camera->GetVideoFrameDimensions(&w, &h); this->width = w; this->height = h; // start acquisition if (camera->StartImageAcquisition() != CAM_SUCCESS) { close(); vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition from IEEE 1394 camera number %i",index); throw (vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while starting image acquisition") ); } init = true; } } // end camera init
/*! Init the selected camera. */ void vp1394CMUGrabber::initCamera() { int camerror; unsigned long width, height; if (camera->CheckLink() != CAM_SUCCESS) { vpERROR_TRACE("C1394Camera error: Found no cameras on the 1394 bus"); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The is no detected camera") ); } camerror = camera->InitCamera(); if ( camerror != CAM_SUCCESS ) { switch (camerror) { case CAM_ERROR_NOT_INITIALIZED: vpERROR_TRACE("vp1394CMUGrabber error: No camera selected",index); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The is no selected camera") ); break; case CAM_ERROR_BUSY: vpERROR_TRACE("vp1394CMUGrabber error: The camera %i is busy",index); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The required camera is in use by other application") ); break; case CAM_ERROR: vpERROR_TRACE("vp1394CMUGrabber error: General I/O error when selecting camera number %i",index); throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"Resolve camera can not be used") ); break; } close(); } if (camera->Has1394b()) camera->Set1394b(TRUE); // Set format and mode if ((_format != -1) && (_mode != -1)) { if (!camera->HasVideoMode(_format, _mode)) { close(); vpERROR_TRACE("vp1394CMUGrabber error: The image format is not supported by the IEEE 1394 camera number %i",index); throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Video mode not supported") ); } if (camera->SetVideoFormat(_format) != CAM_SUCCESS) { close(); vpERROR_TRACE("vp1394CMUGrabber error: Can't set video format of IEEE 1394 camera number %i",index); throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Can't set video format") ); } if (camera->SetVideoMode(_mode) != CAM_SUCCESS) { close(); vpERROR_TRACE("vp1394CMUGrabber error: Can't set video mode of IEEE 1394 camera number %i",index); throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Can't set video mode") ); } } else { // Get the current format and mode _format = camera->GetVideoFormat(); _mode = camera->GetVideoMode(); } // Update the color coding _color = getVideoColorCoding(); //std::cout << "color coding: " << _color << std::endl; // Set fps if (_fps!=-1) { if (!camera->HasVideoFrameRate(_format,_mode,_fps)) { close(); vpERROR_TRACE("vp1394CMUGrabber error: The frame rate is not supported by the IEEE 1394 camera number %i for the selected image format",index); throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"The frame rate is not supported") ); } if (camera->SetVideoFrameRate(_fps) != CAM_SUCCESS) { close(); vpERROR_TRACE("vp1394CMUGrabber error: Can't set video frame rate of IEEE 1394 camera number %i",index); throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Can't set video frame rate") ); } } // Set shutter and gain if ( _modeauto == false ) { unsigned short min,max; C1394CameraControl *Control; Control = camera->GetCameraControl(FEATURE_GAIN); Control->Inquire(); Control->GetRange(&min,&max); if (_gain<min) { _gain = min; std::cout << "vp1394CMUGrabber warning: Desired gain register value of IEEE 1394 camera number " << index << " can't be less than " << _gain << std::endl; } else if (_gain>max) { _gain = max; std::cout << "vp1394CMUGrabber warning: Desired gain register value of IEEE 1394 camera number " << index << " can't be greater than " << _gain << std::endl; } Control->SetAutoMode(false); if(Control->SetValue(_gain) != CAM_SUCCESS) { std::cout << "vp1394CMUGrabber warning: Can't set gain register value of IEEE 1394 camera number " << index << std::endl; } Control = camera->GetCameraControl(FEATURE_SHUTTER); Control->Inquire(); Control->GetRange(&min,&max); if (_shutter<min) { _shutter = min; std::cout << "vp1394CMUGrabber warning: Desired exposure time register value of IEEE 1394 camera number " << index << " can't be less than " << _shutter << std::endl; } else if (_shutter>max) { _shutter = max; std::cout << "vp1394CMUGrabber warning: Desired exposure time register value of IEEE 1394 camera number " << index << " can't be greater than " << _shutter << std::endl; } Control->SetAutoMode(false); if(Control->SetValue(_shutter) != CAM_SUCCESS) { std::cout << "vp1394CMUGrabber warning: Can't set exposure time register value of IEEE 1394 camera number " << index << std::endl; } } else { camera->GetCameraControl(FEATURE_SHUTTER)->SetAutoMode(true); camera->GetCameraControl(FEATURE_GAIN)->SetAutoMode(true); } // Set trigger off camera->GetCameraControlTrigger()->SetOnOff(false); camera->GetVideoFrameDimensions(&width,&height); this->height = height; this->width = width; init = true; } // end camera init