00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "prosilica/prosilica.h"
00036 #include <PvRegIo.h>
00037 #include <cassert>
00038 #include <cstdio>
00039 #include <ctime>
00040 #include <cstring>
00041 #include <arpa/inet.h>
00042
00043 #include <ros/console.h>
00044
00045 #define CHECK_ERR(fnc, amsg) \
00046 do { \
00047 tPvErr err = fnc; \
00048 if (err != ePvErrSuccess) { \
00049 char msg[256]; \
00050 snprintf(msg, 256, "%s: %s", amsg, errorStrings[err]); \
00051 throw ProsilicaException(err, msg); \
00052 } \
00053 } while (false)
00054
00055 namespace prosilica {
00056
00057 static const unsigned int MAX_CAMERA_LIST = 10;
00058 static const char* autoValues[] = {"Manual", "Auto", "AutoOnce"};
00059 static const char* triggerModes[] = {"Freerun", "SyncIn1", "SyncIn2", "FixedRate", "Software"};
00060 static const char* acquisitionModes[] = {"Continuous","SingleFrame","MultiFrame","Recorder"};
00061 static const char* errorStrings[] = {"No error",
00062 "Unexpected camera fault",
00063 "Unexpected fault in PvApi or driver",
00064 "Camera handle is invalid",
00065 "Bad parameter to API call",
00066 "Sequence of API calls is incorrect",
00067 "Camera or attribute not found",
00068 "Camera cannot be opened in the specified mode",
00069 "Camera was unplugged",
00070 "Setup is invalid (an attribute is invalid)",
00071 "System/network resources or memory not available",
00072 "1394 bandwidth not available",
00073 "Too many frames on queue",
00074 "Frame buffer is too small",
00075 "Frame cancelled by user",
00076 "The data for the frame was lost",
00077 "Some data in the frame is missing",
00078 "Timeout during wait",
00079 "Attribute value is out of the expected range",
00080 "Attribute is not this type (wrong access function)",
00081 "Attribute write forbidden at this time",
00082 "Attribute is not available at this time",
00083 "A firewall is blocking the traffic"};
00084
00085 static tPvCameraInfo cameraList[MAX_CAMERA_LIST];
00086 static unsigned long cameraNum = 0;
00087
00088 void init()
00089 {
00090 CHECK_ERR( PvInitialize(), "Failed to initialize Prosilica API" );
00091
00092
00093
00094 for (int tries = 0; tries < 5; ++tries)
00095 {
00096 cameraNum = PvCameraList(cameraList, MAX_CAMERA_LIST, NULL);
00097 if (cameraNum)
00098 return;
00099 usleep(200000);
00100 }
00101
00103 }
00104
00105 void fini()
00106 {
00107 PvUnInitialize();
00108 }
00109
00110 size_t numCameras()
00111 {
00112 return cameraNum;
00113 }
00114
00115 uint64_t getGuid(size_t i)
00116 {
00117 assert(i < MAX_CAMERA_LIST);
00118 if (i >= cameraNum)
00119 throw ProsilicaException(ePvErrBadParameter, "No camera at index i");
00120 return cameraList[i].UniqueId;
00121 }
00122
00124 static void openCamera(boost::function<tPvErr (tPvCameraInfo*)> info_fn,
00125 boost::function<tPvErr (tPvAccessFlags)> open_fn)
00126 {
00127 tPvCameraInfo info;
00128 CHECK_ERR( info_fn(&info), "Unable to find requested camera" );
00129
00130 if (!(info.PermittedAccess & ePvAccessMaster))
00131 throw ProsilicaException(ePvErrAccessDenied,
00132 "Unable to open camera as master. "
00133 "Another process is already using it.");
00134
00135 CHECK_ERR( open_fn(ePvAccessMaster), "Unable to open requested camera" );
00136 }
00137
00138 Camera::Camera(unsigned long guid, size_t bufferSize)
00139 : bufferSize_(bufferSize), FSTmode_(None)
00140 {
00141 openCamera(boost::bind(PvCameraInfo, guid, _1),
00142 boost::bind(PvCameraOpen, guid, _1, &handle_));
00143
00144 setup();
00145 }
00146
00147 Camera::Camera(const char* ip_address, size_t bufferSize)
00148 : bufferSize_(bufferSize), FSTmode_(None)
00149 {
00150 unsigned long addr = inet_addr(ip_address);
00151 tPvIpSettings settings;
00152 openCamera(boost::bind(PvCameraInfoByAddr, addr, _1, &settings),
00153 boost::bind(PvCameraOpenByAddr, addr, _1, &handle_));
00154
00155 setup();
00156 }
00157
00158 void Camera::setup()
00159 {
00160
00161 tPvUint32 maxPacketSize = 9000;
00162 PvCaptureAdjustPacketSize(handle_, maxPacketSize);
00163
00164
00165 unsigned long max_data_rate = getMaxDataRate();
00166 if (max_data_rate < GIGE_MAX_DATA_RATE) {
00167 ROS_WARN("Detected max data rate is %lu bytes/s, typical maximum data rate for a "
00168 "GigE port is %lu bytes/s. Are you using a GigE network card and cable?\n",
00169 max_data_rate, GIGE_MAX_DATA_RATE);
00170 }
00171 setAttribute("StreamBytesPerSecond", max_data_rate);
00172
00173
00174 setBinning();
00175 setRoiToWholeFrame();
00176
00177
00178 CHECK_ERR( PvAttrUint32Get(handle_, "TotalBytesPerFrame", &frameSize_),
00179 "Unable to retrieve frame size" );
00180
00181
00182 frames_ = new tPvFrame[bufferSize_];
00183 memset(frames_, 0, sizeof(tPvFrame) * bufferSize_);
00184 for (unsigned int i = 0; i < bufferSize_; ++i)
00185 {
00186 frames_[i].ImageBuffer = new char[frameSize_];
00187 frames_[i].ImageBufferSize = frameSize_;
00188 frames_[i].Context[0] = (void*)this;
00189 }
00190 }
00191
00192 Camera::~Camera()
00193 {
00194 stop();
00195
00196 PvCameraClose(handle_);
00197
00198 if (frames_)
00199 {
00200 for (unsigned int i = 0; i < bufferSize_; ++i)
00201 delete[] (char*)frames_[i].ImageBuffer;
00202 delete[] frames_;
00203 }
00204 }
00205
00206 void Camera::setFrameCallback(boost::function<void (tPvFrame*)> callback)
00207 {
00208 userCallback_ = callback;
00209 }
00210
00211 void Camera::start(FrameStartTriggerMode fmode, AcquisitionMode amode)
00212 {
00213 assert( FSTmode_ == None && fmode != None );
00215 assert( fmode == SyncIn1 || fmode == SyncIn2 || fmode == Software || !userCallback_.empty() );
00216
00217
00218 CHECK_ERR( PvCaptureStart(handle_), "Could not start capture");
00219
00220 if (fmode == Freerun || fmode == SyncIn1 || fmode == SyncIn2)
00221 for (unsigned int i = 0; i < bufferSize_; ++i)
00222 PvCaptureQueueFrame(handle_, frames_ + i, Camera::frameDone);
00223
00224
00225 try {
00227 CHECK_ERR( PvAttrEnumSet(handle_, "AcquisitionMode", acquisitionModes[amode]),
00228 "Could not set acquisition mode" );
00229 CHECK_ERR( PvAttrEnumSet(handle_, "FrameStartTriggerMode", triggerModes[fmode]),
00230 "Could not set trigger mode" );
00231 CHECK_ERR( PvCommandRun(handle_, "AcquisitionStart"),
00232 "Could not start acquisition" );
00233 }
00234 catch (ProsilicaException& e) {
00235 PvCaptureEnd(handle_);
00236 throw;
00237 }
00238 FSTmode_ = fmode;
00239 Amode_ = amode;
00240 }
00241
00242 void Camera::stop()
00243 {
00244 if (FSTmode_ == None)
00245 return;
00246
00247 PvCommandRun(handle_, "AcquisitionStop");
00248 PvCaptureEnd(handle_);
00249 PvCaptureQueueClear(handle_);
00250 FSTmode_ = None;
00251 }
00252
00253 tPvFrame* Camera::grab(unsigned long timeout_ms)
00254 {
00255 assert( FSTmode_ == Software );
00256
00257 unsigned long time_so_far = 0;
00258 while (time_so_far < timeout_ms)
00259 {
00263 boost::this_thread::sleep(boost::posix_time::millisec(400));
00264
00265
00266 tPvFrame* frame = &frames_[0];
00267 CHECK_ERR( PvCaptureQueueFrame(handle_, frame, NULL), "Couldn't queue frame" );
00268
00269
00270 CHECK_ERR( PvCommandRun(handle_, "FrameStartTriggerSoftware"),
00271 "Couldn't trigger capture" );
00272
00273
00274
00275 tPvErr e = ePvErrSuccess;
00276 do
00277 {
00278 try
00279 {
00280 if (e != ePvErrSuccess)
00281 ROS_DEBUG("Retrying CaptureWait due to error: %s", errorStrings[e]);
00282 clock_t start_time = clock();
00283 e = PvCaptureWaitForFrameDone(handle_, frame, timeout_ms - time_so_far);
00284 if (timeout_ms != PVINFINITE)
00285 time_so_far += ((clock() - start_time) * 1000) / CLOCKS_PER_SEC;
00286 }
00287 catch (prosilica::ProsilicaException &e) {
00288 ROS_ERROR("Prosilica exception during grab, will retry: %s\n", e.what());
00289 }
00290 } while (e == ePvErrTimeout && time_so_far < timeout_ms);
00291
00292 if (e != ePvErrSuccess)
00293 return NULL;
00294
00295 if (frame->Status == ePvErrSuccess)
00296 return frame;
00297
00298 ROS_DEBUG("Error in frame: %s", errorStrings[frame->Status]);
00299
00300
00301 if (frame->Status != ePvErrDataMissing && frame->Status != ePvErrDataLost)
00302 return NULL;
00303 }
00304
00305 return NULL;
00306 }
00307
00308 void Camera::setExposure(unsigned int val, AutoSetting isauto)
00309 {
00310 CHECK_ERR( PvAttrEnumSet(handle_, "ExposureMode", autoValues[isauto]),
00311 "Couldn't set exposure mode" );
00312
00313 if (isauto == Manual)
00314 CHECK_ERR( PvAttrUint32Set(handle_, "ExposureValue", val),
00315 "Couldn't set exposure value" );
00316 }
00317
00318 void Camera::setGain(unsigned int val, AutoSetting isauto)
00319 {
00322 if (PvAttrIsAvailable(handle_, "GainMode") == ePvErrSuccess)
00323 {
00324 CHECK_ERR( PvAttrEnumSet(handle_, "GainMode", autoValues[isauto]),
00325 "Couldn't set gain mode" );
00326 }
00327
00328 if (isauto == Manual)
00329 CHECK_ERR( PvAttrUint32Set(handle_, "GainValue", val),
00330 "Couldn't set gain value" );
00331 }
00332
00333 void Camera::setWhiteBalance(unsigned int blue, unsigned int red, AutoSetting isauto)
00334 {
00335 if (PvAttrIsAvailable(handle_, "WhitebalMode") == ePvErrSuccess)
00336 {
00337 CHECK_ERR( PvAttrEnumSet(handle_, "WhitebalMode", autoValues[isauto]),
00338 "Couldn't set white balance mode" );
00339 }
00340
00341 if (isauto == Manual && PvAttrIsAvailable(handle_, "WhitebalValueBlue"))
00342 {
00343 CHECK_ERR( PvAttrUint32Set(handle_, "WhitebalValueBlue", blue),
00344 "Couldn't set white balance blue value" );
00345 CHECK_ERR( PvAttrUint32Set(handle_, "WhitebalValueRed", red),
00346 "Couldn't set white balance red value" );
00347 }
00348 }
00349
00350 void Camera::setRoi(unsigned int x, unsigned int y,
00351 unsigned int width, unsigned int height)
00352 {
00353 CHECK_ERR( PvAttrUint32Set(handle_, "RegionX", x),
00354 "Couldn't set region x (left edge)" );
00355 CHECK_ERR( PvAttrUint32Set(handle_, "RegionY", y),
00356 "Couldn't set region y (top edge)" );
00357 CHECK_ERR( PvAttrUint32Set(handle_, "Width", width),
00358 "Couldn't set region width" );
00359 CHECK_ERR( PvAttrUint32Set(handle_, "Height", height),
00360 "Couldn't set region height" );
00361 }
00362
00363 void Camera::setRoiToWholeFrame()
00364 {
00365 tPvUint32 min_val, max_val;
00366 CHECK_ERR( PvAttrUint32Set(handle_, "RegionX", 0),
00367 "Couldn't set region x (left edge)" );
00368 CHECK_ERR( PvAttrUint32Set(handle_, "RegionY", 0),
00369 "Couldn't set region y (top edge)" );
00370 CHECK_ERR( PvAttrRangeUint32(handle_, "Width", &min_val, &max_val),
00371 "Couldn't get range of Width attribute" );
00372 CHECK_ERR( PvAttrUint32Set(handle_, "Width", max_val),
00373 "Couldn't set region width" );
00374 CHECK_ERR( PvAttrRangeUint32(handle_, "Height", &min_val, &max_val),
00375 "Couldn't get range of Height attribute" );
00376 CHECK_ERR( PvAttrUint32Set(handle_, "Height", max_val),
00377 "Couldn't set region height" );
00378 }
00379
00380 void Camera::setBinning(unsigned int binning_x, unsigned int binning_y)
00381 {
00382
00383 if (!hasAttribute("BinningX") && binning_x == 1 && binning_y == 1)
00384 return;
00385
00386 CHECK_ERR( PvAttrUint32Set(handle_, "BinningX", binning_x),
00387 "Couldn't set horizontal binning" );
00388 CHECK_ERR( PvAttrUint32Set(handle_, "BinningY", binning_y),
00389 "Couldn't set vertical binning" );
00390 }
00391
00392 bool Camera::hasAttribute(const std::string &name)
00393 {
00394 return (PvAttrIsAvailable(handle_, name.c_str()) == ePvErrSuccess);
00395 }
00396
00397 static void getStringValuedAttribute(std::string &value,
00398 boost::function<tPvErr (char*, unsigned long, unsigned long*)> get_fn)
00399 {
00400 if (value.size() == 0)
00401 value.resize(32);
00402
00403 unsigned long actual_size;
00404 CHECK_ERR( get_fn(&value[0], value.size(), &actual_size),
00405 "Couldn't get attribute" );
00406
00407 if (actual_size >= value.size()) {
00408 value.resize(actual_size + 1);
00409 CHECK_ERR( get_fn(&value[0], value.size(), &actual_size),
00410 "Couldn't get attribute" );
00411 }
00412 }
00413
00414 void Camera::getAttributeEnum(const std::string &name, std::string &value)
00415 {
00416 getStringValuedAttribute(value,
00417 boost::bind(PvAttrEnumGet, handle_, name.c_str(), _1, _2, _3));
00418 }
00419
00420 void Camera::getAttribute(const std::string &name, tPvUint32 &value)
00421 {
00422 std::string err_msg = "Couldn't get attribute " + name;
00423 CHECK_ERR( PvAttrUint32Get(handle_, name.c_str(), &value),
00424 err_msg.c_str());
00425
00426 }
00427
00428 void Camera::getAttribute(const std::string &name, tPvFloat32 &value)
00429 {
00430 std::string err_msg = "Couldn't get attribute " + name;
00431 CHECK_ERR( PvAttrFloat32Get(handle_, name.c_str(), &value),
00432 err_msg.c_str());
00433 }
00434
00435 void Camera::getAttribute(const std::string &name, std::string &value)
00436 {
00437 getStringValuedAttribute(value,
00438 boost::bind(PvAttrStringGet, handle_, name.c_str(), _1, _2, _3));
00439 }
00440
00441 void Camera::setAttributeEnum(const std::string &name, const std::string &value)
00442 {
00443 std::string err_msg = "Couldn't get attribute " + name;
00444 CHECK_ERR( PvAttrEnumSet(handle_, name.c_str(), value.c_str()),
00445 err_msg.c_str());
00446 }
00447
00448 void Camera::setAttribute(const std::string &name, tPvUint32 value)
00449 {
00450 std::string err_msg = "Couldn't set attribute " + name;
00451 CHECK_ERR( PvAttrUint32Set(handle_, name.c_str(), value),
00452 err_msg.c_str());
00453 }
00454
00455 void Camera::setAttribute(const std::string &name, tPvFloat32 value)
00456 {
00457 std::string err_msg = "Couldn't set attribute " + name;
00458 CHECK_ERR( PvAttrFloat32Set(handle_, name.c_str(), value),
00459 err_msg.c_str());
00460 }
00461
00462 void Camera::setAttribute(const std::string &name, const std::string &value)
00463 {
00464 std::string err_msg = "Couldn't set attribute " + name;
00465 CHECK_ERR( PvAttrStringSet(handle_, name.c_str(), value.c_str()),
00466 err_msg.c_str());
00467 }
00468
00469 void Camera::runCommand(const std::string& name)
00470 {
00471 std::string err_msg = "Couldn't run command " + name;
00472 CHECK_ERR( PvCommandRun(handle_, name.c_str()), err_msg.c_str());
00473 }
00474
00475 unsigned long Camera::guid()
00476 {
00477 unsigned long id;
00478 CHECK_ERR( PvAttrUint32Get(handle_, "UniqueId", &id),
00479 "Couldn't retrieve unique id" );
00480 return id;
00481 }
00482
00483 unsigned long Camera::getMaxDataRate()
00484 {
00485 tPvUint32 min_data_rate, max_data_rate;
00486 CHECK_ERR( PvAttrRangeUint32(handle_, "StreamBytesPerSecond", &min_data_rate, &max_data_rate),
00487 "Couldn't get range of attribute StreamBytesPerSecond" );
00488 return max_data_rate;
00489 }
00490
00491 static const unsigned long USER_ADDRESS = 0x17200;
00492
00493 void Camera::writeUserMemory(const char* data, size_t size)
00494 {
00495 assert(size <= USER_MEMORY_SIZE);
00496
00497 unsigned char buffer[USER_MEMORY_SIZE] = {0};
00498 memcpy(buffer, data, size);
00499
00500 unsigned long written;
00501 CHECK_ERR( PvMemoryWrite(handle_, USER_ADDRESS, USER_MEMORY_SIZE, buffer, &written),
00502 "Couldn't write to user memory" );
00503 }
00504
00505 void Camera::readUserMemory(char* data, size_t size)
00506 {
00507 assert(size <= USER_MEMORY_SIZE);
00508
00509 unsigned char buffer[USER_MEMORY_SIZE];
00510
00511 CHECK_ERR( PvMemoryRead(handle_, USER_ADDRESS, USER_MEMORY_SIZE, buffer),
00512 "Couldn't read from user memory" );
00513
00514 memcpy(data, buffer, size);
00515 }
00516
00517 void Camera::frameDone(tPvFrame* frame)
00518 {
00519
00520 if (frame->Status == ePvErrUnplugged || frame->Status == ePvErrCancelled)
00521 return;
00522
00523 Camera* camPtr = (Camera*) frame->Context[0];
00524 if (frame->Status == ePvErrSuccess && camPtr && !camPtr->userCallback_.empty()) {
00525
00526 boost::lock_guard<boost::mutex> guard(camPtr->frameMutex_);
00527 camPtr->userCallback_(frame);
00528 }
00529 else if (frame->Status == ePvErrDataMissing) {
00530
00531 ROS_DEBUG("Error in frame: %s\n", errorStrings[frame->Status]);
00532 }
00533 else {
00534 ROS_WARN("Error in frame: %s\n", errorStrings[frame->Status]);
00535 }
00536
00537 PvCaptureQueueFrame(camPtr->handle_, frame, Camera::frameDone);
00538 }
00539
00540 tPvHandle Camera::handle()
00541 {
00542 return handle_;
00543 }
00544
00545 }