CameraNode.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012-2016, Kevin Hallenbeck
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 Kevin Hallenbeck nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <ueye/CameraNode.h>
00036 
00037 namespace ueye
00038 {
00039 
00040 CameraNode::CameraNode(ros::NodeHandle node, ros::NodeHandle priv_nh) :
00041     srv_(priv_nh), it_(node)
00042 {
00043   running_ = false;
00044   configured_ = false;
00045   force_streaming_ = false;
00046   auto_exposure_ = false;
00047   auto_gain_ = false;
00048   trigger_mode_ = zoom_ = -1;
00049 
00050   // Check for a valid uEye installation and supported version
00051   const char *version;
00052   int major, minor, build;
00053   if (cam_.checkVersion(major, minor, build, version)) {
00054     ROS_INFO("Loaded uEye SDK %s.", version);
00055   } else {
00056     ROS_WARN("Loaded uEye SDK %d.%d.%d. Expecting %s.", major, minor, build, version);
00057   }
00058 
00059   // Make sure there is at least one camera available
00060   int num_cameras = cam_.getNumberOfCameras();
00061   if (num_cameras > 0) {
00062     if (num_cameras == 1) {
00063       ROS_INFO("Found 1 uEye camera.");
00064     } else {
00065       ROS_INFO("Found %d uEye cameras.", num_cameras);
00066     }
00067   } else {
00068     ROS_ERROR("Found 0 uEye cameras.");
00069     ros::shutdown();
00070     return;
00071   }
00072 
00073   // Open camera with either serialNo, deviceId, or cameraId
00074   int id = 0;
00075   if (priv_nh.getParam("serialNo", id)) {
00076     if (!cam_.openCameraSerNo(id)) {
00077       ROS_ERROR("Failed to open uEye camera with serialNo: %u.", id);
00078       ros::shutdown();
00079       return;
00080     }
00081   } else if (priv_nh.getParam("deviceId", id)) {
00082     if (!cam_.openCameraDevId(id)) {
00083       ROS_ERROR("Failed to open uEye camera with deviceId: %u.", id);
00084       ros::shutdown();
00085       return;
00086     }
00087   } else {
00088     priv_nh.getParam("cameraId", id);
00089     if (!cam_.openCameraCamId(id)) {
00090       ROS_ERROR("Failed to open uEye camera with cameraId: %u.", id);
00091       ros::shutdown();
00092       return;
00093     }
00094   }
00095   ROS_INFO("Opened camera %s %u", cam_.getCameraName(), cam_.getCameraSerialNo());
00096 
00097   // Setup Dynamic Reconfigure
00098   dynamic_reconfigure::Server<monoConfig>::CallbackType f = boost::bind(&CameraNode::reconfig, this, _1, _2);
00099   srv_.setCallback(f);  //start dynamic reconfigure
00100 
00101   // Service call for setting calibration.
00102   srv_cam_info_ = node.advertiseService("set_camera_info", &CameraNode::setCameraInfo, this);
00103 
00104   // Special publisher for images to support compression
00105   pub_stream_ = it_.advertiseCamera("image_raw", 0);
00106 
00107   // Set up Timer
00108   timer_ = node.createTimer(ros::Duration(1 / 5.0), &CameraNode::timerCallback, this);
00109 }
00110 
00111 CameraNode::~CameraNode()
00112 {
00113   closeCamera();
00114 }
00115 
00116 void CameraNode::handlePath(std::string &path)
00117 {
00118   // Set default path if not present
00119   if (path.length() == 0) {
00120     path = ros::package::getPath("ueye");
00121   }
00122 
00123   // Remove trailing "/" from folder path
00124   unsigned int length = path.length();
00125   if (length > 0) {
00126     if (path.c_str()[length - 1] == '/') {
00127       path = path.substr(0, length - 1);
00128     }
00129   }
00130   config_path_ = path;
00131 }
00132 void CameraNode::reconfig(monoConfig &config, uint32_t level)
00133 {
00134   force_streaming_ = config.force_streaming;
00135   handlePath(config.config_path);
00136 
00137   // Trigger
00138   if (trigger_mode_ != config.trigger) {
00139     stopCamera();
00140     TriggerMode mode;
00141     switch (config.trigger) {
00142       case mono_TGR_HARDWARE_RISING:
00143         mode = TRIGGER_LO_HI;
00144         break;
00145       case mono_TGR_HARDWARE_FALLING:
00146         mode = TRIGGER_HI_LO;
00147         break;
00148       case mono_TGR_AUTO:
00149       default:
00150         mode = TRIGGER_OFF;
00151         break;
00152     }
00153     if (!cam_.setTriggerMode(mode)) {
00154       cam_.setTriggerMode(TRIGGER_OFF);
00155       config.trigger = mono_TGR_AUTO;
00156     }
00157   }
00158   trigger_mode_ = config.trigger;
00159 
00160   // Color Mode
00161   uEyeColor color;
00162   switch (config.color) {
00163     default:
00164     case mono_COLOR_MONO8:
00165       color = MONO8;
00166       break;
00167     case mono_COLOR_MONO16:
00168       color = MONO16;
00169       break;
00170     case mono_COLOR_YUV:
00171       color = YUV;
00172       break;
00173     case mono_COLOR_YCbCr:
00174       color = YCbCr;
00175       break;
00176     case mono_COLOR_BGR5:
00177       color = BGR5;
00178       break;
00179     case mono_COLOR_BGR565:
00180       color = BGR565;
00181       break;
00182     case mono_COLOR_BGR8:
00183       color = BGR8;
00184       break;
00185     case mono_COLOR_BGRA8:
00186       color = BGRA8;
00187       break;
00188     case mono_COLOR_BGRY8:
00189       color = BGRY8;
00190       break;
00191     case mono_COLOR_RGB8:
00192       color = RGB8;
00193       break;
00194     case mono_COLOR_RGBA8:
00195       color = RGBA8;
00196       break;
00197     case mono_COLOR_RGBY8:
00198       color = RGBY8;
00199       break;
00200   }
00201   if (cam_.getColorMode() != color) {
00202     cam_.setColorMode(color);
00203   }
00204 
00205   // Latch Auto Parameters
00206   if (auto_gain_ && !config.auto_gain) {
00207     config.gain = cam_.getHardwareGain();
00208   }
00209   auto_gain_ = config.auto_gain;
00210   if (auto_exposure_ && !config.auto_exposure) {
00211     config.exposure_time = cam_.getExposure();
00212   }
00213   auto_exposure_ = config.auto_exposure;
00214 
00215   // Hardware Gamma Correction
00216   if (cam_.getHardwareGamma() != config.hardware_gamma) {
00217     cam_.setHardwareGamma(&config.hardware_gamma);
00218   }
00219 
00220   // Gain Boost
00221   if (cam_.getGainBoost() != config.gain_boost) {
00222     cam_.setGainBoost(&config.gain_boost);
00223   }
00224 
00225   // Hardware Gain
00226   if (cam_.getAutoGain() != config.auto_gain) {
00227     cam_.setAutoGain(&config.auto_gain);
00228   }
00229   if (!config.auto_gain) {
00230     cam_.setHardwareGain(&config.gain);
00231   }
00232 
00233   // Zoom
00234   if (cam_.getZoom() != config.zoom) {
00235     cam_.setZoom(&config.zoom);
00236   }
00237 
00238   // Pixel Clock
00239   if (cam_.getPixelClock() != config.pixel_clock) {
00240     cam_.setPixelClock(&config.pixel_clock);
00241   }
00242 
00243   // Frame Rate
00244   cam_.setFrameRate(&config.frame_rate);
00245 
00246   // Exposure
00247   if (cam_.getAutoExposure() != config.auto_exposure) {
00248     cam_.setAutoExposure(&config.auto_exposure);
00249   }
00250   if (!config.auto_exposure) {
00251     cam_.setExposure(&config.exposure_time);
00252   }
00253 
00254   // Zoom
00255   if (zoom_ != config.zoom) {
00256     zoom_ = config.zoom;
00257     loadIntrinsics();
00258   }
00259 
00260   msg_camera_info_.header.frame_id = config.frame_id;
00261   configured_ = true;
00262 }
00263 
00264 void CameraNode::timerCallback(const ros::TimerEvent& event)
00265 {
00266   if ((pub_stream_.getNumSubscribers() > 0) || force_streaming_) {
00267     startCamera();
00268   } else {
00269     stopCamera();
00270   }
00271 }
00272 
00273 // Support camera calibration requests
00274 // http://www.ros.org/wiki/camera_calibration/Tutorials/MonocularCalibration
00275 bool CameraNode::setCameraInfo(sensor_msgs::SetCameraInfo::Request& req, sensor_msgs::SetCameraInfo::Response& rsp)
00276 {
00277   ROS_INFO("New camera info received");
00278   sensor_msgs::CameraInfo &info = req.camera_info;
00279   info.header.frame_id = msg_camera_info_.header.frame_id;
00280 
00281   // Sanity check: the image dimensions should match the resolution of the sensor.
00282   unsigned int height = cam_.getHeight();
00283   unsigned int width = cam_.getWidth();
00284 
00285   if (info.width != width || info.height != height) {
00286     rsp.success = false;
00287     rsp.status_message = (boost::format("Camera_info resolution %ix%i does not match current video "
00288                                         "setting, camera running at resolution %ix%i.") % info.width % info.height
00289         % width % height).str();
00290     ROS_ERROR("%s", rsp.status_message.c_str());
00291     return true;
00292   }
00293 
00294   std::string camname = cam_.getCameraName();
00295   std::stringstream ini_stream;
00296   if (!camera_calibration_parsers::writeCalibrationIni(ini_stream, camname, info)) {
00297     rsp.status_message = "Error formatting camera_info for storage.";
00298     rsp.success = false;
00299   } else {
00300     std::string ini = ini_stream.str();
00301     std::fstream param_file;
00302     std::string filename = config_path_ + "/" + configFileName(cam_);
00303     param_file.open(filename.c_str(), std::ios::in | std::ios::out | std::ios::trunc);
00304 
00305     if (param_file.is_open()) {
00306       param_file << ini.c_str();
00307       param_file.close();
00308 
00309       msg_camera_info_ = info;
00310       rsp.success = true;
00311     } else {
00312       rsp.success = false;
00313       rsp.status_message = "file write failed";
00314     }
00315   }
00316   if (!rsp.success) {
00317     ROS_ERROR("%s", rsp.status_message.c_str());
00318   }
00319   return true;
00320 }
00321 
00322 // Try to load previously saved camera calibration from a file.
00323 void CameraNode::loadIntrinsics()
00324 {
00325   char buffer[12800];
00326 
00327   std::string MyPath = config_path_ + "/" + configFileName(cam_);
00328   std::fstream param_file;
00329   param_file.open(MyPath.c_str(), std::ios::in);
00330 
00331   if (param_file.is_open()) {
00332     param_file.read(buffer, 12800);
00333     param_file.close();
00334   }
00335 
00336   // Parse calibration file
00337   std::string camera_name;
00338   if (camera_calibration_parsers::parseCalibrationIni(buffer, camera_name, msg_camera_info_)) {
00339     ROS_INFO("Loaded calibration for camera '%s'", camera_name.c_str());
00340   } else {
00341     ROS_WARN("Failed to load intrinsics for camera from file");
00342   }
00343 }
00344 
00345 // Add properties to image message
00346 sensor_msgs::ImagePtr CameraNode::processFrame(const char *frame, size_t size, sensor_msgs::CameraInfoPtr &info)
00347 {
00348   msg_camera_info_.header.stamp = ros::Time::now();
00349   msg_camera_info_.roi.x_offset = 0;
00350   msg_camera_info_.roi.y_offset = 0;
00351   msg_camera_info_.height = cam_.getHeight();
00352   msg_camera_info_.width = cam_.getWidth();
00353   msg_camera_info_.roi.width = 0;
00354   msg_camera_info_.roi.height = 0;
00355   sensor_msgs::CameraInfoPtr msg(new sensor_msgs::CameraInfo(msg_camera_info_));
00356   info = msg;
00357 
00358   sensor_msgs::ImagePtr msg_image(new sensor_msgs::Image());
00359   msg_image->header = msg_camera_info_.header;
00360   msg_image->height = msg_camera_info_.height;
00361   msg_image->width = msg_camera_info_.width;
00362   msg_image->encoding = Camera::colorModeToString(cam_.getColorMode());
00363   msg_image->is_bigendian = false;
00364   msg_image->step = size / msg_image->height;
00365   msg_image->data.resize(size);
00366   memcpy(msg_image->data.data(), frame, size);
00367   return msg_image;
00368 }
00369 
00370 // Timestamp and publish the image. Called by the streaming thread.
00371 void CameraNode::publishImage(const char *frame, size_t size)
00372 {
00373   sensor_msgs::CameraInfoPtr info;
00374   sensor_msgs::ImagePtr img = processFrame(frame, size, info);
00375   pub_stream_.publish(img, info);
00376 }
00377 
00378 void CameraNode::startCamera()
00379 {
00380   if (running_ || !configured_)
00381     return;
00382   cam_.startVideoCapture(boost::bind(&CameraNode::publishImage, this, _1, _2));
00383   ROS_INFO("Started video stream.");
00384   running_ = true;
00385 }
00386 
00387 void CameraNode::stopCamera()
00388 {
00389   if (!running_)
00390     return;
00391   cam_.stopVideoCapture();
00392   ROS_INFO("Stopped video stream.");
00393   running_ = false;
00394 }
00395 
00396 void CameraNode::closeCamera()
00397 {
00398   stopCamera();
00399   cam_.closeCamera();
00400 }
00401 
00402 } // namespace ueye


ueye
Author(s): Kevin Hallenbeck
autogenerated on Thu Sep 7 2017 09:31:27