ueye_cam_nodelet.cpp
Go to the documentation of this file.
00001 /*******************************************************************************
00002 * DO NOT MODIFY - AUTO-GENERATED
00003 *
00004 *
00005 * DISCLAMER:
00006 *
00007 * This project was created within an academic research setting, and thus should
00008 * be considered as EXPERIMENTAL code. There may be bugs and deficiencies in the
00009 * code, so please adjust expectations accordingly. With that said, we are
00010 * intrinsically motivated to ensure its correctness (and often its performance).
00011 * Please use the corresponding web repository tool (e.g. github/bitbucket/etc.)
00012 * to file bugs, suggestions, pull requests; we will do our best to address them
00013 * in a timely manner.
00014 *
00015 *
00016 * SOFTWARE LICENSE AGREEMENT (BSD LICENSE):
00017 *
00018 * Copyright (c) 2013-2016, Anqi Xu and contributors
00019 * All rights reserved.
00020 *
00021 * Redistribution and use in source and binary forms, with or without
00022 * modification, are permitted provided that the following conditions
00023 * are met:
00024 *
00025 *  * Redistributions of source code must retain the above copyright
00026 *    notice, this list of conditions and the following disclaimer.
00027 *  * Redistributions in binary form must reproduce the above
00028 *    copyright notice, this list of conditions and the following
00029 *    disclaimer in the documentation and/or other materials provided
00030 *    with the distribution.
00031 *  * Neither the name of the School of Computer Science, McGill University,
00032 *    nor the names of its contributors may be used to endorse or promote
00033 *    products derived from this software without specific prior written
00034 *    permission.
00035 *
00036 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00037 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00038 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00039 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
00040 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00041 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00042 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00043 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00044 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00045 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00046 *******************************************************************************/
00047 
00048 #include "ueye_cam/ueye_cam_nodelet.hpp"
00049 #include <cstdlib> // needed for getenv()
00050 #include <ros/package.h>
00051 #include <camera_calibration_parsers/parse.h>
00052 #include <std_msgs/UInt64.h>
00053 #include <sensor_msgs/fill_image.h>
00054 #include <sensor_msgs/image_encodings.h>
00055 
00056 
00057 //#define DEBUG_PRINTOUT_FRAME_GRAB_RATES
00058 
00059 
00060 using namespace std;
00061 using namespace sensor_msgs::image_encodings;
00062 
00063 
00064 namespace ueye_cam {
00065 
00066 
00067 const std::string UEyeCamNodelet::DEFAULT_FRAME_NAME = "camera";
00068 const std::string UEyeCamNodelet::DEFAULT_CAMERA_NAME = "camera";
00069 const std::string UEyeCamNodelet::DEFAULT_CAMERA_TOPIC = "image_raw";
00070 const std::string UEyeCamNodelet::DEFAULT_TIMEOUT_TOPIC = "timeout_count";
00071 const std::string UEyeCamNodelet::DEFAULT_COLOR_MODE = "";
00072 constexpr int UEyeCamDriver::ANY_CAMERA; // Needed since CMakeLists.txt creates 2 separate libraries: one for non-ROS parent class, and one for ROS child class
00073 
00074 
00075 // Note that these default settings will be overwritten by queryCamParams() during connectCam()
00076 UEyeCamNodelet::UEyeCamNodelet():
00077     nodelet::Nodelet(),
00078     UEyeCamDriver(ANY_CAMERA, DEFAULT_CAMERA_NAME),
00079     frame_grab_alive_(false),
00080     ros_cfg_(NULL),
00081     cfg_sync_requested_(false),
00082     ros_frame_count_(0),
00083     timeout_count_(0),
00084     cam_topic_(DEFAULT_CAMERA_TOPIC),
00085     timeout_topic_(DEFAULT_TIMEOUT_TOPIC),
00086     cam_intr_filename_(""),
00087     cam_params_filename_(""),
00088     init_clock_tick_(0),
00089     init_publish_time_(0),
00090     prev_output_frame_idx_(0) {
00091   ros_image_.is_bigendian = (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__); // TODO: what about MS Windows?
00092   cam_params_.image_width = DEFAULT_IMAGE_WIDTH;
00093   cam_params_.image_height = DEFAULT_IMAGE_HEIGHT;
00094   cam_params_.image_left = -1;
00095   cam_params_.image_top = -1;
00096   cam_params_.color_mode = DEFAULT_COLOR_MODE;
00097   cam_params_.subsampling = cam_subsampling_rate_;
00098   cam_params_.binning = cam_binning_rate_;
00099   cam_params_.sensor_scaling = cam_sensor_scaling_rate_;
00100   cam_params_.auto_gain = false;
00101   cam_params_.master_gain = 0;
00102   cam_params_.red_gain = 0;
00103   cam_params_.green_gain = 0;
00104   cam_params_.blue_gain = 0;
00105   cam_params_.gain_boost = 0;
00106   cam_params_.auto_exposure = false;
00107   cam_params_.exposure = DEFAULT_EXPOSURE;
00108   cam_params_.auto_white_balance = false;
00109   cam_params_.white_balance_red_offset = 0;
00110   cam_params_.white_balance_blue_offset = 0;
00111   cam_params_.auto_frame_rate = false;
00112   cam_params_.frame_rate = DEFAULT_FRAME_RATE;
00113   cam_params_.output_rate = 0; // disable by default
00114   cam_params_.pixel_clock = DEFAULT_PIXEL_CLOCK;
00115   cam_params_.ext_trigger_mode = false;
00116   cam_params_.flash_delay = 0;
00117   cam_params_.flash_duration = DEFAULT_FLASH_DURATION;
00118   cam_params_.flip_upd = false;
00119   cam_params_.flip_lr = false;
00120 }
00121 
00122 
00123 UEyeCamNodelet::~UEyeCamNodelet() {
00124   disconnectCam();
00125 
00126   // NOTE: sometimes deleting dynamic reconfigure object will lock up
00127   //       (suspect the scoped lock is not releasing the recursive mutex)
00128   //
00129   //if (ros_cfg_ != NULL) {
00130   //  delete ros_cfg_;
00131   //  ros_cfg_ = NULL;
00132   //}
00133 }
00134 
00135 
00136 void UEyeCamNodelet::onInit() {
00137   ros::NodeHandle& nh = getNodeHandle();
00138   ros::NodeHandle& local_nh = getPrivateNodeHandle();
00139   image_transport::ImageTransport it(nh);
00140 
00141   // Load camera-agnostic ROS parameters
00142   local_nh.param<string>("camera_name", cam_name_, DEFAULT_CAMERA_NAME);
00143   local_nh.param<string>("frame_name", frame_name_, DEFAULT_FRAME_NAME);
00144   local_nh.param<string>("camera_topic", cam_topic_, DEFAULT_CAMERA_TOPIC);
00145   local_nh.param<string>("timeout_topic", timeout_topic_, DEFAULT_TIMEOUT_TOPIC);
00146   local_nh.param<string>("camera_intrinsics_file", cam_intr_filename_, "");
00147   local_nh.param<int>("camera_id", cam_id_, ANY_CAMERA);
00148   local_nh.param<string>("camera_parameters_file", cam_params_filename_, "");
00149   if (cam_id_ < 0) {
00150     WARN_STREAM("Invalid camera ID specified: " << cam_id_ <<
00151       "; setting to ANY_CAMERA");
00152     cam_id_ = ANY_CAMERA;
00153   }
00154 
00155   loadIntrinsicsFile();
00156 
00157   // Setup dynamic reconfigure server
00158   ros_cfg_ = new ReconfigureServer(ros_cfg_mutex_, local_nh);
00159   ReconfigureServer::CallbackType f;
00160   f = bind(&UEyeCamNodelet::configCallback, this, _1, _2);
00161 
00162   // Setup publishers, subscribers, and services
00163   ros_cam_pub_ = it.advertiseCamera(cam_name_ + "/" + cam_topic_, 1);
00164   set_cam_info_srv_ = nh.advertiseService(cam_name_ + "/set_camera_info",
00165       &UEyeCamNodelet::setCamInfo, this);
00166   timeout_pub_ = nh.advertise<std_msgs::UInt64>(cam_name_ + "/" + timeout_topic_, 1, true);
00167   std_msgs::UInt64 timeout_msg; timeout_msg.data = 0; timeout_pub_.publish(timeout_msg);
00168 
00169   // Initiate camera and start capture
00170   if (connectCam() != IS_SUCCESS) {
00171     ERROR_STREAM("Failed to initialize [" << cam_name_ << "]");
00172     return;
00173   }
00174 
00175   ros_cfg_->setCallback(f); // this will call configCallback, which will configure the camera's parameters
00176   startFrameGrabber();
00177   INFO_STREAM(
00178       "UEye camera [" << cam_name_ << "] initialized on topic " << ros_cam_pub_.getTopic() << endl <<
00179       "Width:\t\t\t" << cam_params_.image_width << endl <<
00180       "Height:\t\t\t" << cam_params_.image_height << endl <<
00181       "Left Pos.:\t\t" << cam_params_.image_left << endl <<
00182       "Top Pos.:\t\t" << cam_params_.image_top << endl <<
00183       "Color Mode:\t\t" << cam_params_.color_mode << endl <<
00184       "Subsampling:\t\t" << cam_params_.subsampling << endl <<
00185       "Binning:\t\t" << cam_params_.binning << endl <<
00186       "Sensor Scaling:\t\t" << cam_params_.sensor_scaling << endl <<
00187       "Auto Gain:\t\t" << cam_params_.auto_gain << endl <<
00188       "Master Gain:\t\t" << cam_params_.master_gain << endl <<
00189       "Red Gain:\t\t" << cam_params_.red_gain << endl <<
00190       "Green Gain:\t\t" << cam_params_.green_gain << endl <<
00191       "Blue Gain:\t\t" << cam_params_.blue_gain << endl <<
00192       "Gain Boost:\t\t" << cam_params_.gain_boost << endl <<
00193       "Auto Exposure:\t\t" << cam_params_.auto_exposure << endl <<
00194       "Exposure (ms):\t\t" << cam_params_.exposure << endl <<
00195       "Auto White Balance:\t" << cam_params_.auto_white_balance << endl <<
00196       "WB Red Offset:\t\t" << cam_params_.white_balance_red_offset << endl <<
00197       "WB Blue Offset:\t\t" << cam_params_.white_balance_blue_offset << endl <<
00198       "Flash Delay (us):\t" << cam_params_.flash_delay << endl <<
00199       "Flash Duration (us):\t" << cam_params_.flash_duration << endl <<
00200       "Ext Trigger Mode:\t" << cam_params_.ext_trigger_mode << endl <<
00201       "Auto Frame Rate:\t" << cam_params_.auto_frame_rate << endl <<
00202       "Frame Rate (Hz):\t" << cam_params_.frame_rate << endl <<
00203       "Output Rate (Hz):\t" << cam_params_.output_rate << endl <<
00204       "Pixel Clock (MHz):\t" << cam_params_.pixel_clock << endl <<
00205       "Mirror Image Upside Down:\t" << cam_params_.flip_upd << endl <<
00206       "Mirror Image Left Right:\t" << cam_params_.flip_lr << endl
00207       );
00208 }
00209 
00210 
00211 INT UEyeCamNodelet::parseROSParams(ros::NodeHandle& local_nh) {
00212   bool hasNewParams = false;
00213   ueye_cam::UEyeCamConfig prevCamParams = cam_params_;
00214   INT is_err = IS_SUCCESS;
00215 
00216   if (local_nh.hasParam("image_width")) {
00217     local_nh.getParam("image_width", cam_params_.image_width);
00218     if (cam_params_.image_width != prevCamParams.image_width) {
00219       if (cam_params_.image_width <= 0) {
00220         WARN_STREAM("Invalid requested image width for [" << cam_name_ <<
00221           "]: " << cam_params_.image_width <<
00222           "; using current width: " << prevCamParams.image_width);
00223         cam_params_.image_width = prevCamParams.image_width;
00224       } else {
00225         hasNewParams = true;
00226       }
00227     }
00228   }
00229   if (local_nh.hasParam("image_height")) {
00230     local_nh.getParam("image_height", cam_params_.image_height);
00231     if (cam_params_.image_height != prevCamParams.image_height) {
00232       if (cam_params_.image_height <= 0) {
00233         WARN_STREAM("Invalid requested image height for [" << cam_name_ <<
00234           "]: " << cam_params_.image_height <<
00235           "; using current height: " << prevCamParams.image_height);
00236         cam_params_.image_height = prevCamParams.image_height;
00237       } else {
00238         hasNewParams = true;
00239       }
00240     }
00241   }
00242   if (local_nh.hasParam("image_top")) {
00243     local_nh.getParam("image_top", cam_params_.image_top);
00244     if (cam_params_.image_top != prevCamParams.image_top) {
00245       hasNewParams = true;
00246     }
00247   }
00248   if (local_nh.hasParam("image_left")) {
00249     local_nh.getParam("image_left", cam_params_.image_left);
00250     if (cam_params_.image_left != prevCamParams.image_left) {
00251       hasNewParams = true;
00252     }
00253   }
00254   if (local_nh.hasParam("color_mode")) {
00255     local_nh.getParam("color_mode", cam_params_.color_mode);
00256     if (cam_params_.color_mode != prevCamParams.color_mode) {
00257       if (cam_params_.color_mode.length() > 0) {
00258         transform(cam_params_.color_mode.begin(),
00259             cam_params_.color_mode.end(),
00260             cam_params_.color_mode.begin(),
00261             ::tolower);
00262         if (name2colormode(cam_params_.color_mode) != 0) {
00263           hasNewParams = true;
00264         } else {
00265           WARN_STREAM("Invalid requested color mode for [" << cam_name_
00266             << "]: " << cam_params_.color_mode
00267             << "; using current mode: " << prevCamParams.color_mode);
00268           cam_params_.color_mode = prevCamParams.color_mode;
00269         }
00270       } else { // Empty requested color mode string
00271         cam_params_.color_mode = prevCamParams.color_mode;
00272       }
00273     }
00274   }
00275   if (local_nh.hasParam("subsampling")) {
00276     local_nh.getParam("subsampling", cam_params_.subsampling);
00277     if (cam_params_.subsampling != prevCamParams.subsampling) {
00278       if (!(cam_params_.subsampling == 1 ||
00279           cam_params_.subsampling == 2 ||
00280           cam_params_.subsampling == 4 ||
00281           cam_params_.subsampling == 8 ||
00282           cam_params_.subsampling == 16)) {
00283         WARN_STREAM("Invalid or unsupported requested subsampling rate for [" <<
00284           cam_name_ << "]: " << cam_params_.subsampling <<
00285           "; using current rate: " << prevCamParams.subsampling);
00286         cam_params_.subsampling = prevCamParams.subsampling;
00287       } else {
00288         hasNewParams = true;
00289       }
00290     }
00291   }
00292   if (local_nh.hasParam("auto_gain")) {
00293     local_nh.getParam("auto_gain", cam_params_.auto_gain);
00294     if (cam_params_.auto_gain != prevCamParams.auto_gain) {
00295       hasNewParams = true;
00296     }
00297   }
00298   if (local_nh.hasParam("master_gain")) {
00299     local_nh.getParam("master_gain", cam_params_.master_gain);
00300     if (cam_params_.master_gain != prevCamParams.master_gain) {
00301       if (cam_params_.master_gain < 0 || cam_params_.master_gain > 100) {
00302         WARN_STREAM("Invalid master gain for [" << cam_name_ << "]: " <<
00303           cam_params_.master_gain << "; using current master gain: " << prevCamParams.master_gain);
00304         cam_params_.master_gain = prevCamParams.master_gain;
00305       } else {
00306         hasNewParams = true;
00307       }
00308     }
00309   }
00310   if (local_nh.hasParam("red_gain")) {
00311     local_nh.getParam("red_gain", cam_params_.red_gain);
00312     if (cam_params_.red_gain != prevCamParams.red_gain) {
00313       if (cam_params_.red_gain < 0 || cam_params_.red_gain > 100) {
00314         WARN_STREAM("Invalid red gain for [" << cam_name_ << "]: " <<
00315           cam_params_.red_gain << "; using current red gain: " << prevCamParams.red_gain);
00316         cam_params_.red_gain = prevCamParams.red_gain;
00317       } else {
00318         hasNewParams = true;
00319       }
00320     }
00321   }
00322   if (local_nh.hasParam("green_gain")) {
00323     local_nh.getParam("green_gain", cam_params_.green_gain);
00324     if (cam_params_.green_gain != prevCamParams.green_gain) {
00325       if (cam_params_.green_gain < 0 || cam_params_.green_gain > 100) {
00326         WARN_STREAM("Invalid green gain for [" << cam_name_ << "]: " <<
00327           cam_params_.green_gain << "; using current green gain: " << prevCamParams.green_gain);
00328         cam_params_.green_gain = prevCamParams.green_gain;
00329       } else {
00330         hasNewParams = true;
00331       }
00332     }
00333   }
00334   if (local_nh.hasParam("blue_gain")) {
00335     local_nh.getParam("blue_gain", cam_params_.blue_gain);
00336     if (cam_params_.blue_gain != prevCamParams.blue_gain) {
00337       if (cam_params_.blue_gain < 0 || cam_params_.blue_gain > 100) {
00338         WARN_STREAM("Invalid blue gain for [" << cam_name_ << "]: " <<
00339           cam_params_.blue_gain << "; using current blue gain: " << prevCamParams.blue_gain);
00340         cam_params_.blue_gain = prevCamParams.blue_gain;
00341       } else {
00342         hasNewParams = true;
00343       }
00344     }
00345   }
00346   if (local_nh.hasParam("gain_boost")) {
00347     local_nh.getParam("gain_boost", cam_params_.gain_boost);
00348     if (cam_params_.gain_boost != prevCamParams.gain_boost) {
00349       hasNewParams = true;
00350     }
00351   }
00352   if (local_nh.hasParam("auto_exposure")) {
00353     local_nh.getParam("auto_exposure", cam_params_.auto_exposure);
00354     if (cam_params_.auto_exposure != prevCamParams.auto_exposure) {
00355       hasNewParams = true;
00356     }
00357   }
00358   if (local_nh.hasParam("exposure")) {
00359     local_nh.getParam("exposure", cam_params_.exposure);
00360     if (cam_params_.exposure != prevCamParams.exposure) {
00361       if (cam_params_.exposure < 0.0) {
00362         WARN_STREAM("Invalid requested exposure: " << cam_params_.exposure <<
00363           "; using current exposure: " << prevCamParams.exposure);
00364         cam_params_.exposure = prevCamParams.exposure;
00365       } else {
00366         hasNewParams = true;
00367       }
00368     }
00369   }
00370   if (local_nh.hasParam("auto_white_balance")) {
00371     local_nh.getParam("auto_white_balance", cam_params_.auto_white_balance);
00372     if (cam_params_.auto_white_balance != prevCamParams.auto_white_balance) {
00373       hasNewParams = true;
00374     }
00375   }
00376   if (local_nh.hasParam("white_balance_red_offset")) {
00377     local_nh.getParam("white_balance_red_offset", cam_params_.white_balance_red_offset);
00378     if (cam_params_.white_balance_red_offset != prevCamParams.white_balance_red_offset) {
00379       if (cam_params_.white_balance_red_offset < -50 || cam_params_.white_balance_red_offset > 50) {
00380         WARN_STREAM("Invalid white balance red offset for [" << cam_name_ << "]: " <<
00381           cam_params_.white_balance_red_offset <<
00382           "; using current white balance red offset: " << prevCamParams.white_balance_red_offset);
00383         cam_params_.white_balance_red_offset = prevCamParams.white_balance_red_offset;
00384       } else {
00385         hasNewParams = true;
00386       }
00387     }
00388   }
00389   if (local_nh.hasParam("white_balance_blue_offset")) {
00390     local_nh.getParam("white_balance_blue_offset", cam_params_.white_balance_blue_offset);
00391     if (cam_params_.white_balance_blue_offset != prevCamParams.white_balance_blue_offset) {
00392       if (cam_params_.white_balance_blue_offset < -50 || cam_params_.white_balance_blue_offset > 50) {
00393         WARN_STREAM("Invalid white balance blue offset for [" << cam_name_ << "]: " <<
00394           cam_params_.white_balance_blue_offset <<
00395           "; using current white balance blue offset: " << prevCamParams.white_balance_blue_offset);
00396         cam_params_.white_balance_blue_offset = prevCamParams.white_balance_blue_offset;
00397       } else {
00398         hasNewParams = true;
00399       }
00400     }
00401   }
00402   if (local_nh.hasParam("ext_trigger_mode")) {
00403     local_nh.getParam("ext_trigger_mode", cam_params_.ext_trigger_mode);
00404     // NOTE: no need to set any parameters, since external trigger / live-run
00405     //       modes come into effect during frame grab loop, which is assumed
00406     //       to not having been initialized yet
00407   }
00408   if (local_nh.hasParam("flash_delay")) {
00409     local_nh.getParam("flash_delay", cam_params_.flash_delay);
00410     // NOTE: no need to set any parameters, since flash delay comes into
00411     //       effect during frame grab loop, which is assumed to not having been
00412     //       initialized yet
00413   }
00414   if (local_nh.hasParam("flash_duration")) {
00415     local_nh.getParam("flash_duration", cam_params_.flash_duration);
00416     if (cam_params_.flash_duration < 0) {
00417       WARN_STREAM("Invalid flash duration for [" << cam_name_ << "]: " <<
00418         cam_params_.flash_duration <<
00419         "; using current flash duration: " << prevCamParams.flash_duration);
00420       cam_params_.flash_duration = prevCamParams.flash_duration;
00421     }
00422     // NOTE: no need to set any parameters, since flash duration comes into
00423     //       effect during frame grab loop, which is assumed to not having been
00424     //       initialized yet
00425   }
00426   if (local_nh.hasParam("auto_frame_rate")) {
00427     local_nh.getParam("auto_frame_rate", cam_params_.auto_frame_rate);
00428     if (cam_params_.auto_frame_rate != prevCamParams.auto_frame_rate) {
00429       hasNewParams = true;
00430     }
00431   }
00432   if (local_nh.hasParam("frame_rate")) {
00433     local_nh.getParam("frame_rate", cam_params_.frame_rate);
00434     if (cam_params_.frame_rate != prevCamParams.frame_rate) {
00435       if (cam_params_.frame_rate <= 0.0) {
00436         WARN_STREAM("Invalid requested frame rate for [" << cam_name_ << "]: " <<
00437           cam_params_.frame_rate <<
00438           "; using current frame rate: " << prevCamParams.frame_rate);
00439         cam_params_.frame_rate = prevCamParams.frame_rate;
00440       } else {
00441         hasNewParams = true;
00442       }
00443     }
00444   }
00445   if (local_nh.hasParam("output_rate")) {
00446     local_nh.getParam("output_rate", cam_params_.output_rate);
00447     if (cam_params_.output_rate < 0.0) {
00448       WARN_STREAM("Invalid requested output rate for [" << cam_name_ << "]: " <<
00449         cam_params_.output_rate <<
00450         "; disable publisher throttling by default");
00451       cam_params_.output_rate = 0;
00452     } else {
00453       cam_params_.output_rate = std::min(cam_params_.frame_rate, cam_params_.output_rate);
00454       // hasNewParams = true; // No need to re-allocate buffer memory or reconfigure camera parameters
00455     }
00456   }
00457   if (local_nh.hasParam("pixel_clock")) {
00458     local_nh.getParam("pixel_clock", cam_params_.pixel_clock);
00459     if (cam_params_.pixel_clock != prevCamParams.pixel_clock) {
00460       if (cam_params_.pixel_clock < 0) {
00461         WARN_STREAM("Invalid requested pixel clock for [" << cam_name_ << "]: " <<
00462           cam_params_.pixel_clock <<
00463           "; using current pixel clock: " << prevCamParams.pixel_clock);
00464         cam_params_.pixel_clock = prevCamParams.pixel_clock;
00465       } else {
00466         hasNewParams = true;
00467       }
00468     }
00469   }
00470   if (local_nh.hasParam("flip_upd")) {
00471     local_nh.getParam("flip_upd", cam_params_.flip_upd);
00472     if (cam_params_.flip_upd != prevCamParams.flip_upd) {
00473       hasNewParams = true;
00474     }
00475   }
00476   if (local_nh.hasParam("flip_lr")) {
00477     local_nh.getParam("flip_lr", cam_params_.flip_lr);
00478     if (cam_params_.flip_lr != prevCamParams.flip_lr) {
00479       hasNewParams = true;
00480     }
00481   }
00482 
00483   if (hasNewParams) {
00484     // Configure color mode, resolution, and subsampling rate
00485     // NOTE: this batch of configurations are mandatory, to ensure proper allocation of local frame buffer
00486     if ((is_err = setColorMode(cam_params_.color_mode, false)) != IS_SUCCESS) return is_err;
00487     if ((is_err = setSubsampling(cam_params_.subsampling, false)) != IS_SUCCESS) return is_err;
00488     if ((is_err = setBinning(cam_params_.binning, false)) != IS_SUCCESS) return is_err;
00489     if ((is_err = setResolution(cam_params_.image_width, cam_params_.image_height,
00490         cam_params_.image_left, cam_params_.image_top, false)) != IS_SUCCESS) return is_err;
00491     if ((is_err = setSensorScaling(cam_params_.sensor_scaling, false)) != IS_SUCCESS) return is_err;
00492 
00493     // Force synchronize settings and re-allocate frame buffer for redundancy
00494     // NOTE: although this might not be needed, assume that parseROSParams()
00495     //       is called only once per nodelet, thus ignore cost
00496     if ((is_err = syncCamConfig()) != IS_SUCCESS) return is_err;
00497 
00498     // Check for mutual exclusivity among requested sensor parameters
00499     if (!cam_params_.auto_exposure) { // Auto frame rate requires auto shutter
00500       cam_params_.auto_frame_rate = false;
00501     }
00502     if (cam_params_.auto_frame_rate) { // Auto frame rate has precedence over auto gain
00503       cam_params_.auto_gain = false;
00504     }
00505 
00506     // Configure camera sensor parameters
00507     // NOTE: failing to configure certain parameters may or may not cause camera to fail;
00508     //       cuurently their failures are treated as non-critical
00509     //#define noop return is_err
00510     #define noop (void)0
00511     if ((is_err = setGain(cam_params_.auto_gain, cam_params_.master_gain,
00512         cam_params_.red_gain, cam_params_.green_gain,
00513         cam_params_.blue_gain, cam_params_.gain_boost)) != IS_SUCCESS) noop;
00514     if ((is_err = setPixelClockRate(cam_params_.pixel_clock)) != IS_SUCCESS) return is_err;
00515     if ((is_err = setFrameRate(cam_params_.auto_frame_rate, cam_params_.frame_rate)) != IS_SUCCESS) return is_err;
00516     if ((is_err = setExposure(cam_params_.auto_exposure, cam_params_.exposure)) != IS_SUCCESS) noop;
00517     if ((is_err = setWhiteBalance(cam_params_.auto_white_balance, cam_params_.white_balance_red_offset,
00518       cam_params_.white_balance_blue_offset)) != IS_SUCCESS) noop;
00519     if ((is_err = setMirrorUpsideDown(cam_params_.flip_upd)) != IS_SUCCESS) noop;
00520     if ((is_err = setMirrorLeftRight(cam_params_.flip_lr)) != IS_SUCCESS) noop;
00521     #undef noop
00522   }
00523 
00524   DEBUG_STREAM("Successfully applied settings from ROS params to [" << cam_name_ << "]");
00525 
00526   return is_err;
00527 }
00528 
00529 
00530 void UEyeCamNodelet::configCallback(ueye_cam::UEyeCamConfig& config, uint32_t level) {
00531   if (!isConnected()) return;
00532 
00533   // See if frame grabber needs to be restarted
00534   bool restartFrameGrabber = false;
00535   bool needToReallocateBuffer = false;
00536   if (level == RECONFIGURE_STOP && frame_grab_alive_) {
00537     restartFrameGrabber = true;
00538     stopFrameGrabber();
00539   }
00540 
00541   // Configure color mode, resolution, and subsampling rate
00542   if (config.color_mode != cam_params_.color_mode) {
00543     needToReallocateBuffer = true;
00544     if (setColorMode(config.color_mode, false) != IS_SUCCESS) return;
00545   }
00546 
00547   if (config.image_width != cam_params_.image_width ||
00548       config.image_height != cam_params_.image_height ||
00549       config.image_left != cam_params_.image_left ||
00550       config.image_top != cam_params_.image_top) {
00551     needToReallocateBuffer = true;
00552     if (setResolution(config.image_width, config.image_height,
00553         config.image_left, config.image_top, false) != IS_SUCCESS) {
00554       // Attempt to restore previous (working) resolution
00555       config.image_width = cam_params_.image_width;
00556       config.image_height = cam_params_.image_height;
00557       config.image_left = cam_params_.image_left;
00558       config.image_top = cam_params_.image_top;
00559       if (setResolution(config.image_width, config.image_height,
00560           config.image_left, config.image_top, false) != IS_SUCCESS) return;
00561     }
00562   }
00563 
00564   if (config.subsampling != cam_params_.subsampling) {
00565     needToReallocateBuffer = true;
00566     if (setSubsampling(config.subsampling, false) != IS_SUCCESS) return;
00567   }
00568 
00569   if (config.binning != cam_params_.binning) {
00570     needToReallocateBuffer = true;
00571     if (setBinning(config.binning, false) != IS_SUCCESS) return;
00572   }
00573 
00574   if (config.sensor_scaling != cam_params_.sensor_scaling) {
00575     needToReallocateBuffer = true;
00576     if (setSensorScaling(config.sensor_scaling, false) != IS_SUCCESS) return;
00577   }
00578 
00579   // Reallocate internal camera buffer, and synchronize both non-ROS and ROS settings
00580   // for redundancy
00581   if (needToReallocateBuffer) {
00582     if (syncCamConfig() != IS_SUCCESS) return;
00583     needToReallocateBuffer = false;
00584   }
00585 
00586   // Check for mutual exclusivity among requested sensor parameters
00587   if (!config.auto_exposure) { // Auto frame rate requires auto shutter
00588     config.auto_frame_rate = false;
00589   }
00590   if (config.auto_frame_rate) { // Auto frame rate has precedence over auto gain
00591     config.auto_gain = false;
00592   }
00593 
00594   // Configure camera sensor parameters
00595   if (config.auto_gain != cam_params_.auto_gain ||
00596       config.master_gain != cam_params_.master_gain ||
00597       config.red_gain != cam_params_.red_gain ||
00598       config.green_gain != cam_params_.green_gain ||
00599       config.blue_gain != cam_params_.blue_gain ||
00600       config.gain_boost != cam_params_.gain_boost) {
00601     // If any of the manual gain params change, then automatically toggle off auto_gain
00602     if (config.master_gain != cam_params_.master_gain ||
00603         config.red_gain != cam_params_.red_gain ||
00604         config.green_gain != cam_params_.green_gain ||
00605         config.blue_gain != cam_params_.blue_gain ||
00606         config.gain_boost != cam_params_.gain_boost) {
00607       config.auto_gain = false;
00608     }
00609 
00610     if (setGain(config.auto_gain, config.master_gain,
00611         config.red_gain, config.green_gain,
00612         config.blue_gain, config.gain_boost) != IS_SUCCESS) return;
00613   }
00614 
00615   if (config.pixel_clock != cam_params_.pixel_clock) {
00616     if (setPixelClockRate(config.pixel_clock) != IS_SUCCESS) return;
00617   }
00618 
00619   if (config.auto_frame_rate != cam_params_.auto_frame_rate ||
00620       config.frame_rate != cam_params_.frame_rate) {
00621     if (setFrameRate(config.auto_frame_rate, config.frame_rate) != IS_SUCCESS) return;
00622   }
00623 
00624   if (config.output_rate != cam_params_.output_rate) {
00625     if (!config.auto_frame_rate) {
00626       config.output_rate = std::min(config.output_rate, config.frame_rate);
00627     } // else, auto-fps is enabled, so don't bother checking validity of user-specified config.output_rate
00628 
00629     // Reset reference time for publisher throttle
00630     output_rate_mutex_.lock();
00631     init_publish_time_ = ros::Time(0);
00632     prev_output_frame_idx_ = 0;
00633     output_rate_mutex_.unlock();
00634   }
00635 
00636   if (config.auto_exposure != cam_params_.auto_exposure ||
00637       config.exposure != cam_params_.exposure) {
00638     if (setExposure(config.auto_exposure, config.exposure) != IS_SUCCESS) return;
00639   }
00640 
00641   if (config.auto_white_balance != cam_params_.auto_white_balance ||
00642       config.white_balance_red_offset != cam_params_.white_balance_red_offset ||
00643       config.white_balance_blue_offset != cam_params_.white_balance_blue_offset) {
00644     if (setWhiteBalance(config.auto_white_balance, config.white_balance_red_offset,
00645         config.white_balance_blue_offset) != IS_SUCCESS) return;
00646   }
00647 
00648   if (config.flip_upd != cam_params_.flip_upd) {
00649     if (setMirrorUpsideDown(config.flip_upd) != IS_SUCCESS) return;
00650   }
00651   if (config.flip_lr != cam_params_.flip_lr) {
00652     if (setMirrorLeftRight(config.flip_lr) != IS_SUCCESS) return;
00653   }
00654 
00655   // NOTE: nothing needs to be done for config.ext_trigger_mode, since frame grabber loop will re-initialize to the right setting
00656 
00657   if (config.flash_delay != cam_params_.flash_delay ||
00658       config.flash_duration != cam_params_.flash_duration) {
00659     // NOTE: need to copy flash parameters to local copies since
00660     //       cam_params_.flash_duration is type int, and also sizeof(int)
00661     //       may not equal to sizeof(INT) / sizeof(UINT)
00662     INT flash_delay = config.flash_delay;
00663     UINT flash_duration = config.flash_duration;
00664     if (setFlashParams(flash_delay, flash_duration) != IS_SUCCESS) return;
00665     // Copy back actual flash parameter values that were set
00666     config.flash_delay = flash_delay;
00667     config.flash_duration = flash_duration;
00668   }
00669 
00670   // Update local copy of parameter set to newly updated set
00671   cam_params_ = config;
00672 
00673   // Restart frame grabber if needed
00674   cfg_sync_requested_ = true;
00675   if (restartFrameGrabber) {
00676     startFrameGrabber();
00677   }
00678 
00679   DEBUG_STREAM("Successfully applied settings from dyncfg to [" << cam_name_ << "]");
00680 }
00681 
00682 
00683 INT UEyeCamNodelet::syncCamConfig(string dft_mode_str) {
00684   INT is_err;
00685 
00686   if ((is_err = UEyeCamDriver::syncCamConfig(dft_mode_str)) != IS_SUCCESS) return is_err;
00687 
00688   // Update ROS color mode string
00689   cam_params_.color_mode = colormode2name(is_SetColorMode(cam_handle_, IS_GET_COLOR_MODE));
00690   if (cam_params_.color_mode.empty()) {
00691     ERROR_STREAM("Force-updating to default color mode for [" << cam_name_ << "]: " <<
00692       dft_mode_str << "\n(THIS IS A CODING ERROR, PLEASE CONTACT PACKAGE AUTHOR)");
00693     cam_params_.color_mode = dft_mode_str;
00694     setColorMode(cam_params_.color_mode);
00695   }
00696 
00697   // Copy internal settings to ROS dynamic configure settings
00698   cam_params_.image_width = cam_aoi_.s32Width;   // Technically, these are width and height for the
00699   cam_params_.image_height = cam_aoi_.s32Height; // sensor's Area of Interest, and not of the image
00700   if (cam_params_.image_left >= 0) cam_params_.image_left = cam_aoi_.s32X; // TODO: 1 ideally want to ensure that aoi top-left does correspond to centering
00701   if (cam_params_.image_top >= 0) cam_params_.image_top = cam_aoi_.s32Y;
00702   cam_params_.subsampling = cam_subsampling_rate_;
00703   cam_params_.binning = cam_binning_rate_;
00704   cam_params_.sensor_scaling = cam_sensor_scaling_rate_;
00705   //cfg_sync_requested_ = true; // WARNING: assume that dyncfg client may want to override current settings
00706 
00707   // (Re-)populate ROS image message
00708   ros_image_.header.frame_id = "/" + frame_name_;
00709   // NOTE: .height, .width, .encoding, .step and .data determined in fillImgMsg();
00710   //       .is_bigendian determined in constructor
00711 
00712   return is_err;
00713 }
00714 
00715 
00716 INT UEyeCamNodelet::queryCamParams() {
00717   INT is_err = IS_SUCCESS;
00718   INT query;
00719   double pval1, pval2;
00720 
00721   // NOTE: assume that color mode, bits per pixel, area of interest info, resolution,
00722   //       sensor scaling rate, subsampling rate, and binning rate have already
00723   //       been synchronized by syncCamConfig()
00724 
00725   if ((is_err = is_SetAutoParameter(cam_handle_,
00726       IS_GET_ENABLE_AUTO_SENSOR_GAIN, &pval1, &pval2)) != IS_SUCCESS &&
00727       (is_err = is_SetAutoParameter(cam_handle_,
00728           IS_GET_ENABLE_AUTO_GAIN, &pval1, &pval2)) != IS_SUCCESS) {
00729     ERROR_STREAM("Failed to query auto gain mode for UEye camera '" <<
00730         cam_name_ << "' (" << err2str(is_err) << ")");
00731     return is_err;
00732   }
00733   cam_params_.auto_gain = (pval1 != 0);
00734 
00735   cam_params_.master_gain = is_SetHardwareGain(cam_handle_, IS_GET_MASTER_GAIN,
00736       IS_IGNORE_PARAMETER, IS_IGNORE_PARAMETER, IS_IGNORE_PARAMETER);
00737   cam_params_.red_gain = is_SetHardwareGain(cam_handle_, IS_GET_RED_GAIN,
00738       IS_IGNORE_PARAMETER, IS_IGNORE_PARAMETER, IS_IGNORE_PARAMETER);
00739   cam_params_.green_gain = is_SetHardwareGain(cam_handle_, IS_GET_GREEN_GAIN,
00740       IS_IGNORE_PARAMETER, IS_IGNORE_PARAMETER, IS_IGNORE_PARAMETER);
00741   cam_params_.blue_gain = is_SetHardwareGain(cam_handle_, IS_GET_BLUE_GAIN,
00742       IS_IGNORE_PARAMETER, IS_IGNORE_PARAMETER, IS_IGNORE_PARAMETER);
00743 
00744   query = is_SetGainBoost(cam_handle_, IS_GET_SUPPORTED_GAINBOOST);
00745   if(query == IS_SET_GAINBOOST_ON) {
00746     query = is_SetGainBoost(cam_handle_, IS_GET_GAINBOOST);
00747     if (query == IS_SET_GAINBOOST_ON) {
00748       cam_params_.gain_boost = true;
00749     } else if (query == IS_SET_GAINBOOST_OFF) {
00750       cam_params_.gain_boost = false;
00751     } else {
00752       ERROR_STREAM("Failed to query gain boost for [" << cam_name_ <<
00753         "] (" << err2str(query) << ")");
00754       return query;
00755     }
00756   } else {
00757     cam_params_.gain_boost = false;
00758   }
00759 
00760   if ((is_err = is_SetAutoParameter(cam_handle_,
00761       IS_GET_ENABLE_AUTO_SENSOR_SHUTTER, &pval1, &pval2)) != IS_SUCCESS &&
00762       (is_err = is_SetAutoParameter(cam_handle_,
00763           IS_GET_ENABLE_AUTO_SHUTTER, &pval1, &pval2)) != IS_SUCCESS) {
00764     ERROR_STREAM("Failed to query auto shutter mode for [" << cam_name_ <<
00765       "] (" << err2str(is_err) << ")");
00766     return is_err;
00767   }
00768   cam_params_.auto_exposure = (pval1 != 0);
00769 
00770   if ((is_err = is_Exposure(cam_handle_, IS_EXPOSURE_CMD_GET_EXPOSURE,
00771       &cam_params_.exposure, sizeof(cam_params_.exposure))) != IS_SUCCESS) {
00772     ERROR_STREAM("Failed to query exposure timing for [" << cam_name_ <<
00773       "] (" << err2str(is_err) << ")");
00774     return is_err;
00775   }
00776 
00777   if ((is_err = is_SetAutoParameter(cam_handle_,
00778       IS_GET_ENABLE_AUTO_SENSOR_WHITEBALANCE, &pval1, &pval2)) != IS_SUCCESS &&
00779       (is_err = is_SetAutoParameter(cam_handle_,
00780           IS_GET_ENABLE_AUTO_WHITEBALANCE, &pval1, &pval2)) != IS_SUCCESS) {
00781     ERROR_STREAM("Failed to query auto white balance mode for [" << cam_name_ <<
00782       "] (" << err2str(is_err) << ")");
00783     return is_err;
00784   }
00785   cam_params_.auto_white_balance = (pval1 != 0);
00786 
00787   if ((is_err = is_SetAutoParameter(cam_handle_,
00788       IS_GET_AUTO_WB_OFFSET, &pval1, &pval2)) != IS_SUCCESS) {
00789     ERROR_STREAM("Failed to query auto white balance red/blue channel offsets for [" <<
00790       cam_name_ << "] (" << err2str(is_err) << ")");
00791     return is_err;
00792   }
00793   cam_params_.white_balance_red_offset = pval1;
00794   cam_params_.white_balance_blue_offset = pval2;
00795 
00796   IO_FLASH_PARAMS currFlashParams;
00797   if ((is_err = is_IO(cam_handle_, IS_IO_CMD_FLASH_GET_PARAMS,
00798       (void*) &currFlashParams, sizeof(IO_FLASH_PARAMS))) != IS_SUCCESS) {
00799     ERROR_STREAM("Could not retrieve current flash parameter info for [" <<
00800       cam_name_ << "] (" << err2str(is_err) << ")");
00801     return is_err;
00802   }
00803   cam_params_.flash_delay = currFlashParams.s32Delay;
00804   cam_params_.flash_duration = currFlashParams.u32Duration;
00805 
00806   if ((is_err = is_SetAutoParameter(cam_handle_,
00807       IS_GET_ENABLE_AUTO_SENSOR_FRAMERATE, &pval1, &pval2)) != IS_SUCCESS &&
00808       (is_err = is_SetAutoParameter(cam_handle_,
00809           IS_GET_ENABLE_AUTO_FRAMERATE, &pval1, &pval2)) != IS_SUCCESS) {
00810     ERROR_STREAM("Failed to query auto frame rate mode for [" << cam_name_ <<
00811       "] (" << err2str(is_err) << ")");
00812     return is_err;
00813   }
00814   cam_params_.auto_frame_rate = (pval1 != 0);
00815 
00816   if ((is_err = is_SetFrameRate(cam_handle_, IS_GET_FRAMERATE, &cam_params_.frame_rate)) != IS_SUCCESS) {
00817     ERROR_STREAM("Failed to query frame rate for [" << cam_name_ <<
00818       "] (" << err2str(is_err) << ")");
00819     return is_err;
00820   }
00821 
00822   UINT currPixelClock;
00823   if ((is_err = is_PixelClock(cam_handle_, IS_PIXELCLOCK_CMD_GET,
00824       (void*) &currPixelClock, sizeof(currPixelClock))) != IS_SUCCESS) {
00825     ERROR_STREAM("Failed to query pixel clock rate for [" << cam_name_ <<
00826       "] (" << err2str(is_err) << ")");
00827     return is_err;
00828   }
00829   cam_params_.pixel_clock = currPixelClock;
00830 
00831   INT currROP = is_SetRopEffect(cam_handle_, IS_GET_ROP_EFFECT, 0, 0);
00832   cam_params_.flip_upd = ((currROP & IS_SET_ROP_MIRROR_UPDOWN) == IS_SET_ROP_MIRROR_UPDOWN);
00833   cam_params_.flip_lr = ((currROP & IS_SET_ROP_MIRROR_LEFTRIGHT) == IS_SET_ROP_MIRROR_LEFTRIGHT);
00834 
00835   // NOTE: do not need to (re-)populate ROS image message, since assume that
00836   //       syncCamConfig() was previously called
00837 
00838   DEBUG_STREAM("Successfully queries parameters from [" << cam_name_ << "]");
00839 
00840   return is_err;
00841 }
00842 
00843 
00844 INT UEyeCamNodelet::connectCam() {
00845   INT is_err = IS_SUCCESS;
00846 
00847   if ((is_err = UEyeCamDriver::connectCam()) != IS_SUCCESS) return is_err;
00848 
00849   // (Attempt to) load UEye camera parameter configuration file
00850   if (cam_params_filename_.length() <= 0) { // Use default filename
00851     cam_params_filename_ = string(getenv("HOME")) + "/.ros/camera_conf/" + cam_name_ + ".ini";
00852   }
00853   if ((is_err = loadCamConfig(cam_params_filename_)) != IS_SUCCESS) return is_err;
00854 
00855   // Query existing configuration parameters from camera
00856   if ((is_err = queryCamParams()) != IS_SUCCESS) return is_err;
00857 
00858   // Parse and load ROS camera settings
00859   if ((is_err = parseROSParams(getPrivateNodeHandle())) != IS_SUCCESS) return is_err;
00860 
00861   return IS_SUCCESS;
00862 }
00863 
00864 
00865 INT UEyeCamNodelet::disconnectCam() {
00866   INT is_err = IS_SUCCESS;
00867 
00868   if (isConnected()) {
00869     stopFrameGrabber();
00870     is_err = UEyeCamDriver::disconnectCam();
00871   }
00872 
00873   return is_err;
00874 }
00875 
00876 
00877 bool UEyeCamNodelet::setCamInfo(sensor_msgs::SetCameraInfo::Request& req,
00878     sensor_msgs::SetCameraInfo::Response& rsp) {
00879   ros_cam_info_ = req.camera_info;
00880   ros_cam_info_.header.frame_id = "/" + frame_name_;
00881   rsp.success = saveIntrinsicsFile();
00882   rsp.status_message = (rsp.success) ?
00883     "successfully wrote camera info to file" :
00884     "failed to write camera info to file";
00885   return true;
00886 }
00887 
00888 
00889 void UEyeCamNodelet::frameGrabLoop() {
00890 #ifdef DEBUG_PRINTOUT_FRAME_GRAB_RATES
00891   ros::Time prevStartGrab = ros::Time::now();
00892   ros::Time prevGrabbedFrame = ros::Time::now();
00893   ros::Time currStartGrab;
00894   ros::Time currGrabbedFrame;
00895   double startGrabSum = 0;
00896   double grabbedFrameSum = 0;
00897   double startGrabSumSqrd = 0;
00898   double grabbedFrameSumSqrd = 0;
00899   unsigned int startGrabCount = 0;
00900   unsigned int grabbedFrameCount = 0;
00901 #endif
00902 
00903   DEBUG_STREAM("Starting threaded frame grabber loop for [" << cam_name_ << "]");
00904 
00905   ros::Rate idleDelay(200);
00906 
00907   int prevNumSubscribers = 0;
00908   int currNumSubscribers = 0;
00909   while (frame_grab_alive_ && ros::ok()) {
00910     // Initialize live video mode if camera was previously asleep, and ROS image topic has subscribers;
00911     // and stop live video mode if ROS image topic no longer has any subscribers
00912     currNumSubscribers = ros_cam_pub_.getNumSubscribers();
00913     if (currNumSubscribers > 0 && prevNumSubscribers <= 0) {
00914       // Reset reference time to prevent throttling first frame
00915       output_rate_mutex_.lock();
00916       init_publish_time_ = ros::Time(0);
00917       prev_output_frame_idx_ = 0;
00918       output_rate_mutex_.unlock();
00919 
00920       if (cam_params_.ext_trigger_mode) {
00921         if (setExtTriggerMode() != IS_SUCCESS) {
00922           ERROR_STREAM("Shutting down driver nodelet for [" << cam_name_ << "]");
00923           ros::shutdown();
00924           return;
00925         }
00926         INFO_STREAM("[" << cam_name_ << "] set to external trigger mode");
00927       } else {
00928         // NOTE: need to copy flash parameters to local copies since
00929         //       cam_params_.flash_duration is type int, and also sizeof(int)
00930         //       may not equal to sizeof(INT) / sizeof(UINT)
00931         INT flash_delay = cam_params_.flash_delay;
00932         UINT flash_duration = cam_params_.flash_duration;
00933         if ((setFreeRunMode() != IS_SUCCESS) ||
00934             (setFlashParams(flash_delay, flash_duration) != IS_SUCCESS)) {
00935           ERROR_STREAM("Shutting down driver nodelet for [" << cam_name_ << "]");
00936           ros::shutdown();
00937           return;
00938         }
00939         // Copy back actual flash parameter values that were set
00940         cam_params_.flash_delay = flash_delay;
00941         cam_params_.flash_duration = flash_duration;
00942         INFO_STREAM("[" << cam_name_ << "] set to free-run mode");
00943       }
00944     } else if (currNumSubscribers <= 0 && prevNumSubscribers > 0) {
00945       if (setStandbyMode() != IS_SUCCESS) {
00946         ERROR_STREAM("Shutting down driver nodelet for [" << cam_name_ << "]");
00947         ros::shutdown();
00948         return;
00949       }
00950       INFO_STREAM("[" << cam_name_ << "] set to standby mode");
00951     }
00952     prevNumSubscribers = currNumSubscribers;
00953 
00954     // Send updated dyncfg parameters if previously changed
00955     if (cfg_sync_requested_) {
00956       if (ros_cfg_mutex_.try_lock()) { // Make sure that dynamic reconfigure server or config callback is not active
00957         ros_cfg_mutex_.unlock();
00958         ros_cfg_->updateConfig(cam_params_);
00959         cfg_sync_requested_ = false;
00960       }
00961     }
00962 
00963 
00964 #ifdef DEBUG_PRINTOUT_FRAME_GRAB_RATES
00965     startGrabCount++;
00966     currStartGrab = ros::Time::now();
00967     if (startGrabCount > 1) {
00968       startGrabSum += (currStartGrab - prevStartGrab).toSec() * 1000.0;
00969       startGrabSumSqrd += ((currStartGrab - prevStartGrab).toSec() * 1000.0)*((currStartGrab - prevStartGrab).toSec() * 1000.0);
00970     }
00971     prevStartGrab = currStartGrab;
00972 #endif
00973 
00974     if (isCapturing()) {
00975       INT eventTimeout = (cam_params_.auto_frame_rate || cam_params_.ext_trigger_mode) ?
00976           (INT) 2000 : (INT) (1000.0 / cam_params_.frame_rate * 2);
00977       if (processNextFrame(eventTimeout) != NULL) {
00978         // Initialize shared pointers from member messages for nodelet intraprocess publishing
00979         sensor_msgs::ImagePtr img_msg_ptr(new sensor_msgs::Image(ros_image_));
00980         sensor_msgs::CameraInfoPtr cam_info_msg_ptr(new sensor_msgs::CameraInfo(ros_cam_info_));
00981         
00982         // Initialize/compute frame timestamp based on clock tick value from camera
00983         if (init_ros_time_.isZero()) {
00984           if(getClockTick(&init_clock_tick_)) {
00985             init_ros_time_ = getImageTimestamp();
00986 
00987             // Deal with instability in getImageTimestamp due to daylight savings time
00988             if (abs((ros::Time::now() - init_ros_time_).toSec()) > abs((ros::Time::now() - (init_ros_time_+ros::Duration(3600,0))).toSec())) { init_ros_time_ += ros::Duration(3600,0); }
00989             if (abs((ros::Time::now() - init_ros_time_).toSec()) > abs((ros::Time::now() - (init_ros_time_-ros::Duration(3600,0))).toSec())) { init_ros_time_ -= ros::Duration(3600,0); }
00990           }
00991         }
00992         img_msg_ptr->header.stamp = cam_info_msg_ptr->header.stamp = getImageTickTimestamp();
00993 
00994         // Process new frame
00995 #ifdef DEBUG_PRINTOUT_FRAME_GRAB_RATES
00996         grabbedFrameCount++;
00997         currGrabbedFrame = ros::Time::now();
00998         if (grabbedFrameCount > 1) {
00999           grabbedFrameSum += (currGrabbedFrame - prevGrabbedFrame).toSec() * 1000.0;
01000           grabbedFrameSumSqrd += ((currGrabbedFrame - prevGrabbedFrame).toSec() * 1000.0)*((currGrabbedFrame - prevGrabbedFrame).toSec() * 1000.0);
01001         }
01002         prevGrabbedFrame = currGrabbedFrame;
01003 
01004         if (grabbedFrameCount > 1) {
01005           WARN_STREAM("\nPre-Grab: " << startGrabSum/startGrabCount << " +/- " <<
01006               sqrt(startGrabSumSqrd/startGrabCount - (startGrabSum/startGrabCount)*(startGrabSum/startGrabCount)) << " ms (" <<
01007               1000.0*startGrabCount/startGrabSum << "Hz)\n" <<
01008               "Post-Grab: " << grabbedFrameSum/grabbedFrameCount << " +/- " <<
01009               sqrt(grabbedFrameSumSqrd/grabbedFrameCount - (grabbedFrameSum/grabbedFrameCount)*(grabbedFrameSum/grabbedFrameCount)) << " ms (" <<
01010               1000.0*grabbedFrameCount/grabbedFrameSum << "Hz)\n" <<
01011               "Target: " << cam_params_.frame_rate << "Hz");
01012         }
01013 #endif
01014 
01015         if (!frame_grab_alive_ || !ros::ok()) break;
01016 
01017         // Throttle publish rate
01018         bool throttle_curr_frame = false;
01019         output_rate_mutex_.lock();
01020         if (!cam_params_.ext_trigger_mode && cam_params_.output_rate > 0) {
01021           if (init_publish_time_.is_zero()) { // Set reference time 
01022             init_publish_time_ = img_msg_ptr->header.stamp;
01023           } else {
01024             double time_elapsed = (img_msg_ptr->header.stamp - init_publish_time_).toSec();
01025             uint64_t curr_output_frame_idx = std::floor(time_elapsed * cam_params_.output_rate);
01026             if (curr_output_frame_idx <= prev_output_frame_idx_) {
01027               throttle_curr_frame = true;
01028             } else {
01029               prev_output_frame_idx_ = curr_output_frame_idx;
01030             }
01031           }
01032         }
01033         output_rate_mutex_.unlock();
01034         if (throttle_curr_frame) continue;
01035 
01036         cam_info_msg_ptr->width = cam_params_.image_width / cam_sensor_scaling_rate_ / cam_binning_rate_;
01037         cam_info_msg_ptr->height = cam_params_.image_height / cam_sensor_scaling_rate_ / cam_binning_rate_;
01038 
01039         // Copy pixel content from internal frame buffer to ROS image
01040         if (!fillMsgData(*img_msg_ptr)) continue;
01041 
01042         img_msg_ptr->header.seq = cam_info_msg_ptr->header.seq = ros_frame_count_++;
01043         img_msg_ptr->header.frame_id = cam_info_msg_ptr->header.frame_id;
01044 
01045         if (!frame_grab_alive_ || !ros::ok()) break;
01046 
01047         ros_cam_pub_.publish(img_msg_ptr, cam_info_msg_ptr);
01048       }
01049     } else {
01050         init_ros_time_ = ros::Time(0);
01051         init_clock_tick_ = 0;
01052     }
01053 
01054     if (!frame_grab_alive_ || !ros::ok()) break;
01055     idleDelay.sleep();
01056   }
01057 
01058   setStandbyMode();
01059   frame_grab_alive_ = false;
01060 
01061   DEBUG_STREAM("Frame grabber loop terminated for [" << cam_name_ << "]");
01062 }
01063 
01064 
01065 void UEyeCamNodelet::startFrameGrabber() {
01066   frame_grab_alive_ = true;
01067   frame_grab_thread_ = thread(bind(&UEyeCamNodelet::frameGrabLoop, this));
01068 }
01069 
01070 
01071 void UEyeCamNodelet::stopFrameGrabber() {
01072   frame_grab_alive_ = false;
01073   if (frame_grab_thread_.joinable()) {
01074     frame_grab_thread_.join();
01075   }
01076   frame_grab_thread_ = thread();
01077 }
01078 
01079 const std::map<INT, std::string> UEyeCamNodelet::ENCODING_DICTIONARY = {
01080     { IS_CM_SENSOR_RAW8, sensor_msgs::image_encodings::BAYER_RGGB8 },
01081     { IS_CM_SENSOR_RAW10, sensor_msgs::image_encodings::BAYER_RGGB16 },
01082     { IS_CM_SENSOR_RAW12, sensor_msgs::image_encodings::BAYER_RGGB16 },
01083     { IS_CM_SENSOR_RAW16, sensor_msgs::image_encodings::BAYER_RGGB16 },
01084     { IS_CM_MONO8, sensor_msgs::image_encodings::MONO8 },
01085     { IS_CM_MONO10, sensor_msgs::image_encodings::MONO16 },
01086     { IS_CM_MONO12, sensor_msgs::image_encodings::MONO16 },
01087     { IS_CM_MONO16, sensor_msgs::image_encodings::MONO16 },
01088     { IS_CM_RGB8_PACKED, sensor_msgs::image_encodings::RGB8 },
01089     { IS_CM_BGR8_PACKED, sensor_msgs::image_encodings::BGR8 },
01090     { IS_CM_RGB10_PACKED, sensor_msgs::image_encodings::RGB16 },
01091     { IS_CM_BGR10_PACKED, sensor_msgs::image_encodings::BGR16 },
01092     { IS_CM_RGB10_UNPACKED, sensor_msgs::image_encodings::RGB16 },
01093     { IS_CM_BGR10_UNPACKED, sensor_msgs::image_encodings::BGR16 },
01094     { IS_CM_RGB12_UNPACKED, sensor_msgs::image_encodings::RGB16 },
01095     { IS_CM_BGR12_UNPACKED, sensor_msgs::image_encodings::BGR16 }
01096 };
01097 
01098 bool UEyeCamNodelet::fillMsgData(sensor_msgs::Image& img) const {
01099   // Copy pixel content from internal frame buffer to img
01100   // and unpack to proper pixel format
01101   INT expected_row_stride = cam_aoi_.s32Width * bits_per_pixel_/8;
01102   if (cam_buffer_pitch_ < expected_row_stride) {
01103     ERROR_STREAM("Camera buffer pitch (" << cam_buffer_pitch_ <<
01104         ") is smaller than expected for [" << cam_name_ << "]: " <<
01105         "width (" << cam_aoi_.s32Width << ") * bytes per pixel (" <<
01106         bits_per_pixel_/8 << ") = " << expected_row_stride);
01107     return false;
01108   }
01109 
01110   // allocate target memory
01111   img.width = cam_aoi_.s32Width;
01112   img.height = cam_aoi_.s32Height;
01113   img.encoding = ENCODING_DICTIONARY.at(color_mode_);
01114   img.step = img.width * sensor_msgs::image_encodings::numChannels(img.encoding) * sensor_msgs::image_encodings::bitDepth(img.encoding)/8;
01115   img.data.resize(img.height * img.step);
01116 
01117   DEBUG_STREAM("Allocated ROS image buffer for [" << cam_name_ << "]:" <<
01118       "\n  size: " << cam_buffer_size_ <<
01119       "\n  width: " << img.width <<
01120       "\n  height: " << img.height <<
01121       "\n  step: " << img.step <<
01122       "\n  encoding: " << img.encoding);
01123 
01124   const std::function<void*(void*, void*, size_t)> unpackCopy = getUnpackCopyFunc(color_mode_);
01125 
01126   if (cam_buffer_pitch_ == expected_row_stride) {
01127     // Content is contiguous, so copy out the entire buffer at once
01128     unpackCopy(img.data.data(), cam_buffer_, img.height*expected_row_stride);
01129   } else { // cam_buffer_pitch_ > expected_row_stride
01130     // Each row contains extra buffer according to cam_buffer_pitch_, so must copy out each row independently
01131     uint8_t* dst_ptr = img.data.data();
01132     char* cam_buffer_ptr = cam_buffer_;
01133     for (INT row = 0; row < cam_aoi_.s32Height; row++) {
01134       unpackCopy(dst_ptr, cam_buffer_ptr, expected_row_stride);
01135       cam_buffer_ptr += cam_buffer_pitch_;
01136       dst_ptr += img.step;
01137     }
01138   }
01139   return true;
01140 }
01141 
01142 
01143 void UEyeCamNodelet::loadIntrinsicsFile() {
01144   if (cam_intr_filename_.length() <= 0) { // Use default filename
01145     cam_intr_filename_ = string(getenv("HOME")) + "/.ros/camera_info/" + cam_name_ + ".yaml";
01146   }
01147 
01148   if (camera_calibration_parsers::readCalibration(cam_intr_filename_, cam_name_, ros_cam_info_)) {
01149     DEBUG_STREAM("Loaded intrinsics parameters for [" << cam_name_ << "]");
01150   }
01151   ros_cam_info_.header.frame_id = "/" + frame_name_;
01152 }
01153 
01154 
01155 bool UEyeCamNodelet::saveIntrinsicsFile() {
01156   if (camera_calibration_parsers::writeCalibration(cam_intr_filename_, cam_name_, ros_cam_info_)) {
01157     DEBUG_STREAM("Saved intrinsics parameters for [" << cam_name_ <<
01158       "] to " << cam_intr_filename_);
01159     return true;
01160   }
01161   return false;
01162 }
01163 
01164 ros::Time UEyeCamNodelet::getImageTimestamp() {
01165   UEYETIME utime;
01166   if(getTimestamp(&utime)) {
01167     struct tm tm;
01168     tm.tm_year = utime.wYear - 1900;
01169     tm.tm_mon = utime.wMonth - 1;
01170     tm.tm_mday = utime.wDay;
01171     tm.tm_hour = utime.wHour;
01172     tm.tm_min = utime.wMinute;
01173     tm.tm_sec = utime.wSecond;
01174     return ros::Time(mktime(&tm),utime.wMilliseconds*1e6);
01175   }
01176   return ros::Time::now();
01177 }
01178 
01179 ros::Time UEyeCamNodelet::getImageTickTimestamp() {
01180   uint64_t tick;
01181   if(getClockTick(&tick)) {
01182     return init_ros_time_ + ros::Duration(double(tick - init_clock_tick_)*1e-7);
01183   }
01184   return ros::Time::now();
01185 }
01186 // TODO: 0 bug where nodelet locks and requires SIGTERM when there are still subscribers (need to find where does code hang)
01187 
01188 
01189 void UEyeCamNodelet::handleTimeout() {
01190   std_msgs::UInt64 timeout_msg;
01191   timeout_msg.data = ++timeout_count_;
01192   timeout_pub_.publish(timeout_msg);
01193 };
01194 
01195 
01196 } // namespace ueye_cam
01197 
01198 
01199 // TODO: 9 bug: when binning (and suspect when subsampling / sensor scaling), white balance / color gains seem to have different effects
01200 
01201 
01202 #include <pluginlib/class_list_macros.h>
01203 PLUGINLIB_EXPORT_CLASS(ueye_cam::UEyeCamNodelet, nodelet::Nodelet)


ueye_cam
Author(s): Anqi Xu
autogenerated on Fri Feb 24 2017 04:14:06