Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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
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),
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
00139
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_;
00152 retries_ = 0;
00153 success = true;
00154 }
00155 }
00156 catch (camera1394::Exception& e)
00157 {
00158 state_ = Driver::CLOSED;
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
00169 diagnostics_.setHardwareID(camera_name_);
00170 double delta = newconfig.frame_rate * 0.1;
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
00182
00183
00184
00185
00186
00187
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_);
00195 }
00196 do_sleep = (state_ == Driver::CLOSED);
00197 if (!do_sleep)
00198 {
00199
00200 sensor_msgs::ImagePtr image(new sensor_msgs::Image);
00201 if (read(image))
00202 {
00203 publish(image);
00204 }
00205 }
00206 }
00207
00208
00209 diagnostics_.update();
00210
00211 if (do_sleep)
00212 {
00213
00214
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
00228 sensor_msgs::CameraInfoPtr
00229 ci(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));
00230
00231
00232 if (!dev_->checkCameraInfo(*image, *ci))
00233 {
00234
00235
00236 if (calibration_matches_)
00237 {
00238
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
00251 calibration_matches_ = true;
00252 ROS_WARN_STREAM("[" << camera_name_
00253 << "] calibration matches video mode now");
00254 }
00255
00256
00257 dev_->setOperationalParameters(*ci);
00258
00259 ci->header.frame_id = config_.frame_id;
00260 ci->header.stamp = image->header.stamp;
00261
00262
00263 image_pub_.publish(image, ci);
00264
00265
00266
00267
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
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
00307
00308 reconfiguring_ = true;
00309 boost::mutex::scoped_lock lock(mutex_);
00310 ROS_DEBUG("dynamic reconfigure level 0x%x", level);
00311
00312
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
00322 closeCamera();
00323 }
00324
00325 if (state_ == Driver::CLOSED)
00326 {
00327
00328 openCamera(newconfig);
00329 }
00330
00331 if (config_.camera_info_url != newconfig.camera_info_url)
00332 {
00333
00334 if (cinfo_->validateURL(newconfig.camera_info_url))
00335 {
00336 cinfo_->loadCameraInfo(newconfig.camera_info_url);
00337 }
00338 else
00339 {
00340
00341 newconfig.camera_info_url = config_.camera_info_url;
00342 }
00343 }
00344
00345 if (state_ != Driver::CLOSED)
00346 {
00347
00348 if (level & Levels::RECONFIGURE_CLOSE)
00349 {
00350
00351 if (false == dev_->features_->initialize(&newconfig))
00352 {
00353 ROS_ERROR_STREAM("[" << camera_name_
00354 << "] feature initialization failure");
00355 closeCamera();
00356 }
00357 }
00358 else
00359 {
00360
00361
00362 dev_->features_->reconfigure(&newconfig);
00363 }
00364 }
00365
00366 config_ = newconfig;
00367 reconfiguring_ = false;
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 };
camera1394
Author(s): Jack O'Quin, Ken Tossell, Patrick Beeson, Nate Koenig, Andrew Howard, Damien Douxchamps, Dan Dennedy
autogenerated on Wed Aug 26 2015 10:56:59