StereoNode.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, 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/StereoNode.h>
00036 
00037 namespace ueye
00038 {
00039 
00040 StereoNode::StereoNode(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   l_stamp_ = r_stamp_ = ros::Time(0);
00050 
00051   // Check for a valid uEye installation and supported version
00052   const char *version;
00053   int major, minor, build;
00054   if (l_cam_.checkVersion(major, minor, build, version)) {
00055     ROS_INFO("Loaded uEye SDK %s.", version);
00056   } else {
00057     ROS_WARN("Loaded uEye SDK %d.%d.%d. Expecting %s.", major, minor, build, version);
00058   }
00059 
00060   // Make sure there is at least one camera available
00061   int num_cameras = l_cam_.getNumberOfCameras();
00062   if (num_cameras > 0) {
00063     if (num_cameras == 1) {
00064       ROS_ERROR("Found 1 uEye camera.");
00065       ros::shutdown();
00066     } else {
00067       ROS_INFO("Found %d uEye cameras.", num_cameras);
00068     }
00069   } else {
00070     ROS_ERROR("Found 0 uEye cameras.");
00071     ros::shutdown();
00072     return;
00073   }
00074 
00075   // Service call for setting calibration.
00076   l_srv_cam_info_ = node.advertiseService("left/set_camera_info", &StereoNode::setCameraInfoL, this);
00077   r_srv_cam_info_ = node.advertiseService("right/set_camera_info", &StereoNode::setCameraInfoR, this);
00078 
00079   // Special publisher for images to support compression
00080   l_pub_stream_ = it_.advertiseCamera("left/image_raw", 0);
00081   r_pub_stream_ = it_.advertiseCamera("right/image_raw", 0);
00082 
00083   // Open cameras with either serialNo, deviceId, or cameraId
00084   int id = 0;
00085   if (priv_nh.getParam("lSerialNo", id)) {
00086     if (!l_cam_.openCameraSerNo(id)) {
00087       ROS_ERROR("Failed to open uEye camera with serialNo: %u.", id);
00088       ros::shutdown();
00089       return;
00090     }
00091   } else if (priv_nh.getParam("lDeviceId", id)) {
00092     if (!l_cam_.openCameraDevId(id)) {
00093       ROS_ERROR("Failed to open uEye camera with deviceId: %u.", id);
00094       ros::shutdown();
00095       return;
00096     }
00097   } else {
00098     priv_nh.getParam("lCameraId", id);
00099     if (!l_cam_.openCameraCamId(id)) {
00100       ROS_ERROR("Failed to open uEye camera with cameraId: %u.", id);
00101       ros::shutdown();
00102       return;
00103     }
00104   }
00105   ROS_INFO("Left  camera: %s %u", l_cam_.getCameraName(), l_cam_.getCameraSerialNo());
00106   id = 0;
00107   if (priv_nh.getParam("rSerialNo", id)) {
00108     if (!r_cam_.openCameraSerNo(id)) {
00109       ROS_ERROR("Failed to open uEye camera with serialNo: %u.", id);
00110       ros::shutdown();
00111       return;
00112     }
00113   } else if (priv_nh.getParam("rDeviceId", id)) {
00114     if (!r_cam_.openCameraDevId(id)) {
00115       ROS_ERROR("Failed to open uEye camera with deviceId: %u.", id);
00116       ros::shutdown();
00117       return;
00118     }
00119   } else {
00120     priv_nh.getParam("rCameraId", id);
00121     if (!r_cam_.openCameraCamId(id)) {
00122       ROS_ERROR("Failed to open uEye camera with cameraId: %u.", id);
00123       ros::shutdown();
00124       return;
00125     }
00126   }
00127   ROS_INFO("Right camera: %s %u", r_cam_.getCameraName(), r_cam_.getCameraSerialNo());
00128 
00129   // Disable trigger delays
00130   l_cam_.setTriggerDelay(0);
00131   r_cam_.setTriggerDelay(0);
00132 
00133   timer_force_trigger_ = node.createTimer(ros::Duration(1.0), &StereoNode::timerForceTrigger, this);
00134   timer_force_trigger_.stop();
00135 
00136   // Setup Dynamic Reconfigure
00137   dynamic_reconfigure::Server<stereoConfig>::CallbackType f = boost::bind(&StereoNode::reconfig, this, _1, _2);
00138   srv_.setCallback(f);  //start dynamic reconfigure
00139 
00140   // Set up Timer
00141   timer_ = node.createTimer(ros::Duration(0.5), &StereoNode::timerCallback, this);
00142 }
00143 
00144 StereoNode::~StereoNode()
00145 {
00146   closeCamera();
00147 }
00148 
00149 void StereoNode::handlePath(std::string &path)
00150 {
00151   // Set default path if not present
00152   if (path.length() == 0) {
00153     path = ros::package::getPath("ueye");
00154   }
00155 
00156   // Remove trailing "/" from folder path
00157   unsigned int length = path.length();
00158   if (length > 0) {
00159     if (path.c_str()[length - 1] == '/') {
00160       path = path.substr(0, length - 1);
00161     }
00162   }
00163   config_path_ = path;
00164 }
00165 void StereoNode::reconfigCam(stereoConfig &config, uint32_t level, Camera &cam)
00166 {
00167   // Color Mode
00168   uEyeColor color;
00169   switch (config.color) {
00170     default:
00171     case mono_COLOR_MONO8:
00172       color = MONO8;
00173       break;
00174     case mono_COLOR_MONO16:
00175       color = MONO16;
00176       break;
00177     case mono_COLOR_YUV:
00178       color = YUV;
00179       break;
00180     case mono_COLOR_YCbCr:
00181       color = YCbCr;
00182       break;
00183     case mono_COLOR_BGR5:
00184       color = BGR5;
00185       break;
00186     case mono_COLOR_BGR565:
00187       color = BGR565;
00188       break;
00189     case mono_COLOR_BGR8:
00190       color = BGR8;
00191       break;
00192     case mono_COLOR_BGRA8:
00193       color = BGRA8;
00194       break;
00195     case mono_COLOR_BGRY8:
00196       color = BGRY8;
00197       break;
00198     case mono_COLOR_RGB8:
00199       color = RGB8;
00200       break;
00201     case mono_COLOR_RGBA8:
00202       color = RGBA8;
00203       break;
00204     case mono_COLOR_RGBY8:
00205       color = RGBY8;
00206       break;
00207   }
00208   if (cam.getColorMode() != color) {
00209     cam.setColorMode(color);
00210   }
00211 
00212   // Hardware Gamma Correction
00213   if (cam.getHardwareGamma() != config.hardware_gamma) {
00214     cam.setHardwareGamma(&config.hardware_gamma);
00215   }
00216 
00217   // Gain Boost
00218   if (cam.getGainBoost() != config.gain_boost) {
00219     cam.setGainBoost(&config.gain_boost);
00220   }
00221 
00222   // Hardware Gain
00223   if (cam.getAutoGain() != config.auto_gain) {
00224     cam.setAutoGain(&config.auto_gain);
00225   }
00226   if (!config.auto_gain) {
00227     cam.setHardwareGain(&config.gain);
00228   }
00229 
00230   // Zoom
00231   if (cam.getZoom() != config.zoom) {
00232     cam.setZoom(&config.zoom);
00233   }
00234 
00235   // Frame Rate
00236   if (config.trigger == stereo_TGR_SOFTWARE) {
00237     //In software trigger mode we don't want to set a frame rate
00238     double d = 2.0;
00239     cam.setFrameRate(&d);
00240   } else {
00241     cam.setFrameRate(&config.frame_rate);
00242     ROS_INFO("config.trigger %d", config.trigger);
00243   }
00244 
00245   // Exposure
00246   if (cam.getAutoExposure() != config.auto_exposure) {
00247     cam.setAutoExposure(&config.auto_exposure);
00248   }
00249   if (!config.auto_exposure) {
00250     cam.setExposure(&config.exposure_time);
00251   }
00252 }
00253 void StereoNode::reconfig(stereoConfig &config, uint32_t level)
00254 {
00255   force_streaming_ = config.force_streaming;
00256   handlePath(config.config_path);
00257 
00258   const FlashMode flash_active_mode = config.flash_polarity ? FLASH_FREERUN_ACTIVE_HI : FLASH_FREERUN_ACTIVE_LO;
00259 
00260   if (trigger_mode_ != config.trigger) {
00261     stopCamera();
00262     TriggerMode l_trigger, r_trigger;
00263     FlashMode l_flash = FLASH_OFF;
00264     FlashMode r_flash = FLASH_OFF;
00265     switch (config.trigger) {
00266       case stereo_TGR_SOFTWARE:
00267       case stereo_TGR_HARDWARE_RISING:
00268         l_trigger = r_trigger = TRIGGER_LO_HI;
00269         break;
00270       case stereo_TGR_HARDWARE_FALLING:
00271         l_trigger = r_trigger = TRIGGER_HI_LO;
00272         break;
00273       case stereo_TGR_L_MASTER_R_RISING:
00274         l_trigger = TRIGGER_OFF;
00275         r_trigger = TRIGGER_LO_HI;
00276         l_flash = flash_active_mode;
00277         break;
00278       case stereo_TGR_L_MASTER_R_FALLING:
00279         l_trigger = TRIGGER_OFF;
00280         r_trigger = TRIGGER_HI_LO;
00281         l_flash = flash_active_mode;
00282         break;
00283       case stereo_TGR_R_MASTER_L_RISING:
00284         l_trigger = TRIGGER_LO_HI;
00285         r_trigger = TRIGGER_OFF;
00286         r_flash = flash_active_mode;
00287         break;
00288       case stereo_TGR_R_MASTER_L_FALLING:
00289         l_trigger = TRIGGER_HI_LO;
00290         r_trigger = TRIGGER_OFF;
00291         r_flash = flash_active_mode;
00292         break;
00293       case stereo_TGR_AUTO:
00294       default:
00295         config.trigger = stereo_TGR_AUTO;
00296         l_trigger = r_trigger = TRIGGER_OFF;
00297         break;
00298     }
00299     if (!(l_cam_.setTriggerMode(l_trigger) && r_cam_.setTriggerMode(r_trigger))) {
00300       ROS_ERROR("Failed to configure triggers");
00301       l_cam_.setTriggerMode(TRIGGER_OFF);
00302       r_cam_.setTriggerMode(TRIGGER_OFF);
00303       config.trigger = stereo_TGR_AUTO;
00304       l_cam_.setFlash(FLASH_OFF);
00305       r_cam_.setFlash(FLASH_OFF);
00306     } else {
00307       l_cam_.setFlash(l_flash, config.flash_delay, config.flash_duration);
00308       r_cam_.setFlash(r_flash, config.flash_delay, config.flash_duration);
00309     }
00310   }
00311 
00312   // Latch Auto Parameters
00313   if (auto_gain_ && !config.auto_gain) {
00314     config.gain = l_cam_.getHardwareGain();
00315   }
00316   auto_gain_ = config.auto_gain;
00317   if (auto_exposure_ && !config.auto_exposure) {
00318     config.exposure_time = l_cam_.getExposure();
00319   }
00320   auto_exposure_ = config.auto_exposure;
00321 
00322   // Pixel Clock
00323   if (l_cam_.getPixelClock() != config.l_pixel_clock) {
00324     l_cam_.setPixelClock(&config.l_pixel_clock);
00325   }
00326   if (r_cam_.getPixelClock() != config.r_pixel_clock) {
00327     r_cam_.setPixelClock(&config.r_pixel_clock);
00328   }
00329 
00330   reconfigCam(config, level, l_cam_);
00331   reconfigCam(config, level, r_cam_);
00332 
00333   trigger_mode_ = config.trigger;
00334   if (trigger_mode_ == stereo_TGR_SOFTWARE) {
00335     timer_force_trigger_.setPeriod(ros::Duration(1 / config.frame_rate));
00336   }
00337 
00338   if (zoom_ != config.zoom) {
00339     zoom_ = config.zoom;
00340     loadIntrinsics(l_cam_, l_msg_camera_info_);
00341     loadIntrinsics(r_cam_, r_msg_camera_info_);
00342   }
00343 
00344   l_msg_camera_info_.header.frame_id = config.l_frame_id;
00345   r_msg_camera_info_.header.frame_id = config.r_frame_id;
00346   configured_ = true;
00347 }
00348 
00349 void StereoNode::timerCallback(const ros::TimerEvent& event)
00350 {
00351   if (force_streaming_ || (l_pub_stream_.getNumSubscribers() > 0) || (r_pub_stream_.getNumSubscribers() > 0)) {
00352     startCamera();
00353   } else {
00354     stopCamera();
00355   }
00356 }
00357 void StereoNode::timerForceTrigger(const ros::TimerEvent& event)
00358 {
00359   if (trigger_mode_ == stereo_TGR_SOFTWARE) {
00360     bool success = true;
00361     success &= l_cam_.forceTrigger();
00362     success &= r_cam_.forceTrigger();
00363     if (!success) {
00364       ROS_WARN("Failed to force trigger");
00365     }
00366   }
00367 }
00368 
00369 // Support camera calibration requests
00370 // http://www.ros.org/wiki/camera_calibration/Tutorials/MonocularCalibration
00371 bool StereoNode::setCameraInfo(sensor_msgs::SetCameraInfo::Request& req, sensor_msgs::SetCameraInfo::Response& rsp,
00372                                Camera& cam, sensor_msgs::CameraInfo &msg_info)
00373 {
00374   ROS_INFO("New camera info received");
00375   sensor_msgs::CameraInfo &info = req.camera_info;
00376   info.header.frame_id = msg_info.header.frame_id;
00377 
00378   // Sanity check: the image dimensions should match the resolution of the sensor.
00379   unsigned int height = cam.getHeight();
00380   unsigned int width = cam.getWidth();
00381 
00382   if (info.width != width || info.height != height) {
00383     rsp.success = false;
00384     rsp.status_message = (boost::format("Camera_info resolution %ix%i does not match current video "
00385                                         "setting, camera running at resolution %ix%i.") % info.width % info.height
00386         % width % height).str();
00387     ROS_ERROR("%s", rsp.status_message.c_str());
00388     return true;
00389   }
00390 
00391   std::string camname = cam.getCameraName();
00392   std::stringstream ini_stream;
00393   if (!camera_calibration_parsers::writeCalibrationIni(ini_stream, camname, info)) {
00394     rsp.status_message = "Error formatting camera_info for storage.";
00395     rsp.success = false;
00396   } else {
00397     std::string ini = ini_stream.str();
00398     std::fstream param_file;
00399     std::string filename = config_path_ + "/" + configFileName(cam);
00400     param_file.open(filename.c_str(), std::ios::in | std::ios::out | std::ios::trunc);
00401 
00402     if (param_file.is_open()) {
00403       param_file << ini.c_str();
00404       param_file.close();
00405 
00406       msg_info = info;
00407       rsp.success = true;
00408     } else {
00409       rsp.success = false;
00410       rsp.status_message = "file write failed";
00411     }
00412   }
00413   if (!rsp.success) {
00414     ROS_ERROR("%s", rsp.status_message.c_str());
00415   }
00416   return true;
00417 }
00418 bool StereoNode::setCameraInfoL(sensor_msgs::SetCameraInfo::Request& req, sensor_msgs::SetCameraInfo::Response& rsp)
00419 {
00420   return setCameraInfo(req, rsp, l_cam_, l_msg_camera_info_);
00421 }
00422 bool StereoNode::setCameraInfoR(sensor_msgs::SetCameraInfo::Request& req, sensor_msgs::SetCameraInfo::Response& rsp)
00423 {
00424   return setCameraInfo(req, rsp, r_cam_, r_msg_camera_info_);
00425 }
00426 
00427 // Try to load previously saved camera calibration from a file.
00428 void StereoNode::loadIntrinsics(Camera &cam, sensor_msgs::CameraInfo &msg_info)
00429 {
00430   char buffer[12800];
00431 
00432   std::string MyPath = config_path_ + "/" + configFileName(cam);
00433   std::fstream param_file;
00434   param_file.open(MyPath.c_str(), std::ios::in);
00435 
00436   if (param_file.is_open()) {
00437     param_file.read(buffer, 12800);
00438     param_file.close();
00439   }
00440 
00441   // Parse calibration file
00442   std::string camera_name;
00443   if (camera_calibration_parsers::parseCalibrationIni(buffer, camera_name, msg_info)) {
00444     ROS_INFO("Calibration : %s %u", camera_name.c_str(), cam.getCameraSerialNo());
00445   } else {
00446     ROS_WARN("Failed to load intrinsics for camera from file");
00447   }
00448 }
00449 
00450 // Add properties to image message
00451 sensor_msgs::ImagePtr StereoNode::processFrame(IplImage* frame, Camera &cam, cv_bridge::CvImage &converter,
00452                                                sensor_msgs::CameraInfoPtr &info, sensor_msgs::CameraInfo &msg_info)
00453 {
00454   msg_info.roi.x_offset = 0;
00455   msg_info.roi.y_offset = 0;
00456   msg_info.height = cam.getHeight();
00457   msg_info.width = cam.getWidth();
00458   msg_info.roi.width = 0;
00459   msg_info.roi.height = 0;
00460   sensor_msgs::CameraInfoPtr msg(new sensor_msgs::CameraInfo(msg_info));
00461   info = msg;
00462 
00463   converter.header = msg_info.header;
00464   converter.encoding = Camera::colorModeToString(cam.getColorMode());
00465   converter.image = frame;
00466   return converter.toImageMsg();
00467 }
00468 
00469 // Timestamp and publish the image. Called by the streaming thread.
00470 void StereoNode::publishImageL(IplImage * frame)
00471 {
00472   l_msg_camera_info_.header.seq++;
00473   l_stamp_ = ros::Time::now();
00474   double diff = (l_stamp_ - r_stamp_).toSec();
00475   if ((diff >= 0) && (diff < 0.02)) {
00476     l_msg_camera_info_.header = r_msg_camera_info_.header;
00477   } else {
00478     l_msg_camera_info_.header.stamp = l_stamp_;
00479   }
00480   sensor_msgs::CameraInfoPtr info;
00481   sensor_msgs::ImagePtr img = processFrame(frame, l_cam_, l_converter_, info, l_msg_camera_info_);
00482   l_pub_stream_.publish(img, info);
00483 }
00484 void StereoNode::publishImageR(IplImage * frame)
00485 {
00486   r_msg_camera_info_.header.seq++;
00487   r_stamp_ = ros::Time::now();
00488   double diff = (r_stamp_ - l_stamp_).toSec();
00489   if ((diff >= 0) && (diff < 0.02)) {
00490     r_msg_camera_info_.header = l_msg_camera_info_.header;
00491   } else {
00492     r_msg_camera_info_.header.stamp = r_stamp_;
00493   }
00494   sensor_msgs::CameraInfoPtr info;
00495   sensor_msgs::ImagePtr img = processFrame(frame, r_cam_, r_converter_, info, r_msg_camera_info_);
00496   r_pub_stream_.publish(img, info);
00497 }
00498 
00499 void StereoNode::startCamera()
00500 {
00501   if (running_ || !configured_)
00502     return;
00503   l_cam_.startVideoCapture(boost::bind(&StereoNode::publishImageL, this, _1));
00504   r_cam_.startVideoCapture(boost::bind(&StereoNode::publishImageR, this, _1));
00505   timer_force_trigger_.start();
00506   ROS_INFO("Started video stream.");
00507   running_ = true;
00508 }
00509 
00510 void StereoNode::stopCamera()
00511 {
00512   timer_force_trigger_.stop();
00513   if (!running_)
00514     return;
00515   ROS_INFO("Stopping video stream.");
00516   l_cam_.stopVideoCapture();
00517   r_cam_.stopVideoCapture();
00518   ROS_INFO("Stopped video stream.");
00519   running_ = false;
00520 }
00521 
00522 void StereoNode::closeCamera()
00523 {
00524   stopCamera();
00525   r_cam_.closeCamera();
00526   l_cam_.closeCamera();
00527 }
00528 
00529 } // namespace ueye


ueye
Author(s): Kevin Hallenbeck
autogenerated on Mon Oct 6 2014 08:20:20