prosilica.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
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   // Spend up to 1s trying to find a camera. Finding no camera is not
00093   // an error; the user may still be able open one by IP address.
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   // adjust packet size according to the current network capacity
00161   tPvUint32 maxPacketSize = 9000;
00162   PvCaptureAdjustPacketSize(handle_, maxPacketSize);
00163 
00164   // set data rate to the max
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   // capture whole frame by default
00174   setBinning();
00175   setRoiToWholeFrame();
00176   
00177   // query for attributes (TODO: more)
00178   CHECK_ERR( PvAttrUint32Get(handle_, "TotalBytesPerFrame", &frameSize_),
00179              "Unable to retrieve frame size" );
00180   
00181   // allocate frame buffers
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; // for frameDone callback
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   // set camera in acquisition mode
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   // start capture after setting acquisition and trigger modes
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_); // reset to non capture mode
00236     throw; // rethrow
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     // Queue up a single frame
00266     tPvFrame* frame = &frames_[0];
00267     CHECK_ERR( PvCaptureQueueFrame(handle_, frame, NULL), "Couldn't queue frame" );
00268     
00269     // Trigger the camera
00270     CHECK_ERR( PvCommandRun(handle_, "FrameStartTriggerSoftware"),
00271                "Couldn't trigger capture" );
00272 
00273     // Wait for frame capture to finish. The wait call may timeout in less
00274     // than the allotted time, so we keep trying until we exceed it.
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; // Something bad happened (camera unplugged?)
00294     
00295     if (frame->Status == ePvErrSuccess)
00296       return frame; // Yay!
00297 
00298     ROS_DEBUG("Error in frame: %s", errorStrings[frame->Status]);
00299 
00300     // Retry if data was lost in transmission. Probably no hope on other errors.
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   // Permit setting to "no binning" on cameras without binning support
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   // don't requeue if capture has stopped
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     // TODO: thread safety OK here?
00526     boost::lock_guard<boost::mutex> guard(camPtr->frameMutex_);
00527     camPtr->userCallback_(frame);
00528   }
00529   else if (frame->Status == ePvErrDataMissing) {
00530     // Avoid warning spew; lots of dropped packets will show up in the diagnostics.
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 } // namespace prosilica


prosilica_camera
Author(s): Patrick Mihelich
autogenerated on Thu Jan 2 2014 11:46:39