driver1394.cpp
Go to the documentation of this file.
00001 // $Id$
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 "driver1394.h"
00043 #include "camera1394/Camera1394Config.h"
00044 #include "features.h"
00045 
00064 namespace camera1394_driver
00065 {
00066   // some convenience typedefs
00067   typedef camera1394::Camera1394Config Config;
00068   typedef driver_base::Driver Driver;
00069   typedef driver_base::SensorLevels Levels;
00070 
00071   Camera1394Driver::Camera1394Driver(ros::NodeHandle priv_nh,
00072                                      ros::NodeHandle camera_nh):
00073     state_(Driver::CLOSED),
00074     reconfiguring_(false),
00075     priv_nh_(priv_nh),
00076     camera_nh_(camera_nh),
00077     camera_name_("camera"),
00078     cycle_(1.0),                        // slow poll when closed
00079     retries_(0),
00080     dev_(new camera1394::Camera1394()),
00081     srv_(priv_nh),
00082     cinfo_(new camera_info_manager::CameraInfoManager(camera_nh_)),
00083     calibration_matches_(true),
00084     it_(new image_transport::ImageTransport(camera_nh_)),
00085     image_pub_(it_->advertiseCamera("image_raw", 1)),
00086     diagnostics_(),
00087     topic_diagnostics_min_freq_(0.),
00088     topic_diagnostics_max_freq_(1000.),
00089     topic_diagnostics_("image_raw", diagnostics_,
00090                        diagnostic_updater::FrequencyStatusParam
00091                        (&topic_diagnostics_min_freq_,
00092                         &topic_diagnostics_max_freq_, 0.1, 10),
00093                        diagnostic_updater::TimeStampStatusParam())
00094   {}
00095 
00096   Camera1394Driver::~Camera1394Driver()
00097   {}
00098 
00103   void Camera1394Driver::closeCamera()
00104   {
00105     if (state_ != Driver::CLOSED)
00106       {
00107         ROS_INFO_STREAM("[" << camera_name_ << "] closing device");
00108         dev_->close();
00109         state_ = Driver::CLOSED;
00110       }
00111   }
00112 
00125   bool Camera1394Driver::openCamera(Config &newconfig)
00126   {
00127     bool success = false;
00128 
00129     try
00130       {
00131         if (0 == dev_->open(newconfig))
00132           {
00133             if (camera_name_ != dev_->device_id_)
00134               {
00135                 camera_name_ = dev_->device_id_;
00136                 if (!cinfo_->setCameraName(camera_name_))
00137                   {
00138                     // GUID is 16 hex digits, which should be valid.
00139                     // If not, use it for log messages anyway.
00140                     ROS_WARN_STREAM("[" << camera_name_
00141                                     << "] name not valid"
00142                                     << " for camera_info_manger");
00143                   }
00144               }
00145             ROS_INFO_STREAM("[" << camera_name_ << "] opened: "
00146                             << newconfig.video_mode << ", "
00147                             << newconfig.frame_rate << " fps, "
00148                             << newconfig.iso_speed << " Mb/s");
00149             state_ = Driver::OPENED;
00150             calibration_matches_ = true;
00151             newconfig.guid = camera_name_; // update configuration parameter
00152             retries_ = 0;
00153             success = true;
00154           }
00155       }
00156     catch (camera1394::Exception& e)
00157       {
00158         state_ = Driver::CLOSED;    // since the open() failed
00159         if (retries_++ > 0)
00160           ROS_DEBUG_STREAM("[" << camera_name_
00161                            << "] exception opening device (retrying): "
00162                            << e.what());
00163         else
00164           ROS_ERROR_STREAM("[" << camera_name_
00165                            << "] device open failed: " << e.what());
00166       }
00167 
00168     // update diagnostics parameters
00169     diagnostics_.setHardwareID(camera_name_);
00170     double delta = newconfig.frame_rate * 0.1; // allow 10% error margin
00171     topic_diagnostics_min_freq_ = newconfig.frame_rate - delta;
00172     topic_diagnostics_max_freq_ = newconfig.frame_rate + delta;
00173 
00174     return success;
00175   }
00176 
00177 
00179   void Camera1394Driver::poll(void)
00180   {
00181     // Do not run concurrently with reconfig().
00182     //
00183     // The mutex lock should be sufficient, but the Linux pthreads
00184     // implementation does not guarantee fairness, and the reconfig()
00185     // callback thread generally suffers from lock starvation for many
00186     // seconds before getting to run.  So, we avoid acquiring the lock
00187     // if there is a reconfig() pending.
00188     bool do_sleep = true;
00189     if (!reconfiguring_)
00190       {
00191         boost::mutex::scoped_lock lock(mutex_);
00192         if (state_ == Driver::CLOSED)
00193           {
00194             openCamera(config_);        // open with current configuration
00195           }
00196         do_sleep = (state_ == Driver::CLOSED);
00197         if (!do_sleep)                  // openCamera() succeeded?
00198           {
00199             // driver is open, read the next image still holding lock
00200             sensor_msgs::ImagePtr image(new sensor_msgs::Image);
00201             if (read(image))
00202               {
00203                 publish(image);
00204               }
00205           }
00206       } // release mutex lock
00207 
00208     // Always run the diagnostics updater: no lock required.
00209     diagnostics_.update();
00210 
00211     if (do_sleep)
00212       {
00213         // device was closed or poll is not running, sleeping avoids
00214         // busy wait (DO NOT hold the lock while sleeping)
00215         cycle_.sleep();
00216       }
00217   }
00218 
00223   void Camera1394Driver::publish(const sensor_msgs::ImagePtr &image)
00224   {
00225     image->header.frame_id = config_.frame_id;
00226 
00227     // get current CameraInfo data
00228     sensor_msgs::CameraInfoPtr
00229       ci(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));
00230 
00231     // check whether CameraInfo matches current video mode
00232     if (!dev_->checkCameraInfo(*image, *ci))
00233       {
00234         // image size does not match: publish a matching uncalibrated
00235         // CameraInfo instead
00236         if (calibration_matches_)
00237           {
00238             // warn user once
00239             calibration_matches_ = false;
00240             ROS_WARN_STREAM("[" << camera_name_
00241                             << "] calibration does not match video mode "
00242                             << "(publishing uncalibrated data)");
00243           }
00244         ci.reset(new sensor_msgs::CameraInfo());
00245         ci->height = image->height;
00246         ci->width = image->width;
00247       }
00248     else if (!calibration_matches_)
00249       {
00250         // calibration OK now
00251         calibration_matches_ = true;
00252         ROS_WARN_STREAM("[" << camera_name_
00253                         << "] calibration matches video mode now");
00254       }
00255 
00256     // fill in operational parameters
00257     dev_->setOperationalParameters(*ci);
00258 
00259     ci->header.frame_id = config_.frame_id;
00260     ci->header.stamp = image->header.stamp;
00261 
00262     // Publish via image_transport
00263     image_pub_.publish(image, ci);
00264 
00265     // Notify diagnostics that a message has been published. That will
00266     // generate a warning if messages are not published at nearly the
00267     // configured frame_rate.
00268     topic_diagnostics_.tick(image->header.stamp);
00269   }
00270 
00276   bool Camera1394Driver::read(sensor_msgs::ImagePtr &image)
00277   {
00278     bool success = true;
00279     try
00280       {
00281         // Read data from the Camera
00282         ROS_DEBUG_STREAM("[" << camera_name_ << "] reading data");
00283         dev_->readData(*image);
00284         ROS_DEBUG_STREAM("[" << camera_name_ << "] read returned");
00285       }
00286     catch (camera1394::Exception& e)
00287       {
00288         ROS_WARN_STREAM("[" << camera_name_
00289                         << "] Exception reading data: " << e.what());
00290         success = false;
00291       }
00292     return success;
00293   }
00294 
00304   void Camera1394Driver::reconfig(Config &newconfig, uint32_t level)
00305   {
00306     // Do not run concurrently with poll().  Tell it to stop running,
00307     // and wait on the lock until it does.
00308     reconfiguring_ = true;
00309     boost::mutex::scoped_lock lock(mutex_);
00310     ROS_DEBUG("dynamic reconfigure level 0x%x", level);
00311 
00312     // resolve frame ID using tf_prefix parameter
00313     if (newconfig.frame_id == "")
00314       newconfig.frame_id = "camera";
00315     std::string tf_prefix = tf::getPrefixParam(priv_nh_);
00316     ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
00317     newconfig.frame_id = tf::resolve(tf_prefix, newconfig.frame_id);
00318 
00319     if (state_ != Driver::CLOSED && (level & Levels::RECONFIGURE_CLOSE))
00320       {
00321         // must close the device before updating these parameters
00322         closeCamera();                  // state_ --> CLOSED
00323       }
00324 
00325     if (state_ == Driver::CLOSED)
00326       {
00327         // open with new values
00328         openCamera(newconfig);
00329       }
00330 
00331     if (config_.camera_info_url != newconfig.camera_info_url)
00332       {
00333         // set the new URL and load CameraInfo (if any) from it
00334         if (cinfo_->validateURL(newconfig.camera_info_url))
00335           {
00336             cinfo_->loadCameraInfo(newconfig.camera_info_url);
00337           }
00338         else
00339           {
00340             // new URL not valid, use the old one
00341             newconfig.camera_info_url = config_.camera_info_url;
00342           }
00343       }
00344 
00345     if (state_ != Driver::CLOSED)       // openCamera() succeeded?
00346       {
00347         // configure IIDC features
00348         if (level & Levels::RECONFIGURE_CLOSE)
00349           {
00350             // initialize all features for newly opened device
00351             if (false == dev_->features_->initialize(&newconfig))
00352               {
00353                 ROS_ERROR_STREAM("[" << camera_name_
00354                                  << "] feature initialization failure");
00355                 closeCamera();          // can't continue
00356               }
00357           }
00358         else
00359           {
00360             // update any features that changed
00361             // TODO replace this with a dev_->reconfigure(&newconfig);
00362             dev_->features_->reconfigure(&newconfig);
00363           }
00364       }
00365 
00366     config_ = newconfig;                // save new parameters
00367     reconfiguring_ = false;             // let poll() run again
00368 
00369     ROS_DEBUG_STREAM("[" << camera_name_
00370                      << "] reconfigured: frame_id " << newconfig.frame_id
00371                      << ", camera_info_url " << newconfig.camera_info_url);
00372   }
00373 
00374 
00381   void Camera1394Driver::setup(void)
00382   {
00383     srv_.setCallback(boost::bind(&Camera1394Driver::reconfig, this, _1, _2));
00384   }
00385 
00386 
00388   void Camera1394Driver::shutdown(void)
00389   {
00390     closeCamera();
00391   }
00392 
00393 }; // end namespace camera1394_driver


camera1394
Author(s): Jack O'Quin, Ken Tossell, Patrick Beeson, Nate Koenig, Andrew Howard, Damien Douxchamps, Dan Dennedy
autogenerated on Sun Oct 5 2014 22:42:12