driver1394stereo.cpp
Go to the documentation of this file.
00001 // $Id: driver1394.cpp 35691 2011-02-02 04:28:58Z joq $
00002 
00003 /*********************************************************************
00004 * Software License Agreement (BSD License)
00005 *
00006 *  Copyright (C) 2009, 2010 Jack O'Quin, Patrick Beeson
00007 *  All rights reserved.
00008 *
00009 *  Redistribution and use in source and binary forms, with or without
00010 *  modification, are permitted provided that the following conditions
00011 *  are met:
00012 *
00013 *   * Redistributions of source code must retain the above copyright
00014 *     notice, this list of conditions and the following disclaimer.
00015 *   * Redistributions in binary form must reproduce the above
00016 *     copyright notice, this list of conditions and the following
00017 *     disclaimer in the documentation and/or other materials provided
00018 *     with the distribution.
00019 *   * Neither the name of the author nor other contributors may be
00020 *     used to endorse or promote products derived from this software
00021 *     without specific prior written permission.
00022 *
00023 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 *  POSSIBILITY OF SUCH DAMAGE.
00035 *********************************************************************/
00036 
00037 #include <boost/format.hpp>
00038 
00039 #include <driver_base/SensorLevels.h>
00040 #include <tf/transform_listener.h>
00041 
00042 #include "driver1394stereo.h"
00043 #include "camera1394stereo/Camera1394StereoConfig.h"
00044 #include "featuresstereo.h"
00045 
00071 namespace camera1394stereo_driver
00072 {
00073   // some convenience typedefs
00074   typedef camera1394stereo::Camera1394StereoConfig Config;
00075   typedef driver_base::Driver Driver;
00076   typedef driver_base::SensorLevels Levels;
00077 
00078   const std::string Camera1394StereoDriver::CameraSelectorString[NUM_CAMERAS] = {"left","right"};
00079 
00080   Camera1394StereoDriver::Camera1394StereoDriver(ros::NodeHandle priv_nh,
00081                                                  ros::NodeHandle camera_nh):
00082     state_(Driver::CLOSED),
00083     reconfiguring_(false),
00084     priv_nh_(priv_nh),
00085     camera_nh_(camera_nh),
00086     camera_name_("stereo_camera"),
00087     dev_(new camera1394stereo::Camera1394Stereo()),
00088     srv_(priv_nh),
00089     cycle_(1.0),                        // slow poll when closed
00090     it_(new image_transport::ImageTransport(camera_nh_))
00091   {
00092     for (int i=0; i< NUM_CAMERAS; i++)
00093     {
00094       single_camera_nh_[i] = ros::NodeHandle(camera_nh_,CameraSelectorString[i]);  // for i-th CameraInfoManager
00095       cinfo_[i] = boost::shared_ptr<camera_info_manager::CameraInfoManager>(new camera_info_manager::CameraInfoManager(single_camera_nh_[i]));
00096       calibration_matches_[i] = true;
00097       image_pub_[i] = it_->advertiseCamera(CameraSelectorString[i]+"/image_raw", 1);
00098     }
00099   }
00100 
00101   Camera1394StereoDriver::~Camera1394StereoDriver()
00102   {}
00103 
00108   void Camera1394StereoDriver::closeCamera()
00109   {
00110     if (state_ != Driver::CLOSED)
00111       {
00112         ROS_INFO_STREAM("[" << camera_name_ << "] closing device");
00113         dev_->close();
00114         state_ = Driver::CLOSED;
00115       }
00116   }
00117 
00127   bool Camera1394StereoDriver::openCamera(Config &newconfig)
00128   {
00129     bool success = false;
00130     int retries = 2;                    // number of retries, if open fails
00131     do
00132       {
00133         try
00134           {
00135             if (0 == dev_->open(newconfig))
00136               {
00137                 if (camera_name_ != dev_->device_id_)
00138                   {
00139                     camera_name_ = dev_->device_id_;
00140                     for (int i=0; i<NUM_CAMERAS; i++)
00141                       if ( ! cinfo_[i]->setCameraName(camera_name_ + "_" +
00142                                                       CameraSelectorString[i] ) )
00143                         {
00144                           // GUID is 16 hex digits, which should be valid.
00145                           // If not, use it for log messages anyway.
00146                           ROS_WARN_STREAM("[" << camera_name_
00147                                           <<"_"<<CameraSelectorString[i]
00148                                           << "] name not valid"
00149                                           << " for camera_info_manger");
00150                         }
00151                   }
00152                 ROS_INFO_STREAM("[" << camera_name_ << "] opened: "
00153                                 << newconfig.video_mode << ", "
00154                                 << newconfig.frame_rate << " fps, "
00155                                 << newconfig.iso_speed << " Mb/s");
00156                 state_ = Driver::OPENED;
00157                 for (int i=0; i<NUM_CAMERAS; i++)
00158                   calibration_matches_[i] = true;
00159                 success = true;
00160               }
00161           }
00162         catch (camera1394stereo::Exception& e)
00163           {
00164             state_ = Driver::CLOSED;    // since the open() failed
00165             if (retries > 0)
00166               {
00167                 ROS_WARN_STREAM("[" << camera_name_
00168                                 << "] exception opening device (retrying): "
00169                                 << e.what());
00170                 ROS_WARN_STREAM("Trying to reset bus with system call...");
00171                 if ( system("dc1394_reset_bus") == 0 )
00172                   ROS_WARN_STREAM("Bus reset system call successful");
00173                 else
00174                   ROS_WARN_STREAM("Bus reset system call failure");
00175               }
00176             else
00177               ROS_ERROR_STREAM("[" << camera_name_
00178                                << "] device open failed: " << e.what());
00179           }
00180       }
00181     while (!success && --retries >= 0);
00182 
00183     return success;
00184   }
00185 
00186 
00188   void Camera1394StereoDriver::poll(void)
00189   {
00190     // Do not run concurrently with reconfig().
00191     //
00192     // The mutex lock should be sufficient, but the Linux pthreads
00193     // implementation does not guarantee fairness, and the reconfig()
00194     // callback thread generally suffers from lock starvation for many
00195     // seconds before getting to run.  So, we avoid acquiring the lock
00196     // if there is a reconfig() pending.
00197     bool do_sleep = true;
00198     if (!reconfiguring_)
00199       {
00200         boost::mutex::scoped_lock lock(mutex_);
00201         do_sleep = (state_ == Driver::CLOSED);
00202         if (!do_sleep)
00203           {
00204             // driver is open, read the next image still holding lock
00205             sensor_msgs::ImagePtr image[NUM_CAMERAS];
00206             for (int i=0; i<NUM_CAMERAS; i++)
00207               image[i] = sensor_msgs::ImagePtr(new sensor_msgs::Image);
00208             if (read(image))
00209               {
00210                 publish(image);
00211               }
00212           }
00213       }
00214 
00215     if (do_sleep)
00216       {
00217         // device was closed or poll is not running, sleeping avoids
00218         // busy wait (DO NOT hold the lock while sleeping)
00219         cycle_.sleep();
00220       }
00221   }
00222 
00227   void Camera1394StereoDriver::publish(const sensor_msgs::ImagePtr image[NUM_CAMERAS])
00228   {
00229     for (int i=0; i<NUM_CAMERAS; i++)
00230       {
00231         image[i]->header.frame_id = config_.frame_id;
00232 
00233         // get current CameraInfo data
00234         sensor_msgs::CameraInfoPtr
00235           ci(new sensor_msgs::CameraInfo(cinfo_[i]->getCameraInfo()));
00236 
00237         // check whether CameraInfo matches current video mode
00238         if (!dev_->checkCameraInfo(*(image[i]), *ci))
00239           {
00240             // image size does not match: publish a matching uncalibrated
00241             // CameraInfo instead
00242             if (calibration_matches_[i])
00243               {
00244                 // warn user once
00245                 calibration_matches_[i] = false;
00246                 ROS_WARN_STREAM("[" << camera_name_
00247                                 << "_" << CameraSelectorString[i]
00248                                 << "] calibration does not match video mode "
00249                                 << "(publishing uncalibrated data)");
00250               }
00251             ci.reset(new sensor_msgs::CameraInfo());
00252             ci->height = image[i]->height;
00253             ci->width = image[i]->width;
00254           }
00255         else if (!calibration_matches_[i])
00256           {
00257             // calibration OK now
00258             calibration_matches_[i] = true;
00259             ROS_WARN_STREAM("[" << camera_name_
00260                             << "_" << CameraSelectorString[i]
00261                             << "] calibration matches video mode now");
00262           }
00263 
00264         // fill in operational parameters
00265         dev_->setOperationalParameters(*ci);
00266 
00267         ci->header.frame_id = config_.frame_id;
00268         ci->header.stamp = image[i]->header.stamp;
00269 
00270         // @todo log a warning if (filtered) time since last published
00271         // image is not reasonably close to configured frame_rate
00272 
00273         // Publish via image_transport
00274         image_pub_[i].publish(image[i], ci);
00275       }
00276   }
00277 
00283   bool Camera1394StereoDriver::read(sensor_msgs::ImagePtr image[NUM_CAMERAS])
00284   {
00285     bool success = true;
00286     try
00287       {
00288         // Read data from the Camera
00289         ROS_DEBUG_STREAM("[" << camera_name_ << "] reading data");
00290         dev_->readData(*(image[LEFT]), *(image[RIGHT]));
00291         ROS_DEBUG_STREAM("[" << camera_name_ << "] read returned");
00292       }
00293     catch (camera1394stereo::Exception& e)
00294       {
00295         ROS_WARN_STREAM("[" << camera_name_
00296                         << "] Exception reading data: " << e.what());
00297         success = false;
00298       }
00299     return success;
00300   }
00301 
00311   void Camera1394StereoDriver::reconfig(Config &newconfig, uint32_t level)
00312   {
00313     // Do not run concurrently with poll().  Tell it to stop running,
00314     // and wait on the lock until it does.
00315     reconfiguring_ = true;
00316     boost::mutex::scoped_lock lock(mutex_);
00317     ROS_DEBUG("dynamic reconfigure level 0x%x", level);
00318 
00319     // resolve frame ID using tf_prefix parameter
00320     if (newconfig.frame_id == "")
00321       newconfig.frame_id = "camera";
00322     std::string tf_prefix = tf::getPrefixParam(priv_nh_);
00323     ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
00324     newconfig.frame_id = tf::resolve(tf_prefix, newconfig.frame_id);
00325 
00326     if (state_ != Driver::CLOSED && (level & Levels::RECONFIGURE_CLOSE))
00327       {
00328         // must close the device before updating these parameters
00329         closeCamera();                  // state_ --> CLOSED
00330       }
00331 
00332     if (state_ == Driver::CLOSED)
00333       {
00334         // open with new values
00335         if (openCamera(newconfig))
00336           {
00337             // update GUID string parameter
00338             // TODO move into dev_->open()
00339             newconfig.guid = camera_name_;
00340           }
00341       }
00342 
00343     std::string camera_info_url[NUM_CAMERAS];
00344     camera_info_url[LEFT] = config_.camera_info_url_left;
00345     camera_info_url[RIGHT] = config_.camera_info_url_right;
00346     std::string new_camera_info_url[NUM_CAMERAS];
00347     new_camera_info_url[LEFT] = newconfig.camera_info_url_left;
00348     new_camera_info_url[RIGHT] = newconfig.camera_info_url_right;
00349 
00350     for (int i=0; i<NUM_CAMERAS; i++)
00351       {
00352         if (camera_info_url[i] != new_camera_info_url[i])
00353           {
00354             // set the new URL and load CameraInfo (if any) from it
00355             if (cinfo_[i]->validateURL(new_camera_info_url[i]))
00356               {
00357                 cinfo_[i]->loadCameraInfo(new_camera_info_url[i]);
00358               }
00359             else
00360               {
00361                 // new URL not valid, use the old one
00362                 ROS_WARN_STREAM("[" << camera_name_
00363                                     << "_" << CameraSelectorString[i]
00364                                     << "] not updating calibration to invalid URL "
00365                                     << new_camera_info_url[i] );
00366               }
00367           }
00368        }
00369     if (state_ != Driver::CLOSED)       // openCamera() succeeded?
00370       {
00371         // configure IIDC features
00372         if (level & Levels::RECONFIGURE_CLOSE)
00373           {
00374             // initialize all features for newly opened device
00375             if (false == dev_->features_->initialize(&newconfig))
00376               {
00377                 ROS_ERROR_STREAM("[" << camera_name_
00378                                  << "] feature initialization failure");
00379                 closeCamera();          // can't continue
00380               }
00381           }
00382         else
00383           {
00384             // update any features that changed
00385             // TODO replace this with a dev_->reconfigure(&newconfig);
00386             dev_->features_->reconfigure(&newconfig);
00387           }
00388       }
00389 
00390     config_ = newconfig;                // save new parameters
00391     reconfiguring_ = false;             // let poll() run again
00392 
00393     ROS_DEBUG_STREAM("[" << camera_name_
00394                      << "] reconfigured: frame_id " << newconfig.frame_id
00395                      << ", camera_info_url_left " << newconfig.camera_info_url_left
00396                      << ", camera_info_url_right " << newconfig.camera_info_url_right);
00397   }
00398 
00399 
00406   void Camera1394StereoDriver::setup(void)
00407   {
00408     srv_.setCallback(boost::bind(&Camera1394StereoDriver::reconfig, this, _1, _2));
00409   }
00410 
00411 
00413   void Camera1394StereoDriver::shutdown(void)
00414   {
00415     closeCamera();
00416   }
00417 
00418 }; // end namespace camera1394_driver


camera1394stereo
Author(s): Joan Pau Beltran
autogenerated on Thu Aug 27 2015 12:39:08