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
00087
00088 diagnostics_(),
00089 topic_diagnostics_min_freq_(0.),
00090 topic_diagnostics_max_freq_(1000.),
00091 topic_diagnostics_("image_raw", diagnostics_,
00092 diagnostic_updater::FrequencyStatusParam
00093 (&topic_diagnostics_min_freq_,
00094 &topic_diagnostics_max_freq_, 0.1, 10),
00095 diagnostic_updater::TimeStampStatusParam
00096 ())
00097 {}
00098
00099 Camera1394Driver::~Camera1394Driver()
00100 {}
00101
00106 void Camera1394Driver::closeCamera()
00107 {
00108 if (state_ != Driver::CLOSED)
00109 {
00110 ROS_INFO_STREAM("[" << camera_name_ << "] closing device");
00111 dev_->close();
00112 state_ = Driver::CLOSED;
00113 }
00114 }
00115
00128 bool Camera1394Driver::openCamera(Config &newconfig)
00129 {
00130 bool success = false;
00131
00132 try
00133 {
00134 if (0 == dev_->open(newconfig))
00135 {
00136 if (camera_name_ != dev_->device_id_)
00137 {
00138 camera_name_ = dev_->device_id_;
00139 if (!cinfo_->setCameraName(camera_name_))
00140 {
00141
00142
00143 ROS_WARN_STREAM("[" << camera_name_
00144 << "] name not valid"
00145 << " for camera_info_manger");
00146 }
00147 }
00148 ROS_INFO_STREAM("[" << camera_name_ << "] opened: "
00149 << newconfig.video_mode << ", "
00150 << newconfig.frame_rate << " fps, "
00151 << newconfig.iso_speed << " Mb/s");
00152 state_ = Driver::OPENED;
00153 calibration_matches_ = true;
00154 newconfig.guid = camera_name_;
00155 retries_ = 0;
00156 success = true;
00157 }
00158 }
00159 catch (camera1394::Exception& e)
00160 {
00161 state_ = Driver::CLOSED;
00162 if (retries_++ > 0)
00163 ROS_DEBUG_STREAM("[" << camera_name_
00164 << "] exception opening device (retrying): "
00165 << e.what());
00166 else
00167 ROS_ERROR_STREAM("[" << camera_name_
00168 << "] device open failed: " << e.what());
00169 }
00170
00171
00172 diagnostics_.setHardwareID(camera_name_);
00173 double delta = newconfig.frame_rate * 0.1;
00174 topic_diagnostics_min_freq_ = newconfig.frame_rate - delta;
00175 topic_diagnostics_max_freq_ = newconfig.frame_rate + delta;
00176
00177 return success;
00178 }
00179
00180
00182 void Camera1394Driver::poll(void)
00183 {
00184
00185
00186
00187
00188
00189
00190
00191 bool do_sleep = true;
00192 if (!reconfiguring_)
00193 {
00194 boost::mutex::scoped_lock lock(mutex_);
00195 if (state_ == Driver::CLOSED)
00196 {
00197 openCamera(config_);
00198 }
00199 do_sleep = (state_ == Driver::CLOSED);
00200 if (!do_sleep)
00201 {
00202
00203 sensor_msgs::ImagePtr image(new sensor_msgs::Image);
00204 if (read(image))
00205 {
00206 publish(image);
00207 }
00208 }
00209 }
00210
00211
00212 diagnostics_.update();
00213
00214 if (do_sleep)
00215 {
00216
00217
00218 cycle_.sleep();
00219 }
00220 }
00221
00226 void Camera1394Driver::publish(const sensor_msgs::ImagePtr &image)
00227 {
00228 image->header.frame_id = config_.frame_id;
00229
00230
00231 sensor_msgs::CameraInfoPtr
00232 ci(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));
00233
00234
00235 if (!dev_->checkCameraInfo(*image, *ci))
00236 {
00237
00238
00239 if (calibration_matches_)
00240 {
00241
00242 calibration_matches_ = false;
00243 ROS_WARN_STREAM("[" << camera_name_
00244 << "] calibration does not match video mode "
00245 << "(publishing uncalibrated data)");
00246 }
00247 ci.reset(new sensor_msgs::CameraInfo());
00248 ci->height = image->height;
00249 ci->width = image->width;
00250 }
00251 else if (!calibration_matches_)
00252 {
00253
00254 calibration_matches_ = true;
00255 ROS_WARN_STREAM("[" << camera_name_
00256 << "] calibration matches video mode now");
00257 }
00258
00259
00260 dev_->setOperationalParameters(*ci);
00261
00262 ci->header.frame_id = config_.frame_id;
00263 ci->header.stamp = image->header.stamp;
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279 image_pub_.publish(image, ci);
00280
00281
00282
00283
00284
00285
00286 topic_diagnostics_.tick(image->header.stamp);
00287 }
00288
00294 bool Camera1394Driver::read(sensor_msgs::ImagePtr &image)
00295 {
00296 bool success = true;
00297 try
00298 {
00299
00300 ROS_DEBUG_STREAM("[" << camera_name_ << "] reading data");
00301 dev_->readData(*image);
00302 ROS_DEBUG_STREAM("[" << camera_name_ << "] read returned");
00303 }
00304 catch (camera1394::Exception& e)
00305 {
00306 ROS_WARN_STREAM("[" << camera_name_
00307 << "] Exception reading data: " << e.what());
00308 success = false;
00309 }
00310 return success;
00311 }
00312
00322 void Camera1394Driver::reconfig(Config &newconfig, uint32_t level)
00323 {
00324
00325
00326 reconfiguring_ = true;
00327 boost::mutex::scoped_lock lock(mutex_);
00328 ROS_DEBUG("dynamic reconfigure level 0x%x", level);
00329
00330
00331 if (newconfig.frame_id == "")
00332 newconfig.frame_id = "camera";
00333 std::string tf_prefix = tf::getPrefixParam(priv_nh_);
00334 ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
00335 newconfig.frame_id = tf::resolve(tf_prefix, newconfig.frame_id);
00336
00337 if (state_ != Driver::CLOSED && (level & Levels::RECONFIGURE_CLOSE))
00338 {
00339
00340 closeCamera();
00341 }
00342
00343 if (state_ == Driver::CLOSED)
00344 {
00345
00346 openCamera(newconfig);
00347 }
00348
00349 if (config_.camera_info_url != newconfig.camera_info_url)
00350 {
00351
00352 if (cinfo_->validateURL(newconfig.camera_info_url))
00353 {
00354 cinfo_->loadCameraInfo(newconfig.camera_info_url);
00355 }
00356 else
00357 {
00358
00359 newconfig.camera_info_url = config_.camera_info_url;
00360 }
00361 }
00362
00363 if (state_ != Driver::CLOSED)
00364 {
00365
00366 if (level & Levels::RECONFIGURE_CLOSE)
00367 {
00368
00369 if (false == dev_->features_->initialize(&newconfig))
00370 {
00371 ROS_ERROR_STREAM("[" << camera_name_
00372 << "] feature initialization failure");
00373 closeCamera();
00374 }
00375 }
00376 else
00377 {
00378
00379
00380 dev_->features_->reconfigure(&newconfig);
00381 }
00382 }
00383
00384 config_ = newconfig;
00385 reconfiguring_ = false;
00386
00387 ROS_DEBUG_STREAM("[" << camera_name_
00388 << "] reconfigured: frame_id " << newconfig.frame_id
00389 << ", camera_info_url " << newconfig.camera_info_url);
00390 }
00391
00392
00399 void Camera1394Driver::setup(void)
00400 {
00401 srv_.setCallback(boost::bind(&Camera1394Driver::reconfig, this, _1, _2));
00402 }
00403
00404
00406 void Camera1394Driver::shutdown(void)
00407 {
00408 closeCamera();
00409 }
00410
00411 };
camera1394
Author(s): Jack O'Quin, Ken Tossell, Patrick Beeson,
Nate Koenig, Andrew Howard,
Damien Douxchamps, Dan Dennedy
Tomas Petricek / petrito1@cmp.felk.cvut.cz
autogenerated on Tue Dec 10 2013 14:25:10