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


ueye
Author(s): Kevin Hallenbeck
autogenerated on Tue Jan 7 2014 11:40:31