00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00031
00032 #include "ros/ros.h"
00033 #include <pluginlib/class_list_macros.h>
00034 #include <nodelet/nodelet.h>
00035
00036 #include "pointgrey_camera_driver/PointGreyCamera.h"
00037
00038 #include <image_transport/image_transport.h>
00039 #include <camera_info_manager/camera_info_manager.h>
00040 #include <sensor_msgs/CameraInfo.h>
00041
00042 #include <wfov_camera_msgs/WFOVImage.h>
00043 #include <image_exposure_msgs/ExposureSequence.h>
00044
00045 #include <diagnostic_updater/diagnostic_updater.h>
00046 #include <diagnostic_updater/publisher.h>
00047
00048 #include <boost/thread.hpp>
00049
00050 #include <dynamic_reconfigure/server.h>
00051
00052 #include <fstream>
00053
00054 namespace pointgrey_camera_driver
00055 {
00056
00057 class PointGreyCameraNodelet: public nodelet::Nodelet
00058 {
00059 public:
00060 PointGreyCameraNodelet() {}
00061
00062 ~PointGreyCameraNodelet()
00063 {
00064 boost::mutex::scoped_lock scopedLock(connect_mutex_);
00065
00066 if(pubThread_)
00067 {
00068 pubThread_->interrupt();
00069 pubThread_->join();
00070
00071 try
00072 {
00073 NODELET_DEBUG("Stopping camera capture.");
00074 pg_.stop();
00075 NODELET_DEBUG("Disconnecting from camera.");
00076 pg_.disconnect();
00077 }
00078 catch(std::runtime_error& e)
00079 {
00080 NODELET_ERROR("%s", e.what());
00081 }
00082 }
00083 }
00084
00085 private:
00093 void paramCallback(pointgrey_camera_driver::PointGreyConfig &config, uint32_t level)
00094 {
00095 config_ = config;
00096
00097 try
00098 {
00099 NODELET_DEBUG("Dynamic reconfigure callback with level: %d", level);
00100 pg_.setNewConfiguration(config, level);
00101
00102
00103 gain_ = config.gain;
00104 wb_blue_ = config.white_balance_blue;
00105 wb_red_ = config.white_balance_red;
00106
00107
00108 binning_x_ = 1;
00109 binning_y_ = 1;
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129 if(config.video_mode == "format7_mode0" || config.video_mode == "format7_mode1" || config.video_mode == "format7_mode2")
00130 {
00131 roi_x_offset_ = config.format7_x_offset;
00132 roi_y_offset_ = config.format7_y_offset;
00133 roi_width_ = config.format7_roi_width;
00134 roi_height_ = config.format7_roi_height;
00135 do_rectify_ = true;
00136 }
00137 else
00138 {
00139
00140 roi_x_offset_ = 0;
00141 roi_y_offset_ = 0;
00142 roi_height_ = 0;
00143 roi_width_ = 0;
00144 do_rectify_ = false;
00145 }
00146 }
00147 catch(std::runtime_error& e)
00148 {
00149 NODELET_ERROR("Reconfigure Callback failed with error: %s", e.what());
00150 }
00151 }
00152
00158 void connectCb()
00159 {
00160 NODELET_DEBUG("Connect callback!");
00161 boost::mutex::scoped_lock scopedLock(connect_mutex_);
00162
00163 if(it_pub_.getNumSubscribers() == 0 && pub_->getPublisher().getNumSubscribers() == 0)
00164 {
00165 if (pubThread_)
00166 {
00167 NODELET_DEBUG("Disconnecting.");
00168 pubThread_->interrupt();
00169 scopedLock.unlock();
00170 pubThread_->join();
00171 scopedLock.lock();
00172 pubThread_.reset();
00173 sub_.shutdown();
00174
00175 try
00176 {
00177 NODELET_DEBUG("Stopping camera capture.");
00178 pg_.stop();
00179 }
00180 catch(std::runtime_error& e)
00181 {
00182 NODELET_ERROR("%s", e.what());
00183 }
00184
00185 try
00186 {
00187 NODELET_DEBUG("Disconnecting from camera.");
00188 pg_.disconnect();
00189 }
00190 catch(std::runtime_error& e)
00191 {
00192 NODELET_ERROR("%s", e.what());
00193 }
00194 }
00195 }
00196 else if(!pubThread_)
00197 {
00198
00199 pubThread_.reset(new boost::thread(boost::bind(&pointgrey_camera_driver::PointGreyCameraNodelet::devicePoll, this)));
00200 }
00201 else
00202 {
00203 NODELET_DEBUG("Do nothing in callback.");
00204 }
00205 }
00206
00212 void onInit()
00213 {
00214
00215 ros::NodeHandle &nh = getMTNodeHandle();
00216 ros::NodeHandle &pnh = getMTPrivateNodeHandle();
00217
00218
00219 int serial = 0;
00220
00221 XmlRpc::XmlRpcValue serial_xmlrpc;
00222 pnh.getParam("serial", serial_xmlrpc);
00223 if (serial_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeInt)
00224 {
00225 pnh.param<int>("serial", serial, 0);
00226 }
00227 else if (serial_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString)
00228 {
00229 std::string serial_str;
00230 pnh.param<std::string>("serial", serial_str, "0");
00231 std::istringstream(serial_str) >> serial;
00232 }
00233 else
00234 {
00235 NODELET_DEBUG("Serial XMLRPC type.");
00236 serial = 0;
00237 }
00238
00239 std::string camera_serial_path;
00240 pnh.param<std::string>("camera_serial_path", camera_serial_path, "");
00241 NODELET_INFO("Camera serial path %s", camera_serial_path.c_str());
00242
00243
00244 while (serial == 0 && !camera_serial_path.empty())
00245 {
00246 serial = readSerialAsHexFromFile(camera_serial_path);
00247 if (serial == 0)
00248 {
00249 NODELET_WARN("Waiting for camera serial path to become available");
00250 ros::Duration(1.0).sleep();
00251 }
00252 }
00253
00254 NODELET_INFO("Using camera serial %d", serial);
00255
00256 pg_.setDesiredCamera((uint32_t)serial);
00257
00258
00259 pnh.param<int>("packet_size", packet_size_, 1400);
00260 pnh.param<bool>("auto_packet_size", auto_packet_size_, true);
00261 pnh.param<int>("packet_delay", packet_delay_, 4000);
00262
00263
00264 pg_.setGigEParameters(auto_packet_size_, packet_size_, packet_delay_);
00265
00266
00267 std::string camera_info_url;
00268 pnh.param<std::string>("camera_info_url", camera_info_url, "");
00269
00270 pnh.param<std::string>("frame_id", frame_id_, "camera");
00271
00272 boost::mutex::scoped_lock scopedLock(connect_mutex_);
00273
00274
00275 srv_ = boost::make_shared <dynamic_reconfigure::Server<pointgrey_camera_driver::PointGreyConfig> > (pnh);
00276 dynamic_reconfigure::Server<pointgrey_camera_driver::PointGreyConfig>::CallbackType f =
00277 boost::bind(&pointgrey_camera_driver::PointGreyCameraNodelet::paramCallback, this, _1, _2);
00278 srv_->setCallback(f);
00279
00280
00281 std::stringstream cinfo_name;
00282 cinfo_name << serial;
00283 cinfo_.reset(new camera_info_manager::CameraInfoManager(nh, cinfo_name.str(), camera_info_url));
00284
00285
00286 it_.reset(new image_transport::ImageTransport(nh));
00287 image_transport::SubscriberStatusCallback cb = boost::bind(&PointGreyCameraNodelet::connectCb, this);
00288 it_pub_ = it_->advertiseCamera("image_raw", 5, cb, cb);
00289
00290
00291 updater_.setHardwareID("pointgrey_camera " + cinfo_name.str());
00292
00293
00294 double desired_freq;
00295 pnh.param<double>("desired_freq", desired_freq, 7.0);
00296 pnh.param<double>("min_freq", min_freq_, desired_freq);
00297 pnh.param<double>("max_freq", max_freq_, desired_freq);
00298 double freq_tolerance;
00299 pnh.param<double>("freq_tolerance", freq_tolerance, 0.1);
00300 int window_size;
00301 pnh.param<int>("window_size", window_size, 100);
00302 double min_acceptable;
00303 pnh.param<double>("min_acceptable_delay", min_acceptable, 0.0);
00304 double max_acceptable;
00305 pnh.param<double>("max_acceptable_delay", max_acceptable, 0.2);
00306 ros::SubscriberStatusCallback cb2 = boost::bind(&PointGreyCameraNodelet::connectCb, this);
00307 pub_.reset(new diagnostic_updater::DiagnosedPublisher<wfov_camera_msgs::WFOVImage>(nh.advertise<wfov_camera_msgs::WFOVImage>("image", 5, cb2, cb2),
00308 updater_,
00309 diagnostic_updater::FrequencyStatusParam(&min_freq_, &max_freq_, freq_tolerance, window_size),
00310 diagnostic_updater::TimeStampStatusParam(min_acceptable, max_acceptable)));
00311 }
00312
00320 int readSerialAsHexFromFile(std::string camera_serial_path)
00321 {
00322 NODELET_DEBUG("Reading camera serial file from: %s", camera_serial_path.c_str());
00323
00324 std::ifstream serial_file(camera_serial_path.c_str());
00325 std::stringstream buffer;
00326 int serial = 0;
00327
00328 if (serial_file.is_open())
00329 {
00330 std::string serial_str((std::istreambuf_iterator<char>(serial_file)), std::istreambuf_iterator<char>());
00331 NODELET_DEBUG("Serial file contents: %s", serial_str.c_str());
00332 buffer << std::hex << serial_str;
00333 buffer >> serial;
00334 NODELET_DEBUG("Serial discovered %d", serial);
00335
00336 return serial;
00337 }
00338
00339 NODELET_WARN("Unable to open serial path: %s", camera_serial_path.c_str());
00340 return 0;
00341 }
00342
00343
00349 void devicePoll()
00350 {
00351 enum State
00352 {
00353 NONE
00354 , ERROR
00355 , STOPPED
00356 , DISCONNECTED
00357 , CONNECTED
00358 , STARTED
00359 };
00360
00361 State state = DISCONNECTED;
00362 State previous_state = NONE;
00363
00364 while(!boost::this_thread::interruption_requested())
00365 {
00366 bool state_changed = state != previous_state;
00367
00368 previous_state = state;
00369
00370 switch(state)
00371 {
00372 case ERROR:
00373
00374
00375 #if STOP_ON_ERROR
00376
00377 {
00378 boost::mutex::scoped_lock scopedLock(connect_mutex_);
00379 sub_.shutdown();
00380 }
00381
00382 try
00383 {
00384 NODELET_DEBUG("Stopping camera.");
00385 pg_.stop();
00386 NODELET_INFO("Stopped camera.");
00387
00388 state = STOPPED;
00389 }
00390 catch(std::runtime_error& e)
00391 {
00392 NODELET_ERROR_COND(state_changed,
00393 "Failed to stop error: %s", e.what());
00394 ros::Duration(1.0).sleep();
00395 }
00396
00397 break;
00398 #endif
00399 case STOPPED:
00400
00401 try
00402 {
00403 NODELET_DEBUG("Disconnecting from camera.");
00404 pg_.disconnect();
00405 NODELET_INFO("Disconnected from camera.");
00406
00407 state = DISCONNECTED;
00408 }
00409 catch(std::runtime_error& e)
00410 {
00411 NODELET_ERROR_COND(state_changed,
00412 "Failed to disconnect with error: %s", e.what());
00413 ros::Duration(1.0).sleep();
00414 }
00415
00416 break;
00417 case DISCONNECTED:
00418
00419 try
00420 {
00421 NODELET_DEBUG("Connecting to camera.");
00422 pg_.connect();
00423 NODELET_INFO("Connected to camera.");
00424
00425
00426 pg_.setNewConfiguration(config_, PointGreyCamera::LEVEL_RECONFIGURE_STOP);
00427
00428
00429 try
00430 {
00431 double timeout;
00432 getMTPrivateNodeHandle().param("timeout", timeout, 1.0);
00433
00434 NODELET_DEBUG("Setting timeout to: %f.", timeout);
00435 pg_.setTimeout(timeout);
00436 }
00437 catch(std::runtime_error& e)
00438 {
00439 NODELET_ERROR("%s", e.what());
00440 }
00441
00442
00443 {
00444 boost::mutex::scoped_lock scopedLock(connect_mutex_);
00445 sub_ = getMTNodeHandle().subscribe("image_exposure_sequence", 10, &pointgrey_camera_driver::PointGreyCameraNodelet::gainWBCallback, this);
00446 }
00447
00448 state = CONNECTED;
00449 }
00450 catch(std::runtime_error& e)
00451 {
00452 NODELET_ERROR_COND(state_changed,
00453 "Failed to connect with error: %s", e.what());
00454 ros::Duration(1.0).sleep();
00455 }
00456
00457 break;
00458 case CONNECTED:
00459
00460 try
00461 {
00462 NODELET_DEBUG("Starting camera.");
00463 pg_.start();
00464 NODELET_INFO("Started camera.");
00465 NODELET_INFO("Attention: if nothing subscribes to the camera topic, the camera_info is not published on the correspondent topic.");
00466 state = STARTED;
00467 }
00468 catch(std::runtime_error& e)
00469 {
00470 NODELET_ERROR_COND(state_changed,
00471 "Failed to start with error: %s", e.what());
00472 ros::Duration(1.0).sleep();
00473 }
00474
00475 break;
00476 case STARTED:
00477 try
00478 {
00479 wfov_camera_msgs::WFOVImagePtr wfov_image(new wfov_camera_msgs::WFOVImage);
00480
00481 NODELET_DEBUG("Starting a new grab from camera.");
00482 pg_.grabImage(wfov_image->image, frame_id_);
00483
00484
00485 wfov_image->header.frame_id = frame_id_;
00486
00487 wfov_image->gain = gain_;
00488 wfov_image->white_balance_blue = wb_blue_;
00489 wfov_image->white_balance_red = wb_red_;
00490
00491 wfov_image->temperature = pg_.getCameraTemperature();
00492
00493 ros::Time time = ros::Time::now();
00494 wfov_image->header.stamp = time;
00495 wfov_image->image.header.stamp = time;
00496
00497
00498 ci_.reset(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));
00499 ci_->header.stamp = wfov_image->image.header.stamp;
00500 ci_->header.frame_id = wfov_image->header.frame_id;
00501
00502 ci_->binning_x = binning_x_;
00503 ci_->binning_y = binning_y_;
00504 ci_->roi.x_offset = roi_x_offset_;
00505 ci_->roi.y_offset = roi_y_offset_;
00506 ci_->roi.height = roi_height_;
00507 ci_->roi.width = roi_width_;
00508 ci_->roi.do_rectify = do_rectify_;
00509
00510 wfov_image->info = *ci_;
00511
00512
00513 pub_->publish(wfov_image);
00514
00515
00516 if(it_pub_.getNumSubscribers() > 0)
00517 {
00518 sensor_msgs::ImagePtr image(new sensor_msgs::Image(wfov_image->image));
00519 it_pub_.publish(image, ci_);
00520 }
00521 }
00522 catch(CameraTimeoutException& e)
00523 {
00524 NODELET_WARN("%s", e.what());
00525 }
00526 catch(CameraImageConsistencyError& e)
00527 {
00528 NODELET_WARN("%s", e.what());
00529 }
00530 catch(std::runtime_error& e)
00531 {
00532 NODELET_ERROR("%s", e.what());
00533
00534 state = ERROR;
00535 }
00536
00537 break;
00538 default:
00539 NODELET_ERROR("Unknown camera state %d!", state);
00540 }
00541
00542
00543 updater_.update();
00544 }
00545 NODELET_DEBUG("Leaving thread.");
00546 }
00547
00548 void gainWBCallback(const image_exposure_msgs::ExposureSequence &msg)
00549 {
00550 try
00551 {
00552 NODELET_DEBUG("Gain callback: Setting gain to %f and white balances to %u, %u", msg.gain, msg.white_balance_blue, msg.white_balance_red);
00553 gain_ = msg.gain;
00554 pg_.setGain(gain_);
00555 wb_blue_ = msg.white_balance_blue;
00556 wb_red_ = msg.white_balance_red;
00557 pg_.setBRWhiteBalance(false, wb_blue_, wb_red_);
00558 }
00559 catch(std::runtime_error& e)
00560 {
00561 NODELET_ERROR("gainWBCallback failed with error: %s", e.what());
00562 }
00563 }
00564
00565 boost::shared_ptr<dynamic_reconfigure::Server<pointgrey_camera_driver::PointGreyConfig> > srv_;
00566 boost::shared_ptr<image_transport::ImageTransport> it_;
00567 boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_;
00568 image_transport::CameraPublisher it_pub_;
00569 boost::shared_ptr<diagnostic_updater::DiagnosedPublisher<wfov_camera_msgs::WFOVImage> > pub_;
00570 ros::Subscriber sub_;
00571
00572 boost::mutex connect_mutex_;
00573
00574 diagnostic_updater::Updater updater_;
00575 double min_freq_;
00576 double max_freq_;
00577
00578 PointGreyCamera pg_;
00579 sensor_msgs::CameraInfoPtr ci_;
00580 std::string frame_id_;
00581 boost::shared_ptr<boost::thread> pubThread_;
00582
00583 double gain_;
00584 uint16_t wb_blue_;
00585 uint16_t wb_red_;
00586
00587
00588 size_t binning_x_;
00589 size_t binning_y_;
00590 size_t roi_x_offset_;
00591 size_t roi_y_offset_;
00592 size_t roi_height_;
00593 size_t roi_width_;
00594 bool do_rectify_;
00595
00596
00598 bool auto_packet_size_;
00600 int packet_size_;
00602 int packet_delay_;
00603
00605 pointgrey_camera_driver::PointGreyConfig config_;
00606 };
00607
00608 PLUGINLIB_DECLARE_CLASS(pointgrey_camera_driver, PointGreyCameraNodelet, pointgrey_camera_driver::PointGreyCameraNodelet, nodelet::Nodelet);
00609 }