prosilica_node.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 // ROS communication
00036 #include <ros/ros.h>
00037 #include <sensor_msgs/Image.h>
00038 #include <sensor_msgs/CameraInfo.h>
00039 #include <sensor_msgs/fill_image.h>
00040 #include <sensor_msgs/SetCameraInfo.h>
00041 #include <std_msgs/Header.h>
00042 #include <image_transport/image_transport.h>
00043 #include <camera_calibration_parsers/parse_ini.h>
00044 #include <polled_camera/publication_server.h>
00045 
00046 // Diagnostics
00047 #include <diagnostic_updater/diagnostic_updater.h>
00048 #include <diagnostic_updater/DiagnosticStatusWrapper.h>
00049 #include <self_test/self_test.h>
00050 
00051 // Dynamic reconfigure
00052 #include <dynamic_reconfigure/server.h>
00053 #include <driver_base/SensorLevels.h>
00054 #include "prosilica_camera/ProsilicaCameraConfig.h"
00055 
00056 // Standard libs
00057 #include <boost/scoped_ptr.hpp>
00058 #include <boost/bind.hpp>
00059 #include <boost/lexical_cast.hpp>
00060 #include <boost/format.hpp>
00061 #include <sstream>
00062 
00063 #include "prosilica/prosilica.h"
00064 #include "prosilica/rolling_sum.h"
00065 
00067 class ProsilicaNode
00068 {
00069 private:
00070   ros::NodeHandle nh_;
00071   image_transport::ImageTransport it_;
00072   image_transport::CameraPublisher streaming_pub_;
00073   polled_camera::PublicationServer poll_srv_;
00074   ros::ServiceServer set_camera_info_srv_;
00075   ros::Subscriber trigger_sub_;
00076   
00077   // Camera
00078   boost::scoped_ptr<prosilica::Camera> cam_;
00079   prosilica::FrameStartTriggerMode trigger_mode_; 
00080   bool running_;
00081   unsigned long max_data_rate_;
00082   tPvUint32 sensor_width_, sensor_height_; // full resolution dimensions (maybe should be in lib)
00083   bool auto_adjust_stream_bytes_per_second_;
00084 
00085   // Hardware triggering
00086   std::string trig_timestamp_topic_;
00087   ros::Time trig_time_;
00088 
00089   // ROS messages
00090   sensor_msgs::Image img_;
00091   sensor_msgs::CameraInfo cam_info_;
00092 
00093   // Dynamic reconfigure
00094   typedef prosilica_camera::ProsilicaCameraConfig Config;
00095   typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
00096   ReconfigureServer reconfigure_server_;
00097   
00098   // Diagnostics
00099   ros::Timer diagnostic_timer_;
00100   boost::shared_ptr<self_test::TestRunner> self_test_;
00101   diagnostic_updater::Updater diagnostic_;
00102   std::string hw_id_;
00103   int count_;
00104   double desired_freq_;
00105   static const int WINDOW_SIZE = 5; // remember previous 5s
00106   unsigned long frames_dropped_total_, frames_completed_total_;
00107   RollingSum<unsigned long> frames_dropped_acc_, frames_completed_acc_;
00108   unsigned long packets_missed_total_, packets_received_total_;
00109   RollingSum<unsigned long> packets_missed_acc_, packets_received_acc_;
00110 
00111   // So we don't get burned by auto-exposure
00112   unsigned long last_exposure_value_;
00113   int consecutive_stable_exposures_;
00114 
00115 public:
00116   ProsilicaNode(const ros::NodeHandle& node_handle)
00117     : nh_(node_handle),
00118       it_(nh_),
00119       cam_(NULL), running_(false), auto_adjust_stream_bytes_per_second_(false),
00120       count_(0),
00121       frames_dropped_total_(0), frames_completed_total_(0),
00122       frames_dropped_acc_(WINDOW_SIZE),
00123       frames_completed_acc_(WINDOW_SIZE),
00124       packets_missed_total_(0), packets_received_total_(0),
00125       packets_missed_acc_(WINDOW_SIZE),
00126       packets_received_acc_(WINDOW_SIZE)
00127   {
00128     // Two-stage initialization: in the constructor we open the requested camera. Most
00129     // parameters controlling capture are set and streaming started in configure(), the
00130     // callback to dynamic_reconfig.
00131     prosilica::init();
00132 
00133     if (prosilica::numCameras() == 0)
00134       ROS_WARN("Found no cameras on local subnet");
00135 
00136     // Determine which camera to use. Opening by IP address is preferred, then guid. If both
00137     // parameters are set we open by IP and verify the guid. If neither are set we default
00138     // to opening the first available camera.
00139     ros::NodeHandle local_nh("~");
00140     unsigned long guid = 0;
00141     std::string guid_str;
00142     if (local_nh.getParam("guid", guid_str) && !guid_str.empty())
00143       guid = strtol(guid_str.c_str(), NULL, 0);
00144 
00145     std::string ip_str;
00146     if (local_nh.getParam("ip_address", ip_str) && !ip_str.empty()) {
00147       cam_.reset( new prosilica::Camera(ip_str.c_str()) );
00148       
00149       // Verify guid is the one expected
00150       unsigned long cam_guid = cam_->guid();
00151       if (guid != 0 && guid != cam_guid)
00152         throw prosilica::ProsilicaException(ePvErrBadParameter,
00153                                             "guid does not match expected");
00154       guid = cam_guid;
00155     }
00156     else {
00157       if (guid == 0) guid = prosilica::getGuid(0);
00158       cam_.reset( new prosilica::Camera(guid) );
00159     }
00160     hw_id_ = boost::lexical_cast<std::string>(guid);
00161     ROS_INFO("Found camera, guid = %s", hw_id_.c_str());
00162     diagnostic_.setHardwareID(hw_id_);
00163 
00164     // Record some attributes of the camera
00165     tPvUint32 dummy;
00166     PvAttrRangeUint32(cam_->handle(), "Width", &dummy, &sensor_width_);
00167     PvAttrRangeUint32(cam_->handle(), "Height", &dummy, &sensor_height_);
00168     
00169     // Try to load intrinsics from on-camera memory.
00170     loadIntrinsics();
00171 
00172     // Set up self tests and diagnostics.
00173     // NB: Need to wait until here to construct self_test_, otherwise an exception
00174     // above from failing to find the camera gives bizarre backtraces
00175     // (http://answers.ros.org/question/430/trouble-with-prosilica_camera-pvapi).
00176     self_test_.reset(new self_test::TestRunner);
00177     self_test_->add( "Info Test", this, &ProsilicaNode::infoTest );
00178     self_test_->add( "Attribute Test", this, &ProsilicaNode::attributeTest );
00179     self_test_->add( "Image Test", this, &ProsilicaNode::imageTest );
00180     
00181     diagnostic_.add( "Frequency Status", this, &ProsilicaNode::freqStatus );
00182     diagnostic_.add( "Frame Statistics", this, &ProsilicaNode::frameStatistics );
00183     diagnostic_.add( "Packet Statistics", this, &ProsilicaNode::packetStatistics );
00184     diagnostic_.add( "Packet Error Status", this, &ProsilicaNode::packetErrorStatus );
00185 
00186     diagnostic_timer_ = nh_.createTimer(ros::Duration(0.1), boost::bind(&ProsilicaNode::runDiagnostics, this));
00187 
00188     // Service call for setting calibration.
00189     set_camera_info_srv_ = nh_.advertiseService("set_camera_info", &ProsilicaNode::setCameraInfo, this);
00190 
00191     // Start dynamic_reconfigure
00192     reconfigure_server_.setCallback(boost::bind(&ProsilicaNode::configure, this, _1, _2));
00193   }
00194 
00195   void configure(Config& config, uint32_t level)
00196   {
00197     ROS_DEBUG("Reconfigure request received");
00198 
00199     if (level >= (uint32_t)driver_base::SensorLevels::RECONFIGURE_STOP)
00200       stop();
00201 
00202     // Trigger mode
00203     if (config.trigger_mode == "streaming") {
00204       trigger_mode_ = prosilica::Freerun;
00206       desired_freq_ = 1; // make sure we get _something_
00207     }
00208     else if (config.trigger_mode == "syncin1") {
00209       trigger_mode_ = prosilica::SyncIn1;
00210       desired_freq_ = config.trig_rate;
00211     }
00212     else if (config.trigger_mode == "syncin2") {
00213       trigger_mode_ = prosilica::SyncIn2;
00214       desired_freq_ = config.trig_rate;
00215     }
00216 #if 0
00217     else if (config.trigger_mode == "fixedrate") {
00218       ROS_DEBUG("Fixed rate not supported yet implementing software");
00219       trigger_mode_ = prosilica::Software;
00221       desired_freq_ = 0;
00222     }
00223 #endif
00224     else if (config.trigger_mode == "polled") {
00225       trigger_mode_ = prosilica::Software;
00226       desired_freq_ = 0;
00227     }
00228     else {
00229       ROS_ERROR("Invalid trigger mode '%s' in reconfigure request", config.trigger_mode.c_str());
00230     }
00231     trig_timestamp_topic_ = config.trig_timestamp_topic;
00232 
00233     // Exposure
00234     if (config.auto_exposure)
00235     {
00236       cam_->setExposure(0, prosilica::Auto);
00237       if (cam_->hasAttribute("ExposureAutoMax"))
00238       {
00239         tPvUint32 us = config.exposure_auto_max*1000000. + 0.5;
00240         cam_->setAttribute("ExposureAutoMax", us);
00241       }
00242       if (cam_->hasAttribute("ExposureAutoTarget"))
00243         cam_->setAttribute("ExposureAutoTarget", (tPvUint32)config.exposure_auto_target);
00244     }
00245     else {
00246       unsigned us = config.exposure*1000000. + 0.5;
00247       cam_->setExposure(us, prosilica::Manual);
00248     }
00249 
00250     // Gain
00251     if (config.auto_gain) {
00252       if (cam_->hasAttribute("GainAutoMax"))
00253       {
00254         cam_->setGain(0, prosilica::Auto);
00255         cam_->setAttribute("GainAutoMax", (tPvUint32)config.gain_auto_max);
00256         cam_->setAttribute("GainAutoTarget", (tPvUint32)config.gain_auto_target);
00257       }
00258       else {
00259         tPvUint32 major, minor;
00260         cam_->getAttribute("FirmwareVerMajor", major);
00261         cam_->getAttribute("FirmwareVerMinor", minor);
00262         ROS_WARN("Auto gain not available for this camera. Auto gain is available "
00263                  "on firmware versions 1.36 and above. You are running version %u.%u.",
00264                  (unsigned)major, (unsigned)minor);
00265         config.auto_gain = false;
00266       }
00267     }
00268     else
00269       cam_->setGain(config.gain, prosilica::Manual);
00270     
00271     // White balance
00272     if (config.auto_whitebalance) {
00273       if (cam_->hasAttribute("WhitebalMode"))
00274         cam_->setWhiteBalance(0, 0, prosilica::Auto);
00275       else {
00276         ROS_WARN("Auto white balance not available for this camera.");
00277         config.auto_whitebalance = false;
00278       }
00279     }
00280     else
00281       cam_->setWhiteBalance(config.whitebalance_blue, config.whitebalance_red, prosilica::Manual);
00282 
00283     // Binning configuration
00284     if (cam_->hasAttribute("BinningX")) {
00285       tPvUint32 max_binning_x, max_binning_y, dummy;
00286       PvAttrRangeUint32(cam_->handle(), "BinningX", &dummy, &max_binning_x);
00287       PvAttrRangeUint32(cam_->handle(), "BinningY", &dummy, &max_binning_y);
00288       config.binning_x = std::min(config.binning_x, (int)max_binning_x);
00289       config.binning_y = std::min(config.binning_y, (int)max_binning_y);
00290       
00291       cam_->setBinning(config.binning_x, config.binning_y);
00292     }
00293     else if (config.binning_x > 1 || config.binning_y > 1)
00294     {
00295       ROS_WARN("Binning not available for this camera.");
00296       config.binning_x = config.binning_y = 1;
00297     }
00298 
00299     // Region of interest configuration
00300     // Make sure ROI fits in image
00301     config.x_offset = std::min(config.x_offset, (int)sensor_width_ - 1);
00302     config.y_offset = std::min(config.y_offset, (int)sensor_height_ - 1);
00303     config.width  = std::min(config.width, (int)sensor_width_ - config.x_offset);
00304     config.height = std::min(config.height, (int)sensor_height_ - config.y_offset);
00305     // If width or height is 0, set it as large as possible
00306     int width  = config.width  ? config.width  : sensor_width_  - config.x_offset;
00307     int height = config.height ? config.height : sensor_height_ - config.y_offset;
00308 
00309     // Adjust full-res ROI to binning ROI
00311     int x_offset = config.x_offset / config.binning_x;
00312     int y_offset = config.y_offset / config.binning_y;
00313     unsigned int right_x  = (config.x_offset + width  + config.binning_x - 1) / config.binning_x;
00314     unsigned int bottom_y = (config.y_offset + height + config.binning_y - 1) / config.binning_y;
00315     // Rounding up is bad when at max resolution which is not divisible by the amount of binning
00316     right_x = std::min(right_x, (unsigned)(sensor_width_ / config.binning_x));
00317     bottom_y = std::min(bottom_y, (unsigned)(sensor_height_ / config.binning_y));
00318     width = right_x - x_offset;
00319     height = bottom_y - y_offset;
00320 
00321     cam_->setRoi(x_offset, y_offset, width, height);
00322     
00323     // TF frame
00324     img_.header.frame_id = cam_info_.header.frame_id = config.frame_id;
00325 
00326     // Normally the node adjusts the bandwidth used by the camera during diagnostics, to use as
00327     // much as possible without dropping packets. But this can create interference if two
00328     // cameras are on the same switch, e.g. for stereo. So we allow the user to set the bandwidth
00329     // directly.
00330     auto_adjust_stream_bytes_per_second_ = config.auto_adjust_stream_bytes_per_second;
00331     if (!auto_adjust_stream_bytes_per_second_)
00332       cam_->setAttribute("StreamBytesPerSecond", (tPvUint32)config.stream_bytes_per_second);
00333 
00335     if (level >= (uint32_t)driver_base::SensorLevels::RECONFIGURE_STOP)
00336       start();
00337   }
00338 
00339   ~ProsilicaNode()
00340   {
00341     stop();
00342     cam_.reset(); // must destroy Camera before calling prosilica::fini
00343     prosilica::fini();
00344   }
00345 
00346   void syncInCallback (const std_msgs::HeaderConstPtr& msg)
00347   {
00349     trig_time_ = msg->stamp;
00350   }
00351   
00353 
00354   void start()
00355   {
00356     if (running_) return;
00357 
00358     if (trigger_mode_ == prosilica::Software) {
00359       poll_srv_ = polled_camera::advertise(nh_, "request_image", &ProsilicaNode::pollCallback, this);
00360       // Auto-exposure tends to go wild the first few frames after startup
00361       // if (auto_expose) normalizeExposure();
00362     }
00363     else {
00364       if ((trigger_mode_ == prosilica::SyncIn1) || (trigger_mode_ == prosilica::SyncIn2)) {
00365         if (!trig_timestamp_topic_.empty())
00366           trigger_sub_ = nh_.subscribe(trig_timestamp_topic_, 1, &ProsilicaNode::syncInCallback, this);
00367       }
00368       else {
00369         assert(trigger_mode_ == prosilica::Freerun);
00370       }
00371       cam_->setFrameCallback(boost::bind(&ProsilicaNode::publishImage, this, _1));
00372       streaming_pub_ = it_.advertiseCamera("image_raw", 1);
00373     }
00374     cam_->start(trigger_mode_, prosilica::Continuous);
00375     running_ = true;
00376   }
00377 
00378   void stop()
00379   {
00380     if (!running_) return;
00381 
00382     cam_->stop(); // Must stop camera before streaming_pub_.
00383     poll_srv_.shutdown();
00384     trigger_sub_.shutdown();
00385     streaming_pub_.shutdown();
00386     
00387     running_ = false;
00388   }
00389 
00390   void pollCallback(polled_camera::GetPolledImage::Request& req,
00391                     polled_camera::GetPolledImage::Response& rsp,
00392                     sensor_msgs::Image& image, sensor_msgs::CameraInfo& info)
00393   {
00394     if (trigger_mode_ != prosilica::Software) {
00395       rsp.success = false;
00396       rsp.status_message = "Camera is not in software triggered mode";
00397       return;
00398     }
00399 
00400     tPvFrame* frame = NULL;
00401 
00402     try {
00403       cam_->setBinning(req.binning_x, req.binning_y);
00404 
00405       if (req.roi.x_offset || req.roi.y_offset || req.roi.width || req.roi.height) {
00406         // GigE cameras use ROI in binned coordinates, so scale the values down.
00407         // The ROI is expanded if necessary to land on binned coordinates.
00408         unsigned int left_x = req.roi.x_offset / req.binning_x;
00409         unsigned int top_y  = req.roi.y_offset / req.binning_y;
00410         unsigned int right_x  = (req.roi.x_offset + req.roi.width  + req.binning_x - 1) / req.binning_x;
00411         unsigned int bottom_y = (req.roi.y_offset + req.roi.height + req.binning_y - 1) / req.binning_y;
00412         unsigned int width = right_x - left_x;
00413         unsigned int height = bottom_y - top_y;
00414         cam_->setRoi(left_x, top_y, width, height);
00415       } else {
00416         cam_->setRoiToWholeFrame();
00417       }
00418 
00419       // Zero duration means "no timeout"
00420       unsigned long timeout = req.timeout.isZero() ? PVINFINITE : req.timeout.toNSec() / 1e6;
00421       frame = cam_->grab(timeout);
00422     }
00423     catch (prosilica::ProsilicaException &e) {
00424       if (e.error_code == ePvErrBadSequence)
00425         throw; // not easily recoverable
00426 
00427       rsp.success = false;
00428       rsp.status_message = (boost::format("Prosilica exception: %s") % e.what()).str();
00429       return;
00430     }
00431 
00432     if (!frame) {
00434       rsp.success = false;
00435       rsp.status_message = "Failed to capture frame, may have timed out";
00436       return;
00437     }
00438 
00439     info = cam_info_;
00440     image.header.frame_id = img_.header.frame_id;
00441     if (!processFrame(frame, image, info)) {
00442       rsp.success = false;
00443       rsp.status_message = "Captured frame but failed to process it";
00444       return;
00445     }
00446     info.roi.do_rectify = req.roi.do_rectify; // do_rectify should be preserved from request
00447 
00448     rsp.success = true;
00449   }
00450 
00451   static bool frameToImage(tPvFrame* frame, sensor_msgs::Image &image)
00452   {
00453     // NOTE: 16-bit and Yuv formats not supported
00454     static const char* BAYER_ENCODINGS[] = { "bayer_rggb8", "bayer_gbrg8", "bayer_grbg8", "bayer_bggr8" };
00455 
00456     std::string encoding;
00457     if (frame->Format == ePvFmtMono8)       encoding = sensor_msgs::image_encodings::MONO8;
00458     else if (frame->Format == ePvFmtBayer8) 
00459     {
00460 #if 1
00461       encoding = BAYER_ENCODINGS[frame->BayerPattern];
00462 #else
00463       image.encoding = sensor_msgs::image_encodings::BGR8;
00464       image.height = frame->Height;
00465       image.width = frame->Width;
00466       image.step = frame->Width * 3;
00467       image.data.resize(frame->Height * (frame->Width * 3));
00468       PvUtilityColorInterpolate(frame, &image.data[2], &image.data[1], &image.data[0], 2, 0);
00469       return true;
00470 #endif
00471     }
00472     else if (frame->Format == ePvFmtRgb24)  encoding = sensor_msgs::image_encodings::RGB8;
00473     else if (frame->Format == ePvFmtBgr24)  encoding = sensor_msgs::image_encodings::BGR8;
00474     else if (frame->Format == ePvFmtRgba32) encoding = sensor_msgs::image_encodings::RGBA8;
00475     else if (frame->Format == ePvFmtBgra32) encoding = sensor_msgs::image_encodings::BGRA8;
00476     else {
00477       ROS_WARN("Received frame with unsupported pixel format %d", frame->Format);
00478       return false;
00479     }
00480 
00481     uint32_t step = frame->ImageSize / frame->Height;
00482     return sensor_msgs::fillImage(image, encoding, frame->Height, frame->Width, step, frame->ImageBuffer);
00483   }
00484   
00485   bool processFrame(tPvFrame* frame, sensor_msgs::Image &img, sensor_msgs::CameraInfo &cam_info)
00486   {
00488     if ((trigger_mode_ == prosilica::SyncIn1 || trigger_mode_ == prosilica::SyncIn2) && !trig_time_.isZero()) {
00489       img.header.stamp = cam_info.header.stamp = trig_time_;
00490       trig_time_ = ros::Time(); // Zero
00491     }
00492     else {
00494       img.header.stamp = cam_info.header.stamp = ros::Time::now();
00495     }
00496     
00500     tPvUint32 binning_x = 1, binning_y = 1;
00501     if (cam_->hasAttribute("BinningX")) {
00502       cam_->getAttribute("BinningX", binning_x);
00503       cam_->getAttribute("BinningY", binning_y);
00504     }
00505     // Binning averages bayer samples, so just call it mono8 in that case
00506     if (frame->Format == ePvFmtBayer8 && (binning_x > 1 || binning_y > 1))
00507       frame->Format = ePvFmtMono8;
00508 
00509     if (!frameToImage(frame, img))
00510       return false;
00511 
00512     // Set the operational parameters in CameraInfo (binning, ROI)
00513     cam_info.binning_x = binning_x;
00514     cam_info.binning_y = binning_y;
00515     // ROI in CameraInfo is in unbinned coordinates, need to scale up
00516     cam_info.roi.x_offset = frame->RegionX * binning_x;
00517     cam_info.roi.y_offset = frame->RegionY * binning_y;
00518     cam_info.roi.height = frame->Height * binning_y;
00519     cam_info.roi.width = frame->Width * binning_x;
00520     cam_info.roi.do_rectify = (frame->Height != sensor_height_ / binning_y) ||
00521                               (frame->Width  != sensor_width_  / binning_x);
00522 
00523     count_++;
00524     return true;
00525   }
00526   
00527   void publishImage(tPvFrame* frame)
00528   {
00529     if (processFrame(frame, img_, cam_info_))
00530       streaming_pub_.publish(img_, cam_info_);
00531   }
00532 
00533   void loadIntrinsics()
00534   {
00535     // Retrieve contents of user memory
00536     std::string buffer(prosilica::Camera::USER_MEMORY_SIZE, '\0');
00537     cam_->readUserMemory(&buffer[0], prosilica::Camera::USER_MEMORY_SIZE);
00538 
00539     // Parse calibration file
00540     std::string camera_name;
00541     if (camera_calibration_parsers::parseCalibrationIni(buffer, camera_name, cam_info_))
00542       ROS_INFO("Loaded calibration for camera '%s'", camera_name.c_str());
00543     else
00544       ROS_WARN("Failed to load intrinsics from camera");
00545   }
00546 
00547   bool setCameraInfo(sensor_msgs::SetCameraInfo::Request& req,
00548                      sensor_msgs::SetCameraInfo::Response& rsp)
00549   {
00550     ROS_INFO("New camera info received");
00551     sensor_msgs::CameraInfo &info = req.camera_info;
00552     
00553     // Sanity check: the image dimensions should match the max resolution of the sensor.
00554     tPvUint32 width, height, dummy;
00555     PvAttrRangeUint32(cam_->handle(), "Width", &dummy, &width);
00556     PvAttrRangeUint32(cam_->handle(), "Height", &dummy, &height);
00557     if (info.width != width || info.height != height) {
00558       rsp.success = false;
00559       rsp.status_message = (boost::format("Camera_info resolution %ix%i does not match current video "
00560                                           "setting, camera running at resolution %ix%i.")
00561                             % info.width % info.height % width % height).str();
00562       ROS_ERROR("%s", rsp.status_message.c_str());
00563       return true;
00564     }
00565     
00566     stop();
00567 
00568     std::string cam_name = "prosilica";
00569     cam_name += hw_id_;
00570     std::stringstream ini_stream;
00571     if (!camera_calibration_parsers::writeCalibrationIni(ini_stream, cam_name, info)) {
00572       rsp.status_message = "Error formatting camera_info for storage.";
00573       rsp.success = false;
00574     }
00575     else {
00576       std::string ini = ini_stream.str();
00577       if (ini.size() > prosilica::Camera::USER_MEMORY_SIZE) {
00578         rsp.success = false;
00579         rsp.status_message = "Unable to write camera_info to camera memory, exceeded storage capacity.";
00580       }
00581       else {
00582         try {
00583           cam_->writeUserMemory(ini.c_str(), ini.size());
00584           cam_info_ = info;
00585           rsp.success = true;
00586         }
00587         catch (prosilica::ProsilicaException &e) {
00588           rsp.success = false;
00589           rsp.status_message = e.what();
00590         }
00591       }
00592     }
00593     if (!rsp.success)
00594       ROS_ERROR("%s", rsp.status_message.c_str());
00595     
00596     start();
00597 
00598     return true;
00599   }
00600 
00601   void normalizeCallback(tPvFrame* frame)
00602   {
00603     unsigned long exposure;
00604     cam_->getAttribute("ExposureValue", exposure);
00605     //ROS_WARN("Exposure value = %u", exposure);
00606 
00607     if (exposure == last_exposure_value_)
00608       consecutive_stable_exposures_++;
00609     else {
00610       last_exposure_value_ = exposure;
00611       consecutive_stable_exposures_ = 0;
00612     }
00613   }
00614   
00615   void normalizeExposure()
00616   {
00617     ROS_INFO("Normalizing exposure");
00619 
00620     last_exposure_value_ = 0;
00621     consecutive_stable_exposures_ = 0;
00622     cam_->setFrameCallback(boost::bind(&ProsilicaNode::normalizeCallback, this, _1));
00623     cam_->start(prosilica::Freerun);
00624 
00626     while (consecutive_stable_exposures_ < 3)
00627       boost::this_thread::sleep(boost::posix_time::millisec(250));
00628 
00629     cam_->stop();
00630   }
00631 
00633   // Diagnostics //
00635   
00636   void runDiagnostics()
00637   {
00638     self_test_->checkTest();
00639     diagnostic_.update();
00640   }
00641   
00642   void freqStatus(diagnostic_updater::DiagnosticStatusWrapper& status)
00643   {
00644     double freq = (double)(count_)/diagnostic_.getPeriod();
00645 
00646     if (freq < (.9*desired_freq_))
00647     {
00648       status.summary(2, "Desired frequency not met");
00649     }
00650     else
00651     {
00652       status.summary(0, "Desired frequency met");
00653     }
00654 
00655     status.add("Images in interval", count_);
00656     status.add("Desired frequency", desired_freq_);
00657     status.add("Actual frequency", freq);
00658     
00659     count_ = 0;
00660   }
00661 
00662   void frameStatistics(diagnostic_updater::DiagnosticStatusWrapper& status)
00663   {
00664     // Get stats from camera driver
00665     float frame_rate;
00666     unsigned long completed, dropped;
00667     cam_->getAttribute("StatFrameRate", frame_rate);
00668     cam_->getAttribute("StatFramesCompleted", completed);
00669     cam_->getAttribute("StatFramesDropped", dropped);
00670 
00671     // Compute rolling totals, percentages
00672     frames_completed_acc_.add(completed - frames_completed_total_);
00673     frames_completed_total_ = completed;
00674     unsigned long completed_recent = frames_completed_acc_.sum();
00675     
00676     frames_dropped_acc_.add(dropped - frames_dropped_total_);
00677     frames_dropped_total_ = dropped;
00678     unsigned long dropped_recent = frames_dropped_acc_.sum();
00679 
00680     float recent_ratio = float(completed_recent) / (completed_recent + dropped_recent);
00681     float total_ratio = float(completed) / (completed + dropped);
00682 
00683     // Set level based on recent % completed frames
00684     if (dropped_recent == 0) {
00685       status.summary(0, "No dropped frames");
00686     }
00687     else if (recent_ratio > 0.8f) {
00688       status.summary(1, "Some dropped frames");
00689     }
00690     else {
00691       status.summary(2, "Excessive proportion of dropped frames");
00692     }
00693 
00694     status.add("Camera Frame Rate", frame_rate);
00695     status.add("Recent % Frames Completed", recent_ratio * 100.0f);
00696     status.add("Overall % Frames Completed", total_ratio * 100.0f);
00697     status.add("Frames Completed", completed);
00698     status.add("Frames Dropped", dropped);
00699   }
00700 
00701   void packetStatistics(diagnostic_updater::DiagnosticStatusWrapper& status)
00702   {
00703     // Get stats from camera driver
00704     unsigned long received, missed, requested, resent;
00705     cam_->getAttribute("StatPacketsReceived", received);
00706     cam_->getAttribute("StatPacketsMissed", missed);
00707     cam_->getAttribute("StatPacketsRequested", requested);
00708     cam_->getAttribute("StatPacketsResent", resent);
00709 
00710     // Compute rolling totals, percentages
00711     packets_received_acc_.add(received - packets_received_total_);
00712     packets_received_total_ = received;
00713     unsigned long received_recent = packets_received_acc_.sum();
00714     
00715     packets_missed_acc_.add(missed - packets_missed_total_);
00716     packets_missed_total_ = missed;
00717     unsigned long missed_recent = packets_missed_acc_.sum();
00718 
00719     float recent_ratio = float(received_recent) / (received_recent + missed_recent);
00720     float total_ratio = float(received) / (received + missed);
00721 
00722     if (missed_recent == 0) {
00723       status.summary(0, "No missed packets");
00724     }
00725     else if (recent_ratio > 0.99f) {
00726       status.summary(1, "Some missed packets");
00727     }
00728     else {
00729       status.summary(2, "Excessive proportion of missed packets");
00730     }
00731 
00732     // Adjust data rate
00733     unsigned long data_rate = 0;
00734     cam_->getAttribute("StreamBytesPerSecond", data_rate);
00735     unsigned long max_data_rate = cam_->getMaxDataRate();
00736     
00737     if (auto_adjust_stream_bytes_per_second_)
00738     {
00739       if (max_data_rate < prosilica::Camera::GIGE_MAX_DATA_RATE)
00740         status.mergeSummary(1, "Max data rate is lower than expected for a GigE port");
00741 
00742       try
00743       {
00745         float multiplier = 1.0f;
00746         if (recent_ratio == 1.0f)
00747         {
00748           multiplier = 1.1f;
00749             }
00750         else if (recent_ratio < 0.99f)
00751         {
00752           multiplier = 0.9f;
00753         }
00754         if (multiplier != 1.0f)
00755         {
00756           unsigned long new_data_rate = std::min((unsigned long)(multiplier * data_rate + 0.5), max_data_rate);
00757           new_data_rate = std::max(new_data_rate, max_data_rate/1000);
00758           if (data_rate != new_data_rate)
00759           {
00760             data_rate = new_data_rate;
00761             cam_->setAttribute("StreamBytesPerSecond", data_rate);
00762             ROS_DEBUG("Changed data rate to %lu bytes per second", data_rate);
00763           }
00764         }
00765       }
00766       catch (prosilica::ProsilicaException &e)
00767       {
00768         if (e.error_code == ePvErrUnplugged)
00769           throw;
00770         ROS_ERROR("Exception occurred: '%s'\n"
00771                   "Possible network issue. Attempting to reset data rate to the current maximum.",
00772                   e.what());
00773         data_rate = max_data_rate;
00774         cam_->setAttribute("StreamBytesPerSecond", data_rate);
00775       }
00776     }
00777     
00778     status.add("Recent % Packets Received", recent_ratio * 100.0f);
00779     status.add("Overall % Packets Received", total_ratio * 100.0f);
00780     status.add("Received Packets", received);
00781     status.add("Missed Packets", missed);
00782     status.add("Requested Packets", requested);
00783     status.add("Resent Packets", resent);
00784     status.add("Data Rate (bytes/s)", data_rate);
00785     status.add("Max Data Rate (bytes/s)", max_data_rate);
00786   }
00787 
00788   void packetErrorStatus(diagnostic_updater::DiagnosticStatusWrapper& status)
00789   {
00790     unsigned long erroneous;
00791     cam_->getAttribute("StatPacketsErroneous", erroneous);
00792 
00793     if (erroneous == 0) {
00794       status.summary(0, "No erroneous packets");
00795     } else {
00796       status.summary(2, "Possible camera hardware failure");
00797     }
00798 
00799     status.add("Erroneous Packets", erroneous);
00800   }
00801 
00803   // Self tests //
00805 
00806   // Try to load camera name, etc. Should catch gross communication failure.
00807   void infoTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00808   {
00809     status.name = "Info Test";
00810 
00811     self_test_->setID(hw_id_);
00812 
00813     std::string camera_name;
00814     try {
00815       cam_->getAttribute("CameraName", camera_name);
00816       status.summary(0, "Connected to Camera");
00817       status.add("Camera Name", camera_name);
00818     }
00819     catch (prosilica::ProsilicaException& e) {
00820       status.summary(2, e.what());
00821     }
00822   }
00823 
00824   // Test validity of all attribute values.
00825   void attributeTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00826   {
00827     status.name = "Attribute Test";
00828 
00829     tPvAttrListPtr list_ptr;
00830     unsigned long list_length;
00831 
00832     if (PvAttrList(cam_->handle(), &list_ptr, &list_length) == ePvErrSuccess) {
00833       status.summary(0, "All attributes in valid range");
00834       for (unsigned int i = 0; i < list_length; ++i) {
00835         const char* attribute = list_ptr[i];
00836         tPvErr e = PvAttrIsValid(cam_->handle(), attribute);
00837         if (e != ePvErrSuccess) {
00838           status.summary(2, "One or more invalid attributes");
00839           if (e == ePvErrOutOfRange)
00840             status.add(attribute, "Out of range");
00841           else if (e == ePvErrNotFound)
00842             status.add(attribute, "Does not exist");
00843           else
00844             status.addf(attribute, "Unexpected error code %u", e);
00845         }
00846       }
00847     }
00848     else {
00849       status.summary(2, "Unable to retrieve attribute list");
00850     }
00851   }
00852 
00853   // Try to capture an image.
00854   void imageTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00855   {
00856     status.name = "Image Capture Test";
00857 
00858     try {
00859       if (trigger_mode_ != prosilica::Software) {
00860         tPvUint32 start_completed, current_completed;
00861         cam_->getAttribute("StatFramesCompleted", start_completed);
00862         for (int i = 0; i < 6; ++i) {
00863           boost::this_thread::sleep(boost::posix_time::millisec(500));
00864           cam_->getAttribute("StatFramesCompleted", current_completed);
00865           if (current_completed > start_completed) {
00866             status.summary(0, "Captured a frame, see diagnostics for detailed stats");
00867             return;
00868           }
00869         }
00870 
00871         // Give up after 3s
00872         status.summary(2, "No frames captured over 3s in freerun mode");
00873         return;
00874       }
00875 
00876       // In software triggered mode, try to grab a frame
00877       cam_->setRoiToWholeFrame();
00878       tPvFrame* frame = cam_->grab(500);
00879       if (frame)
00880         {
00881           status.summary(0, "Successfully triggered a frame capture");
00882         }
00883       else
00884         {
00885           status.summary(2, "Attempted to grab a frame, but received NULL");
00886         }
00887         
00888     }
00889     catch (prosilica::ProsilicaException &e) {
00890       status.summary(2, e.what());
00891     }
00892   }
00893 };
00894 
00895 int main(int argc, char **argv)
00896 {
00897   ros::init(argc, argv, "prosilica_driver");
00898 
00899   ros::NodeHandle nh("camera");
00900   ProsilicaNode pn(nh);
00901   ros::spin();
00902 
00903   return 0;
00904 }


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