00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00032 #include "pointgrey_camera_driver/PointGreyCamera.h"
00033
00034 #include <iostream>
00035 #include <sstream>
00036 #include <flycapture/FlyCapture2Defs.h>
00037
00038 using namespace FlyCapture2;
00039
00040 PointGreyCamera::PointGreyCamera():
00041 busMgr_(), cam_()
00042 {
00043 serial_ = 0;
00044 captureRunning_ = false;
00045 }
00046
00047 PointGreyCamera::~PointGreyCamera()
00048 {
00049 }
00050
00051 bool PointGreyCamera::setNewConfiguration(pointgrey_camera_driver::PointGreyConfig &config, const uint32_t &level)
00052 {
00053 if(!cam_.IsConnected())
00054 {
00055 PointGreyCamera::connect();
00056 }
00057
00058
00059 boost::mutex::scoped_lock scopedLock(mutex_);
00060
00061
00062 bool retVal = true;
00063
00064
00065 VideoMode vMode;
00066 Mode fmt7Mode;
00067 retVal &= PointGreyCamera::getVideoModeFromString(config.video_mode, vMode, fmt7Mode);
00068
00069
00070
00071 if(level != PointGreyCamera::LEVEL_RECONFIGURE_RUNNING)
00072 {
00073 bool wasRunning = PointGreyCamera::stop();
00074 if(vMode == VIDEOMODE_FORMAT7)
00075 {
00076 PixelFormat fmt7PixFmt;
00077 PointGreyCamera::getFormat7PixelFormatFromString(config.format7_color_coding, fmt7PixFmt);
00078
00079 uint16_t uwidth = (uint16_t)config.format7_roi_width;
00080 uint16_t uheight = (uint16_t)config.format7_roi_height;
00081 uint16_t uoffsetx = (uint16_t)config.format7_x_offset;
00082 uint16_t uoffsety = (uint16_t)config.format7_y_offset;
00083 retVal &= PointGreyCamera::setFormat7(fmt7Mode, fmt7PixFmt, uwidth, uheight, uoffsetx, uoffsety);
00084 config.format7_roi_width = uwidth;
00085 config.format7_roi_height = uheight;
00086 config.format7_x_offset = uoffsetx;
00087 config.format7_y_offset = uoffsety;
00088 }
00089 else
00090 {
00091
00092 PointGreyCamera::setVideoMode(vMode);
00093 }
00094
00095 if(wasRunning)
00096 {
00097 PointGreyCamera::start();
00098 }
00099 }
00100
00101
00102 retVal &= PointGreyCamera::setProperty(FRAME_RATE, false, config.frame_rate);
00103
00104
00105 retVal &= PointGreyCamera::setProperty(AUTO_EXPOSURE, config.auto_exposure, config.exposure);
00106
00107
00108 retVal &= PointGreyCamera::setProperty(SHARPNESS, config.auto_sharpness, config.sharpness);
00109
00110
00111 retVal &= PointGreyCamera::setProperty(SATURATION, config.auto_saturation, config.saturation);
00112
00113
00114 double shutter = 1000.0 * config.shutter_speed;
00115 retVal &= PointGreyCamera::setProperty(SHUTTER, config.auto_shutter, shutter);
00116 config.shutter_speed = shutter / 1000.0;
00117
00118
00119 retVal &= PointGreyCamera::setProperty(GAIN, config.auto_gain, config.gain);
00120
00121
00122 unsigned int pan = config.pan;
00123 unsigned int not_used = 0;
00124 retVal &= PointGreyCamera::setProperty(PAN, false, pan, not_used);
00125 config.pan = pan;
00126
00127
00128 unsigned int tilt = config.tilt;
00129 retVal &= PointGreyCamera::setProperty(TILT, false, tilt, not_used);
00130 config.tilt = tilt;
00131
00132
00133 retVal &= PointGreyCamera::setProperty(BRIGHTNESS, false, config.brightness);
00134
00135
00136 retVal &= PointGreyCamera::setProperty(GAMMA, false, config.gamma);
00137
00138
00139 uint16_t blue = config.white_balance_blue;
00140 uint16_t red = config.white_balance_red;
00141 retVal &= PointGreyCamera::setWhiteBalance(config.auto_white_balance, blue, red);
00142 config.white_balance_blue = blue;
00143 config.white_balance_red = red;
00144
00145
00146 switch (config.trigger_polarity)
00147 {
00148 case pointgrey_camera_driver::PointGrey_Low:
00149 case pointgrey_camera_driver::PointGrey_High:
00150 {
00151 bool temp = config.trigger_polarity;
00152 retVal &= PointGreyCamera::setExternalTrigger(config.enable_trigger, config.trigger_mode, config.trigger_source, config.trigger_parameter, config.trigger_delay, temp);
00153 config.strobe1_polarity = temp;
00154 }
00155 break;
00156 default:
00157 retVal &= false;
00158 }
00159
00160
00161 switch (config.strobe1_polarity)
00162 {
00163 case pointgrey_camera_driver::PointGrey_Low:
00164 case pointgrey_camera_driver::PointGrey_High:
00165 {
00166 bool temp = config.strobe1_polarity;
00167 retVal &= PointGreyCamera::setExternalStrobe(config.enable_strobe1, pointgrey_camera_driver::PointGrey_GPIO1, config.strobe1_duration, config.strobe1_delay, temp);
00168 config.strobe1_polarity = temp;
00169 }
00170 break;
00171 default:
00172 retVal &= false;
00173 }
00174
00175
00176 switch (config.strobe2_polarity)
00177 {
00178 case pointgrey_camera_driver::PointGrey_Low:
00179 case pointgrey_camera_driver::PointGrey_High:
00180 {
00181 bool temp = config.strobe2_polarity;
00182 retVal &= PointGreyCamera::setExternalStrobe(config.enable_strobe2, pointgrey_camera_driver::PointGrey_GPIO2, config.strobe2_duration, config.strobe2_delay, temp);
00183 config.strobe2_polarity = temp;
00184 }
00185 break;
00186 default:
00187 retVal &= false;
00188 }
00189
00190
00191 return retVal;
00192 }
00193
00194 void PointGreyCamera::setGain(double &gain)
00195 {
00196 PointGreyCamera::setProperty(GAIN, false, gain);
00197 }
00198
00199 void PointGreyCamera::setBRWhiteBalance(bool auto_white_balance, uint16_t &blue, uint16_t &red)
00200 {
00201 PointGreyCamera::setWhiteBalance(auto_white_balance, blue, red);
00202 }
00203
00204 void PointGreyCamera::setVideoMode(FlyCapture2::VideoMode &videoMode)
00205 {
00206
00207 FrameRate frameRate = FRAMERATE_7_5;
00208 if(videoMode == VIDEOMODE_640x480Y8)
00209 {
00210 frameRate = FRAMERATE_30;
00211 }
00212 else if(videoMode == VIDEOMODE_1280x960Y8)
00213 {
00214 frameRate = FRAMERATE_15;
00215 }
00216 else if(videoMode == VIDEOMODE_1280x960Y16)
00217 {
00218 frameRate = FRAMERATE_7_5;
00219 }
00220 else if(videoMode == VIDEOMODE_FORMAT7)
00221 {
00222 frameRate = FRAMERATE_FORMAT7;
00223 }
00224 Error error = cam_.SetVideoModeAndFrameRate(videoMode, frameRate);
00225 PointGreyCamera::handleError("PointGreyCamera::setVideoMode Could not set video mode", error);
00226 }
00227
00228 bool PointGreyCamera::setFormat7(FlyCapture2::Mode &fmt7Mode, FlyCapture2::PixelFormat &fmt7PixFmt, uint16_t &roi_width, uint16_t &roi_height, uint16_t &roi_offset_x, uint16_t &roi_offset_y)
00229 {
00230
00231 bool retVal = true;
00232
00233 Error error;
00234
00235
00236 Format7Info fmt7Info;
00237 bool supported;
00238 fmt7Info.mode = fmt7Mode;
00239 error = cam_.GetFormat7Info(&fmt7Info, &supported);
00240 PointGreyCamera::handleError("PointGreyCamera::setFormat7 Could not get Format 7 information", error);
00241 if(!supported)
00242 {
00243 throw std::runtime_error("PointGreyCamera::setFormat7 Format 7 mode not supported on this camera.");
00244 }
00245
00246
00247 Format7ImageSettings fmt7ImageSettings;
00248 fmt7ImageSettings.mode = fmt7Mode;
00249 fmt7ImageSettings.pixelFormat = fmt7PixFmt;
00250
00251
00252 roi_width = roi_width / fmt7Info.imageHStepSize * fmt7Info.imageHStepSize;
00253 if(roi_width == 0)
00254 {
00255 fmt7ImageSettings.width = fmt7Info.maxWidth;
00256 }
00257 else if(roi_width > fmt7Info.maxWidth)
00258 {
00259 roi_width = fmt7Info.maxWidth;
00260 fmt7ImageSettings.width = fmt7Info.maxWidth;
00261 retVal &= false;
00262 }
00263 else
00264 {
00265 fmt7ImageSettings.width = roi_width;
00266 }
00267
00268
00269 roi_height = roi_height / fmt7Info.imageVStepSize * fmt7Info.imageVStepSize;
00270 if(roi_height == 0)
00271 {
00272 fmt7ImageSettings.height = fmt7Info.maxHeight;
00273 }
00274 else if(roi_height > fmt7Info.maxHeight)
00275 {
00276 roi_height = fmt7Info.maxHeight;
00277 fmt7ImageSettings.height = fmt7Info.maxHeight;
00278 retVal &= false;
00279 }
00280 else
00281 {
00282 fmt7ImageSettings.height = roi_height;
00283 }
00284
00285
00286 roi_offset_x = roi_offset_x / fmt7Info.offsetHStepSize * fmt7Info.offsetHStepSize;
00287 if(roi_offset_x > (fmt7Info.maxWidth - fmt7ImageSettings.width))
00288 {
00289 roi_offset_x = fmt7Info.maxWidth - fmt7ImageSettings.width;
00290 retVal &= false;
00291 }
00292 fmt7ImageSettings.offsetX = roi_offset_x;
00293
00294
00295 roi_offset_y = roi_offset_y / fmt7Info.offsetVStepSize * fmt7Info.offsetVStepSize;
00296 if(roi_offset_y > fmt7Info.maxHeight - fmt7ImageSettings.height)
00297 {
00298 roi_offset_y = fmt7Info.maxHeight - fmt7ImageSettings.height;
00299 retVal &= false;
00300 }
00301 fmt7ImageSettings.offsetY = roi_offset_y;
00302
00303
00304 Format7PacketInfo fmt7PacketInfo;
00305 bool valid;
00306 error = cam_.ValidateFormat7Settings(&fmt7ImageSettings, &valid, &fmt7PacketInfo);
00307 PointGreyCamera::handleError("PointGreyCamera::setFormat7 Error validating Format 7 settings", error);
00308 if(!valid)
00309 {
00310 throw std::runtime_error("PointGreyCamera::setFormat7 Format 7 Settings Not Valid.");
00311 }
00312
00313
00314 error = cam_.SetFormat7Configuration(&fmt7ImageSettings, fmt7PacketInfo.recommendedBytesPerPacket);
00315 PointGreyCamera::handleError("PointGreyCamera::setFormat7 Could not send Format7 configuration to the camera", error);
00316
00317
00318 CameraInfo cInfo;
00319 error = cam_.GetCameraInfo(&cInfo);
00320 PointGreyCamera::handleError("PointGreyCamera::setFormat7 Failed to get camera info.", error);
00321 isColor_ = cInfo.isColorCamera;
00322
00323 return retVal;
00324 }
00325
00326 bool PointGreyCamera::getVideoModeFromString(std::string &vmode, FlyCapture2::VideoMode &vmode_out, FlyCapture2::Mode &fmt7Mode)
00327 {
00328
00329 bool retVal = true;
00330
00331
00332 CameraInfo cInfo;
00333 Error error = cam_.GetCameraInfo(&cInfo);
00334 PointGreyCamera::handleError("PointGreyCamera::getVideoModeFromString Failed to get camera info.", error);
00335
00336 if(vmode.compare("640x480_mono8") == 0)
00337 {
00338 vmode_out = VIDEOMODE_640x480Y8;
00339 }
00340 else if(vmode.compare("640x480_mono16") == 0)
00341 {
00342 vmode_out = VIDEOMODE_640x480Y16;
00343 }
00344 else if(vmode.compare("1280x960_mono8") == 0)
00345 {
00346 vmode_out = VIDEOMODE_1280x960Y8;
00347 if(cInfo.isColorCamera)
00348 {
00349 vmode = "1280x960_bayer8";
00350 retVal &= false;
00351 }
00352 }
00353 else if(vmode.compare("1280x960_bayer8") == 0)
00354 {
00355 vmode_out = VIDEOMODE_1280x960Y8;
00356 if(!cInfo.isColorCamera)
00357 {
00358 vmode = "1280x960_mono8";
00359 retVal &= false;
00360 }
00361 }
00362 else if(vmode.compare("1280x960_mono16") == 0)
00363 {
00364 vmode_out = VIDEOMODE_1280x960Y16;
00365 }
00366 else if(vmode.compare("format7_mode0") == 0)
00367 {
00368 fmt7Mode = MODE_0;
00369 vmode_out = VIDEOMODE_FORMAT7;
00370 }
00371 else if(vmode.compare("format7_mode1") == 0)
00372 {
00373 fmt7Mode = MODE_1;
00374 vmode_out = VIDEOMODE_FORMAT7;
00375 }
00376 else if(vmode.compare("format7_mode2") == 0)
00377 {
00378 fmt7Mode = MODE_2;
00379 vmode_out = VIDEOMODE_FORMAT7;
00380 }
00381 else if(vmode.compare("format7_mode3") == 0)
00382 {
00383 fmt7Mode = MODE_3;
00384 vmode_out = VIDEOMODE_FORMAT7;
00385 }
00386 else if(vmode.compare("format7_mode4") == 0)
00387 {
00388 fmt7Mode = MODE_4;
00389 vmode_out = VIDEOMODE_FORMAT7;
00390 }
00391 else if(vmode.compare("format7_mode5") == 0)
00392 {
00393 fmt7Mode = MODE_5;
00394 vmode_out = VIDEOMODE_FORMAT7;
00395 }
00396 else if(vmode.compare("format7_mode7") == 0)
00397 {
00398 fmt7Mode = MODE_7;
00399 vmode_out = VIDEOMODE_FORMAT7;
00400 }
00401 else
00402 {
00403 vmode = "640x480_mono8";
00404 vmode_out = VIDEOMODE_640x480Y8;
00405 retVal &= false;
00406 }
00407
00408 return retVal;
00409 }
00410
00411 bool PointGreyCamera::getFormat7PixelFormatFromString(std::string &sformat, FlyCapture2::PixelFormat &fmt7PixFmt)
00412 {
00413
00414 bool retVal = true;
00415
00416
00417 CameraInfo cInfo;
00418 Error error = cam_.GetCameraInfo(&cInfo);
00419 PointGreyCamera::handleError("PointGreyCamera::getFormat7PixelFormatFromString Failed to get camera info.", error);
00420
00421 if(cInfo.isColorCamera)
00422 {
00423 if(sformat.compare("raw8") == 0)
00424 {
00425 fmt7PixFmt = PIXEL_FORMAT_RAW8;
00426 }
00427 else if(sformat.compare("raw16") == 0)
00428 {
00429 fmt7PixFmt = PIXEL_FORMAT_RAW16;
00430 }
00431 else if(sformat.compare("mono8") == 0)
00432 {
00433 fmt7PixFmt = PIXEL_FORMAT_MONO8;
00434 }
00435 else if(sformat.compare("mono16") == 0)
00436 {
00437 fmt7PixFmt = PIXEL_FORMAT_MONO16;
00438 }
00439 else if(sformat.compare("rgb8") == 0){
00440 fmt7PixFmt = PIXEL_FORMAT_RGB;
00441 }
00442 else
00443 {
00444 sformat = "raw8";
00445 fmt7PixFmt = PIXEL_FORMAT_RAW8;
00446 retVal &= false;
00447 }
00448 }
00449 else
00450 {
00451 if(sformat.compare("mono8") == 0)
00452 {
00453 fmt7PixFmt = PIXEL_FORMAT_MONO8;
00454 }
00455 else if(sformat.compare("mono16") == 0)
00456 {
00457 fmt7PixFmt = PIXEL_FORMAT_MONO16;
00458 }
00459 else
00460 {
00461 sformat = "mono8";
00462 fmt7PixFmt = PIXEL_FORMAT_MONO8;
00463 retVal &= false;
00464 }
00465 }
00466
00467 return retVal;
00468 }
00469
00470 bool PointGreyCamera::setProperty(const FlyCapture2::PropertyType &type, const bool &autoSet, unsigned int &valueA, unsigned int &valueB)
00471 {
00472
00473 bool retVal = true;
00474
00475 PropertyInfo pInfo;
00476 pInfo.type = type;
00477 Error error = cam_.GetPropertyInfo(&pInfo);
00478 PointGreyCamera::handleError("PointGreyCamera::setProperty Could not get property info.", error);
00479
00480 if(pInfo.present)
00481 {
00482 Property prop;
00483 prop.type = type;
00484 prop.autoManualMode = (autoSet && pInfo.autoSupported);
00485 prop.absControl = false;
00486 prop.onOff = pInfo.onOffSupported;
00487
00488 if(valueA < pInfo.min)
00489 {
00490 valueA = pInfo.min;
00491 retVal &= false;
00492 }
00493 else if(valueA > pInfo.max)
00494 {
00495 valueA = pInfo.max;
00496 retVal &= false;
00497 }
00498 if(valueB < pInfo.min)
00499 {
00500 valueB = pInfo.min;
00501 retVal &= false;
00502 }
00503 else if(valueB > pInfo.max)
00504 {
00505 valueB = pInfo.max;
00506 retVal &= false;
00507 }
00508 prop.valueA = valueA;
00509 prop.valueB = valueB;
00510 error = cam_.SetProperty(&prop);
00511 PointGreyCamera::handleError("PointGreyCamera::setProperty Failed to set property ", error);
00513
00514 error = cam_.GetProperty(&prop);
00515 PointGreyCamera::handleError("PointGreyCamera::setProperty Failed to confirm property ", error);
00516 if(!prop.autoManualMode)
00517 {
00518 valueA = prop.valueA;
00519 valueB = prop.valueB;
00520 }
00521 }
00522 else
00523 {
00524 valueA = 0;
00525 valueB = 0;
00526 }
00527
00528 return retVal;
00529 }
00530
00531 bool PointGreyCamera::setProperty(const FlyCapture2::PropertyType &type, const bool &autoSet, double &value)
00532 {
00533
00534 bool retVal = true;
00535
00536 PropertyInfo pInfo;
00537 pInfo.type = type;
00538 Error error = cam_.GetPropertyInfo(&pInfo);
00539 PointGreyCamera::handleError("PointGreyCamera::setProperty Could not get property info.", error);
00540
00541 if(pInfo.present)
00542 {
00543 Property prop;
00544 prop.type = type;
00545 prop.autoManualMode = (autoSet && pInfo.autoSupported);
00546 prop.absControl = pInfo.absValSupported;
00547 prop.onOff = pInfo.onOffSupported;
00548
00549 if(value < pInfo.absMin)
00550 {
00551 value = pInfo.absMin;
00552 retVal &= false;
00553 }
00554 else if(value > pInfo.absMax)
00555 {
00556 value = pInfo.absMax;
00557 retVal &= false;
00558 }
00559 prop.absValue = value;
00560 error = cam_.SetProperty(&prop);
00561 PointGreyCamera::handleError("PointGreyCamera::setProperty Failed to set property ", error);
00563
00564 error = cam_.GetProperty(&prop);
00565 PointGreyCamera::handleError("PointGreyCamera::setProperty Failed to confirm property ", error);
00566 if(!prop.autoManualMode)
00567 {
00568 value = prop.absValue;
00569 }
00570 }
00571 else
00572 {
00573 value = 0.0;
00574 }
00575 return retVal;
00576 }
00577
00578 bool PointGreyCamera::setWhiteBalance(bool &auto_white_balance, uint16_t &blue, uint16_t &red)
00579 {
00580
00581 CameraInfo cInfo;
00582 Error error = cam_.GetCameraInfo(&cInfo);
00583 handleError("PointGreyCamera::setWhiteBalance Failed to get camera info.", error);
00584
00585 if(!cInfo.isColorCamera)
00586 {
00587
00588 auto_white_balance = false;
00589 red = 0;
00590 blue = 0;
00591 return false;
00592 }
00593
00594 unsigned white_balance_addr = 0x80c;
00595 unsigned enable = 1 << 31;
00596 unsigned value = 1 << 25;
00597
00598 if (auto_white_balance) {
00599 PropertyInfo prop_info;
00600 prop_info.type = WHITE_BALANCE;
00601 error = cam_.GetPropertyInfo(&prop_info);
00602 handleError("PointGreyCamera::setWhiteBalance Failed to get property info.", error);
00603 if (!prop_info.autoSupported) {
00604
00605
00606 auto_white_balance = false;
00607 blue = 800;
00608 red = 550;
00609 return false;
00610 }
00611
00612 error = cam_.WriteRegister(white_balance_addr, enable);
00613 handleError("PointGreyCamera::setWhiteBalance Failed to write to register.", error);
00614
00615 value |= 1 << 24;
00616 } else {
00617
00618 value |= 0 << 24;
00619 }
00620
00621 value |= blue << 12 | red;
00622 error = cam_.WriteRegister(white_balance_addr, value);
00623 handleError("PointGreyCamera::setWhiteBalance Failed to write to register.", error);
00624 return true;
00625 }
00626
00627 void PointGreyCamera::setTimeout(const double &timeout)
00628 {
00629 FC2Config pConfig;
00630 Error error = cam_.GetConfiguration(&pConfig);
00631 PointGreyCamera::handleError("PointGreyCamera::setTimeout Could not get camera configuration", error);
00632 pConfig.grabTimeout = (int)(1000.0 * timeout);
00633 if(pConfig.grabTimeout < 0.00001)
00634 {
00635 pConfig.grabTimeout = -1;
00636 }
00637 error = cam_.SetConfiguration(&pConfig);
00638 PointGreyCamera::handleError("PointGreyCamera::setTimeout Could not set camera configuration", error);
00639 }
00640
00641 float PointGreyCamera::getCameraTemperature()
00642 {
00643 Property tProp;
00644 tProp.type = TEMPERATURE;
00645 Error error = cam_.GetProperty(&tProp);
00646 PointGreyCamera::handleError("PointGreyCamera::getCameraTemperature Could not get property.", error);
00647 return tProp.valueA / 10.0f - 273.15f;
00648 }
00649
00650 float PointGreyCamera::getCameraFrameRate()
00651 {
00652 Property fProp;
00653 fProp.type = FRAME_RATE;
00654 Error error = cam_.GetProperty(&fProp);
00655 PointGreyCamera::handleError("PointGreyCamera::getCameraFrameRate Could not get property.", error);
00656 std::cout << "Frame Rate is: " << fProp.absValue << std::endl;
00657 return fProp.absValue;
00658 }
00659
00660 static int sourceNumberFromGpioName(const std::string s)
00661 {
00662 if(s.compare("gpio0") == 0)
00663 {
00664 return 0;
00665 }
00666 else if(s.compare("gpio1") == 0)
00667 {
00668 return 1;
00669 }
00670 else if(s.compare("gpio2") == 0)
00671 {
00672 return 2;
00673 }
00674 else if(s.compare("gpio3") == 0)
00675 {
00676 return 3;
00677 }
00678 else
00679 {
00680
00681 return -1;
00682 }
00683 }
00684
00685 bool PointGreyCamera::setExternalStrobe(bool &enable, const std::string &dest, double &duration, double &delay, bool &polarityHigh)
00686 {
00687
00688 bool retVal = true;
00689
00690
00691 int pin;
00692 pin = sourceNumberFromGpioName(dest);
00693 if (pin < 0)
00694 {
00695
00696 return false;
00697 }
00698
00699 StrobeInfo strobeInfo;
00700 strobeInfo.source = pin;
00701 Error error = cam_.GetStrobeInfo(&strobeInfo);
00702 PointGreyCamera::handleError("PointGreyCamera::setExternalStrobe Could not check external strobe support.", error);
00703 if(strobeInfo.present != true)
00704 {
00705
00706 enable = false;
00707 return false;
00708 }
00709
00710 StrobeControl strobeControl;
00711 strobeControl.source = pin;
00712 error = cam_.GetStrobe(&strobeControl);
00713 PointGreyCamera::handleError("PointGreyCamera::setExternalStrobe Could not get strobe control.", error);
00714 strobeControl.duration = duration;
00715 strobeControl.delay = delay;
00716 strobeControl.onOff = enable;
00717 strobeControl.polarity = polarityHigh;
00718
00719 error = cam_.SetStrobe(&strobeControl);
00720 PointGreyCamera::handleError("PointGreyCamera::setExternalStrobe Could not set strobe control.", error);
00721 error = cam_.GetStrobe(&strobeControl);
00722 PointGreyCamera::handleError("PointGreyCamera::setExternalStrobe Could not get strobe control.", error);
00723 delay = strobeControl.delay;
00724 enable = strobeControl.onOff;
00725 polarityHigh = strobeControl.polarity;
00726
00727 return retVal;
00728 }
00729
00730 bool PointGreyCamera::setExternalTrigger(bool &enable, std::string &mode, std::string &source, int32_t ¶meter, double &delay, bool &polarityHigh)
00731 {
00732
00733 bool retVal = true;
00734
00735 TriggerModeInfo triggerModeInfo;
00736 Error error = cam_.GetTriggerModeInfo(&triggerModeInfo);
00737 PointGreyCamera::handleError("PointGreyCamera::setExternalTrigger Could not check external trigger support.", error);
00738 if(triggerModeInfo.present != true)
00739 {
00740
00741 enable = false;
00742 return false;
00743 }
00744
00745 TriggerMode triggerMode;
00746 error = cam_.GetTriggerMode(&triggerMode);
00747 PointGreyCamera::handleError("PointGreyCamera::setExternalTrigger Could not get trigger mode.", error);
00748 triggerMode.onOff = enable;
00749
00750
00751 std::string tmode = mode;
00752 if(tmode.compare("mode0") == 0)
00753 {
00754 triggerMode.mode = 0;
00755 }
00756 else if(tmode.compare("mode1") == 0)
00757 {
00758 triggerMode.mode = 1;
00759 }
00760 else if(tmode.compare("mode3") == 0)
00761 {
00762 triggerMode.mode = 3;
00763 }
00764 else if(tmode.compare("mode14") == 0)
00765 {
00766 triggerMode.mode = 14;
00767 }
00768 else
00769 {
00770
00771 triggerMode.mode = 0;
00772 mode = "mode0";
00773 retVal &= false;
00774 }
00775
00776
00777 triggerMode.parameter = parameter;
00778
00779
00780 std::string tsource = source;
00781 int pin = sourceNumberFromGpioName(tsource);
00782 if (pin < 0)
00783 {
00784
00785 triggerMode.source = 0;
00786 source = "gpio0";
00787 retVal &= false;
00788 }
00789 else
00790 {
00791 triggerMode.source = pin;
00792 }
00793
00794 triggerMode.polarity = polarityHigh;
00795
00796 error = cam_.SetTriggerMode(&triggerMode);
00797 PointGreyCamera::handleError("PointGreyCamera::setExternalTrigger Could not set trigger mode.", error);
00798 error = cam_.GetTriggerMode(&triggerMode);
00799 PointGreyCamera::handleError("PointGreyCamera::setExternalTrigger Could not get trigger mode.", error);
00800 enable = triggerMode.onOff;
00801 std::stringstream buff;
00802 buff << "mode" << triggerMode.mode;
00803 mode = buff.str();
00804
00807
00808 TriggerDelay triggerDelay;
00809 triggerDelay.type = TRIGGER_DELAY;
00810 triggerDelay.absControl = true;
00811 triggerDelay.absValue = delay;
00812 triggerDelay.onOff = true;
00813 error = cam_.SetTriggerDelay(&triggerDelay);
00814 PointGreyCamera::handleError("PointGreyCamera::setExternalTrigger Could not set trigger delay.", error);
00815 error = cam_.GetTriggerDelay(&triggerDelay);
00816 PointGreyCamera::handleError("PointGreyCamera::setExternalTrigger Could not get trigger delay.", error);
00817 delay = triggerDelay.absValue;
00818
00819 return retVal;
00820 }
00821
00822 void PointGreyCamera::setGigEParameters(bool auto_packet_size, unsigned int packet_size, unsigned int packet_delay)
00823 {
00824 auto_packet_size_ = auto_packet_size;
00825 packet_size_ = packet_size;
00826 packet_delay_ = packet_delay;
00827 }
00828
00829 void PointGreyCamera::setupGigEPacketSize(PGRGuid & guid)
00830 {
00831 GigECamera cam;
00832 Error error;
00833 error = cam.Connect(&guid);
00834 PointGreyCamera::handleError("PointGreyCamera::connect could not connect as GigE camera", error);
00835 unsigned int packet_size;
00836 error = cam.DiscoverGigEPacketSize(&packet_size);
00837 PointGreyCamera::handleError("PointGreyCamera::connect could not discover GigE packet_size", error);
00838 GigEProperty prop;
00839 prop.propType = PACKET_SIZE;
00840 error = cam.GetGigEProperty(&prop);
00841 PointGreyCamera::handleError("PointGreyCamera::connect could not get GigE packet_size", error);
00842 prop.value = packet_size;
00843 error = cam.SetGigEProperty(&prop);
00844 PointGreyCamera::handleError("PointGreyCamera::connect could not set GigE packet_size", error);
00845 }
00846
00847 void PointGreyCamera::setupGigEPacketSize(PGRGuid & guid, unsigned int packet_size)
00848 {
00849 GigECamera cam;
00850 Error error;
00851 error = cam.Connect(&guid);
00852 PointGreyCamera::handleError("PointGreyCamera::connect could not connect as GigE camera", error);
00853 GigEProperty prop;
00854 prop.propType = PACKET_SIZE;
00855 prop.value = packet_size;
00856 error = cam.SetGigEProperty(&prop);
00857 PointGreyCamera::handleError("PointGreyCamera::connect could not set GigE packet_size", error);
00858 }
00859
00860 void PointGreyCamera::setupGigEPacketDelay(PGRGuid & guid, unsigned int packet_delay)
00861 {
00862 GigECamera cam;
00863 Error error;
00864 error = cam.Connect(&guid);
00865 PointGreyCamera::handleError("PointGreyCamera::connect could not connect as GigE camera", error);
00866 GigEProperty prop;
00867 prop.propType = PACKET_DELAY;
00868 prop.value = packet_delay;
00869 error = cam.SetGigEProperty(&prop);
00870 PointGreyCamera::handleError("PointGreyCamera::connect could not set GigE packet_delay", error);
00871 }
00872
00873 void PointGreyCamera::connect()
00874 {
00875 if(!cam_.IsConnected())
00876 {
00877 Error error;
00878 PGRGuid guid;
00879 if(serial_ != 0)
00880 {
00881 error = busMgr_.GetCameraFromSerialNumber(serial_, &guid);
00882 std::stringstream serial_string;
00883 serial_string << serial_;
00884 std::string msg = "PointGreyCamera::connect Could not find camera with serial number: " + serial_string.str() + ". Is that camera plugged in?";
00885 PointGreyCamera::handleError(msg, error);
00886 }
00887 else
00888 {
00889 error = busMgr_.GetCameraFromIndex(0, &guid);
00890 PointGreyCamera::handleError("PointGreyCamera::connect Failed to get first connected camera", error);
00891 }
00892
00893 FlyCapture2::InterfaceType ifType;
00894 error = busMgr_.GetInterfaceTypeFromGuid(&guid, &ifType);
00895 PointGreyCamera::handleError("PointGreyCamera::connect Failed to get interface style of camera", error);
00896 if (ifType == FlyCapture2::INTERFACE_GIGE)
00897 {
00898
00899 if (auto_packet_size_)
00900 setupGigEPacketSize(guid);
00901 else
00902 setupGigEPacketSize(guid, packet_size_);
00903
00904
00905 setupGigEPacketDelay(guid, packet_delay_);
00906
00907
00908 GigECamera cam;
00909 Error error;
00910 error = cam.Connect(&guid);
00911 PointGreyCamera::handleError("PointGreyCamera::connect could not connect as GigE camera", error);
00912 GigEConfig gigeconfig;
00913 error = cam.GetGigEConfig(&gigeconfig);
00914 PointGreyCamera::handleError("PointGreyCamera::GetGigEConfig could not get GigE setting", error);
00915 gigeconfig.enablePacketResend = true;
00916 error = cam.SetGigEConfig(&gigeconfig);
00917 PointGreyCamera::handleError("PointGreyCamera::SetGigEConfig could not set GigE settings (packet resend)", error);
00918 }
00919
00920 error = cam_.Connect(&guid);
00921 PointGreyCamera::handleError("PointGreyCamera::connect Failed to connect to camera", error);
00922
00923
00924 CameraInfo cInfo;
00925 error = cam_.GetCameraInfo(&cInfo);
00926 PointGreyCamera::handleError("PointGreyCamera::connect Failed to get camera info.", error);
00927 isColor_ = cInfo.isColorCamera;
00928
00929
00930 EmbeddedImageInfo info;
00931 info.timestamp.onOff = true;
00932 info.gain.onOff = true;
00933 info.shutter.onOff = true;
00934 info.brightness.onOff = true;
00935 info.exposure.onOff = true;
00936 info.whiteBalance.onOff = true;
00937 info.frameCounter.onOff = true;
00938 info.ROIPosition.onOff = true;
00939 error = cam_.SetEmbeddedImageInfo(&info);
00940 PointGreyCamera::handleError("PointGreyCamera::connect Could not enable metadata", error);
00941 }
00942 }
00943
00944 void PointGreyCamera::disconnect()
00945 {
00946 boost::mutex::scoped_lock scopedLock(mutex_);
00947 captureRunning_ = false;
00948 if(cam_.IsConnected())
00949 {
00950 Error error = cam_.Disconnect();
00951 PointGreyCamera::handleError("PointGreyCamera::disconnect Failed to disconnect camera", error);
00952 }
00953 }
00954
00955 void PointGreyCamera::start()
00956 {
00957 if(cam_.IsConnected() && !captureRunning_)
00958 {
00959
00960 Error error = cam_.StartCapture();
00961 PointGreyCamera::handleError("PointGreyCamera::start Failed to start capture", error);
00962 captureRunning_ = true;
00963 }
00964 }
00965
00966 bool PointGreyCamera::stop()
00967 {
00968 if(cam_.IsConnected() && captureRunning_)
00969 {
00970
00971 captureRunning_ = false;
00972 Error error = cam_.StopCapture();
00973 PointGreyCamera::handleError("PointGreyCamera::stop Failed to stop capture", error);
00974 return true;
00975 }
00976 return false;
00977 }
00978
00979 void PointGreyCamera::grabImage(sensor_msgs::Image &image, const std::string &frame_id)
00980 {
00981 boost::mutex::scoped_lock scopedLock(mutex_);
00982 if(cam_.IsConnected() && captureRunning_)
00983 {
00984
00985 Image rawImage;
00986
00987 Error error = cam_.RetrieveBuffer(&rawImage);
00988 PointGreyCamera::handleError("PointGreyCamera::grabImage Failed to retrieve buffer", error);
00989 metadata_ = rawImage.GetMetadata();
00990
00991
00992 TimeStamp embeddedTime = rawImage.GetTimeStamp();
00993 image.header.stamp.sec = embeddedTime.seconds;
00994 image.header.stamp.nsec = 1000 * embeddedTime.microSeconds;
00995
00996
00997 uint8_t bitsPerPixel = rawImage.GetBitsPerPixel();
00998
00999
01000 std::string imageEncoding = sensor_msgs::image_encodings::MONO8;
01001 BayerTileFormat bayer_format = rawImage.GetBayerTileFormat();
01002 if(isColor_ && bayer_format != NONE)
01003 {
01004 if(bitsPerPixel == 16)
01005 {
01006 switch(bayer_format)
01007 {
01008 case RGGB:
01009 imageEncoding = sensor_msgs::image_encodings::BAYER_RGGB16;
01010 break;
01011 case GRBG:
01012 imageEncoding = sensor_msgs::image_encodings::BAYER_GRBG16;
01013 break;
01014 case GBRG:
01015 imageEncoding = sensor_msgs::image_encodings::BAYER_GBRG16;
01016 break;
01017 case BGGR:
01018 imageEncoding = sensor_msgs::image_encodings::BAYER_BGGR16;
01019 break;
01020 }
01021 }
01022 else
01023 {
01024 switch(bayer_format)
01025 {
01026 case RGGB:
01027 imageEncoding = sensor_msgs::image_encodings::BAYER_RGGB8;
01028 break;
01029 case GRBG:
01030 imageEncoding = sensor_msgs::image_encodings::BAYER_GRBG8;
01031 break;
01032 case GBRG:
01033 imageEncoding = sensor_msgs::image_encodings::BAYER_GBRG8;
01034 break;
01035 case BGGR:
01036 imageEncoding = sensor_msgs::image_encodings::BAYER_BGGR8;
01037 break;
01038 }
01039 }
01040 }
01041 else
01042 {
01043 if(bitsPerPixel == 16)
01044 {
01045 imageEncoding = sensor_msgs::image_encodings::MONO16;
01046 }
01047 else if(bitsPerPixel==24)
01048 {
01049 imageEncoding = sensor_msgs::image_encodings::RGB8;
01050 }
01051 else
01052 {
01053 imageEncoding = sensor_msgs::image_encodings::MONO8;
01054 }
01055 }
01056
01057 fillImage(image, imageEncoding, rawImage.GetRows(), rawImage.GetCols(), rawImage.GetStride(), rawImage.GetData());
01058 image.header.frame_id = frame_id;
01059 }
01060 else if(cam_.IsConnected())
01061 {
01062 throw CameraNotRunningException("PointGreyCamera::grabImage: Camera is currently not running. Please start the capture.");
01063 }
01064 else
01065 {
01066 throw std::runtime_error("PointGreyCamera::grabImage not connected!");
01067 }
01068 }
01069
01070 void PointGreyCamera::grabStereoImage(sensor_msgs::Image &image, const std::string &frame_id, sensor_msgs::Image &second_image, const std::string &second_frame_id)
01071 {
01072 boost::mutex::scoped_lock scopedLock(mutex_);
01073 if(cam_.IsConnected() && captureRunning_)
01074 {
01075
01076 Image rawImage;
01077
01078 Error error = cam_.RetrieveBuffer(&rawImage);
01079 PointGreyCamera::handleError("PointGreyCamera::grabStereoImage Failed to retrieve buffer", error);
01080 metadata_ = rawImage.GetMetadata();
01081
01082
01083 TimeStamp embeddedTime = rawImage.GetTimeStamp();
01084 image.header.stamp.sec = embeddedTime.seconds;
01085 image.header.stamp.nsec = 1000 * embeddedTime.microSeconds;
01086
01087
01088
01089
01090
01091
01092 std::string imageEncoding = sensor_msgs::image_encodings::MONO8;
01093 BayerTileFormat bayer_format = rawImage.GetBayerTileFormat();
01094
01095 if(isColor_ && bayer_format != NONE)
01096 {
01097 switch(bayer_format)
01098 {
01099 case RGGB:
01100 imageEncoding = sensor_msgs::image_encodings::BAYER_RGGB8;
01101 break;
01102 case GRBG:
01103 imageEncoding = sensor_msgs::image_encodings::BAYER_GRBG8;
01104 break;
01105 case GBRG:
01106 imageEncoding = sensor_msgs::image_encodings::BAYER_GBRG8;
01107 break;
01108 case BGGR:
01109 imageEncoding = sensor_msgs::image_encodings::BAYER_BGGR8;
01110 break;
01111 }
01112 }
01113 else
01114 {
01115 imageEncoding = sensor_msgs::image_encodings::MONO8;
01116 }
01117
01118
01119 image.encoding = imageEncoding;
01120 second_image.encoding = imageEncoding;
01121 image.height = rawImage.GetRows();
01122 second_image.height = image.height;
01123 image.width = rawImage.GetCols();
01124 second_image.width = image.width;
01125 image.step = rawImage.GetStride() / 2;
01126 second_image.step = image.step;
01127 image.is_bigendian = 0;
01128 second_image.is_bigendian = 0;
01129 size_t st0 = (image.height * image.step);
01130 image.data.resize(st0);
01131 second_image.data.resize(st0);
01132 image.header.frame_id = frame_id;
01133 second_image.header.frame_id = second_frame_id;
01134
01135
01136 const uint8_t* raw_data = rawImage.GetData();
01137
01138
01139 for(size_t i = 0; i < rawImage.GetRows(); i++)
01140 {
01141 for(size_t j = 0; j < rawImage.GetCols(); j++)
01142 {
01143 size_t index = i * image.step + j;
01144 size_t raw_index = 2 * index;
01145 image.data[index] = raw_data[raw_index];
01146 second_image.data[index] = raw_data[raw_index + 1];
01147 }
01148 }
01149
01150
01151 }
01152 else if(cam_.IsConnected())
01153 {
01154 throw CameraNotRunningException("PointGreyCamera::grabStereoImage: Camera is currently not running. Please start the capture.");
01155 }
01156 else
01157 {
01158 throw std::runtime_error("PointGreyCamera::grabStereoImage not connected!");
01159 }
01160 }
01161
01162 uint PointGreyCamera::getGain()
01163 {
01164 return metadata_.embeddedGain >> 20;
01165 }
01166
01167 uint PointGreyCamera::getShutter()
01168 {
01169 return metadata_.embeddedShutter >> 20;
01170 }
01171
01172 uint PointGreyCamera::getBrightness()
01173 {
01174 return metadata_.embeddedTimeStamp >> 20;
01175 }
01176
01177 uint PointGreyCamera::getExposure()
01178 {
01179 return metadata_.embeddedBrightness >> 20;
01180 }
01181
01182 uint PointGreyCamera::getWhiteBalance()
01183 {
01184 return metadata_.embeddedExposure >> 8;
01185 }
01186
01187 uint PointGreyCamera::getROIPosition()
01188 {
01189 return metadata_.embeddedROIPosition >> 24;
01190 }
01191
01192 void PointGreyCamera::setDesiredCamera(const uint32_t &id)
01193 {
01194 serial_ = id;
01195 }
01196
01197 std::vector<uint32_t> PointGreyCamera::getAttachedCameras()
01198 {
01199 std::vector<uint32_t> cameras;
01200 unsigned int num_cameras;
01201 Error error = busMgr_.GetNumOfCameras(&num_cameras);
01202 PointGreyCamera::handleError("PointGreyCamera::getAttachedCameras: Could not get number of cameras", error);
01203 for(unsigned int i = 0; i < num_cameras; i++)
01204 {
01205 unsigned int this_serial;
01206 error = busMgr_.GetCameraSerialNumberFromIndex(i, &this_serial);
01207 PointGreyCamera::handleError("PointGreyCamera::getAttachedCameras: Could not get get serial number from index", error);
01208 cameras.push_back(this_serial);
01209 }
01210 return cameras;
01211 }
01212
01213 void PointGreyCamera::handleError(const std::string &prefix, const FlyCapture2::Error &error)
01214 {
01215 if(error == PGRERROR_TIMEOUT)
01216 {
01217 throw CameraTimeoutException("PointGreyCamera: Failed to retrieve buffer within timeout.");
01218 }
01219 else if(error == PGRERROR_IMAGE_CONSISTENCY_ERROR)
01220 {
01221 throw CameraImageConsistencyError("PointGreyCamera: Image consistency error.");
01222 }
01223 else if(error != PGRERROR_OK)
01224 {
01225 std::string start(" | FlyCapture2::ErrorType ");
01226 std::stringstream out;
01227 out << error.GetType();
01228 std::string desc(error.GetDescription());
01229 throw std::runtime_error(prefix + start + out.str() + " " + desc);
01230 }
01231 }