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
00038
00039 #include "driver.h"
00040 #include <sensor_msgs/image_encodings.h>
00041 #include <sensor_msgs/distortion_models.h>
00042 #include <boost/algorithm/string/replace.hpp>
00043
00044 using namespace std;
00045 namespace freenect_camera {
00046
00047 DriverNodelet::~DriverNodelet ()
00048 {
00049
00050 init_thread_.interrupt();
00051 init_thread_.join();
00052
00053
00054 close_diagnostics_ = true;
00055 diagnostics_thread_.join();
00056
00057 FreenectDriver& driver = FreenectDriver::getInstance ();
00058 driver.shutdown();
00059
00062 }
00063
00064 void DriverNodelet::onInit ()
00065 {
00066
00067
00068 init_thread_ = boost::thread(boost::bind(&DriverNodelet::onInitImpl, this));
00069 }
00070
00071 void DriverNodelet::onInitImpl ()
00072 {
00073 ros::NodeHandle& nh = getNodeHandle();
00074 ros::NodeHandle& param_nh = getPrivateNodeHandle();
00075
00076
00077 image_transport::ImageTransport it(nh);
00078 ros::NodeHandle rgb_nh(nh, "rgb");
00079 image_transport::ImageTransport rgb_it(rgb_nh);
00080 ros::NodeHandle ir_nh(nh, "ir");
00081 image_transport::ImageTransport ir_it(ir_nh);
00082 ros::NodeHandle depth_nh(nh, "depth");
00083 image_transport::ImageTransport depth_it(depth_nh);
00084 ros::NodeHandle depth_registered_nh(nh, "depth_registered");
00085 image_transport::ImageTransport depth_registered_it(depth_registered_nh);
00086 ros::NodeHandle projector_nh(nh, "projector");
00087
00088 rgb_frame_counter_ = depth_frame_counter_ = ir_frame_counter_ = 0;
00089 publish_rgb_ = publish_ir_ = publish_depth_ = true;
00090
00091
00092
00093 param_nh.param("debug" , libfreenect_debug_, false);
00094
00095
00096 updateModeMaps();
00097 setupDevice();
00098
00099
00100 reconfigure_server_.reset( new ReconfigureServer(param_nh) );
00101 reconfigure_server_->setCallback(boost::bind(&DriverNodelet::configCb, this, _1, _2));
00102
00103
00104 param_nh.param("rgb_frame_id", rgb_frame_id_, string("/openni_rgb_optical_frame"));
00105 param_nh.param("depth_frame_id", depth_frame_id_, string("/openni_depth_optical_frame"));
00106 NODELET_INFO("rgb_frame_id = '%s' ", rgb_frame_id_.c_str());
00107 NODELET_INFO("depth_frame_id = '%s' ", depth_frame_id_.c_str());
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117 std::string serial_number = device_->getSerialNumber();
00118 std::string rgb_name, ir_name;
00119 if (serial_number.empty())
00120 {
00121 rgb_name = "rgb";
00122 ir_name = "depth";
00123 }
00124 else
00125 {
00126 rgb_name = "rgb_" + serial_number;
00127 ir_name = "depth_" + serial_number;
00128 }
00129
00130 std::string rgb_info_url, ir_info_url;
00131 param_nh.param("rgb_camera_info_url", rgb_info_url, std::string());
00132 param_nh.param("depth_camera_info_url", ir_info_url, std::string());
00133
00134 double diagnostics_max_frequency, diagnostics_min_frequency;
00135 double diagnostics_tolerance, diagnostics_window_time;
00136 param_nh.param("enable_rgb_diagnostics", enable_rgb_diagnostics_, false);
00137 param_nh.param("enable_ir_diagnostics", enable_ir_diagnostics_, false);
00138 param_nh.param("enable_depth_diagnostics", enable_depth_diagnostics_, false);
00139 param_nh.param("diagnostics_max_frequency", diagnostics_max_frequency, 30.0);
00140 param_nh.param("diagnostics_min_frequency", diagnostics_min_frequency, 30.0);
00141 param_nh.param("diagnostics_tolerance", diagnostics_tolerance, 0.05);
00142 param_nh.param("diagnostics_window_time", diagnostics_window_time, 5.0);
00143
00144
00145 log4cxx::LoggerPtr logger_ccp = log4cxx::Logger::getLogger("ros.camera_calibration_parsers");
00146 log4cxx::LoggerPtr logger_cim = log4cxx::Logger::getLogger("ros.camera_info_manager");
00147 logger_ccp->setLevel(log4cxx::Level::getFatal());
00148 logger_cim->setLevel(log4cxx::Level::getWarn());
00149
00150
00151
00152
00153 log4cxx::LoggerPtr logger_its = log4cxx::Logger::getLogger("ros.image_transport.sync");
00154 logger_its->setLevel(log4cxx::Level::getError());
00155 ros::console::notifyLoggerLevelsChanged();
00156
00157
00158 rgb_info_manager_ = boost::make_shared<camera_info_manager::CameraInfoManager>(rgb_nh, rgb_name, rgb_info_url);
00159 ir_info_manager_ = boost::make_shared<camera_info_manager::CameraInfoManager>(ir_nh, ir_name, ir_info_url);
00160
00161 if (!rgb_info_manager_->isCalibrated())
00162 NODELET_WARN("Using default parameters for RGB camera calibration.");
00163 if (!ir_info_manager_->isCalibrated())
00164 NODELET_WARN("Using default parameters for IR camera calibration.");
00165
00166
00167 {
00168
00169
00170
00171
00172 boost::lock_guard<boost::mutex> lock(connect_mutex_);
00173
00174
00175 pub_freq_max_ = diagnostics_max_frequency;
00176 pub_freq_min_ = diagnostics_min_frequency;
00177 diagnostic_updater_.reset(new diagnostic_updater::Updater);
00178
00179
00180 std::string hardware_id = std::string(device_->getProductName()) + "-" +
00181 std::string(device_->getSerialNumber());
00182 diagnostic_updater_->setHardwareID(hardware_id);
00183
00184
00185 if (device_->hasImageStream())
00186 {
00187 image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::rgbConnectCb, this);
00188 ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::rgbConnectCb, this);
00189 pub_rgb_ = rgb_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
00190 if (enable_rgb_diagnostics_) {
00191 pub_rgb_freq_.reset(new TopicDiagnostic("RGB Image", *diagnostic_updater_,
00192 FrequencyStatusParam(&pub_freq_min_, &pub_freq_max_,
00193 diagnostics_tolerance, diagnostics_window_time)));
00194 }
00195 }
00196
00197 if (device_->hasIRStream())
00198 {
00199 image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::irConnectCb, this);
00200 ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::irConnectCb, this);
00201 pub_ir_ = ir_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
00202 if (enable_ir_diagnostics_) {
00203 pub_ir_freq_.reset(new TopicDiagnostic("IR Image", *diagnostic_updater_,
00204 FrequencyStatusParam(&pub_freq_min_, &pub_freq_max_,
00205 diagnostics_tolerance, diagnostics_window_time)));
00206 }
00207 }
00208
00209 if (device_->hasDepthStream())
00210 {
00211 image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::depthConnectCb, this);
00212 ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::depthConnectCb, this);
00213 pub_depth_ = depth_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
00214 if (enable_depth_diagnostics_) {
00215 pub_depth_freq_.reset(new TopicDiagnostic("Depth Image", *diagnostic_updater_,
00216 FrequencyStatusParam(&pub_freq_min_, &pub_freq_max_,
00217 diagnostics_tolerance, diagnostics_window_time)));
00218 }
00219
00220 pub_projector_info_ = projector_nh.advertise<sensor_msgs::CameraInfo>("camera_info", 1, rssc, rssc);
00221
00222 if (device_->isDepthRegistrationSupported()) {
00223 pub_depth_registered_ = depth_registered_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
00224 }
00225 }
00226 }
00227
00228
00229 close_diagnostics_ = false;
00230 diagnostics_thread_ = boost::thread(boost::bind(&DriverNodelet::updateDiagnostics, this));
00231
00232
00233 if (param_nh.getParam("time_out", time_out_) && time_out_ > 0.0)
00234 {
00235 time_stamp_ = ros::Time(0,0);
00236 watch_dog_timer_ = nh.createTimer(ros::Duration(time_out_), &DriverNodelet::watchDog, this);
00237 }
00238 }
00239
00240 void DriverNodelet::updateDiagnostics() {
00241 while (!close_diagnostics_) {
00242 diagnostic_updater_->update();
00243 boost::this_thread::sleep(boost::posix_time::milliseconds(10));
00244 }
00245 }
00246
00247 void DriverNodelet::setupDevice ()
00248 {
00249
00250 FreenectDriver& driver = FreenectDriver::getInstance();
00251
00252
00253 if (libfreenect_debug_)
00254 driver.enableDebug();
00255
00256 do {
00257 driver.updateDeviceList ();
00258
00259 if (driver.getNumberDevices () == 0)
00260 {
00261 NODELET_INFO ("No devices connected.... waiting for devices to be connected");
00262 boost::this_thread::sleep(boost::posix_time::seconds(3));
00263 continue;
00264 }
00265
00266 NODELET_INFO ("Number devices connected: %d", driver.getNumberDevices ());
00267 for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
00268 {
00269 NODELET_INFO("%u. device on bus %03u:%02u is a %s (%03x) from %s (%03x) with serial id \'%s\'",
00270 deviceIdx + 1, driver.getBus(deviceIdx), driver.getAddress(deviceIdx),
00271 driver.getProductName(deviceIdx), driver.getProductID(deviceIdx),
00272 driver.getVendorName(deviceIdx), driver.getVendorID(deviceIdx),
00273 driver.getSerialNumber(deviceIdx));
00274 }
00275
00276 try {
00277 string device_id;
00278 if (!getPrivateNodeHandle().getParam("device_id", device_id))
00279 {
00280 NODELET_WARN ("~device_id is not set! Using first device.");
00281 device_ = driver.getDeviceByIndex(0);
00282 }
00283 else if (device_id.find ('@') != string::npos)
00284 {
00285 size_t pos = device_id.find ('@');
00286 unsigned bus = atoi (device_id.substr (0, pos).c_str ());
00287 unsigned address = atoi (device_id.substr (pos + 1, device_id.length () - pos - 1).c_str ());
00288 NODELET_INFO ("Searching for device with bus@address = %d@%d", bus, address);
00289 device_ = driver.getDeviceByAddress (bus, address);
00290 }
00291 else if (device_id[0] == '#')
00292 {
00293 unsigned index = atoi (device_id.c_str() + 1);
00294 NODELET_INFO ("Searching for device with index = %d", index);
00295 device_ = driver.getDeviceByIndex (index - 1);
00296 }
00297 else
00298 {
00299 NODELET_INFO ("Searching for device with serial number = '%s'", device_id.c_str ());
00300 device_ = driver.getDeviceBySerialNumber (device_id);
00301 }
00302 }
00303 catch (exception& e)
00304 {
00305 if (!device_)
00306 {
00307 NODELET_INFO ("No matching device found.... waiting for devices. Reason: %s", e.what ());
00308 boost::this_thread::sleep(boost::posix_time::seconds(3));
00309 continue;
00310 }
00311 else
00312 {
00313 NODELET_FATAL ("Could not retrieve device. Reason: %s", e.what ());
00314 exit (-1);
00315 }
00316 }
00317 } while (!device_);
00318
00319 NODELET_INFO ("Opened '%s' on bus %d:%d with serial number '%s'", device_->getProductName (),
00320 device_->getBus (), device_->getAddress (), device_->getSerialNumber ());
00321
00322 device_->registerImageCallback(&DriverNodelet::rgbCb, *this);
00323 device_->registerDepthCallback(&DriverNodelet::depthCb, *this);
00324 device_->registerIRCallback (&DriverNodelet::irCb, *this);
00325 }
00326
00327 void DriverNodelet::rgbConnectCb()
00328 {
00329 boost::lock_guard<boost::mutex> lock(connect_mutex_);
00330 bool need_rgb = pub_rgb_.getNumSubscribers() > 0;
00331
00332 if (need_rgb && !device_->isImageStreamRunning())
00333 {
00334
00335 if (device_->isIRStreamRunning())
00336 {
00337 NODELET_ERROR("Cannot stream RGB and IR at the same time. Streaming RGB only.");
00338 device_->stopIRStream();
00339 }
00340
00341 device_->startImageStream();
00342 startSynchronization();
00343 time_stamp_ = ros::Time(0,0);
00344 }
00345 else if (!need_rgb && device_->isImageStreamRunning())
00346 {
00347 stopSynchronization();
00348 device_->stopImageStream();
00349
00350
00351 bool need_ir = pub_ir_.getNumSubscribers() > 0;
00352 if (need_ir && !device_->isIRStreamRunning())
00353 {
00354 device_->startIRStream();
00355 time_stamp_ = ros::Time(0,0);
00356 }
00357 }
00358 }
00359
00360 void DriverNodelet::depthConnectCb()
00361 {
00362 boost::lock_guard<boost::mutex> lock(connect_mutex_);
00364 bool need_depth =
00365 device_->isDepthRegistered() ? pub_depth_registered_.getNumSubscribers() > 0 : pub_depth_.getNumSubscribers() > 0;
00367
00368 if (need_depth && !device_->isDepthStreamRunning())
00369 {
00370 device_->startDepthStream();
00371 startSynchronization();
00372 time_stamp_ = ros::Time(0,0);
00373
00374 }
00375 else if (!need_depth && device_->isDepthStreamRunning())
00376 {
00377 stopSynchronization();
00378 device_->stopDepthStream();
00379 }
00380 }
00381
00382 void DriverNodelet::irConnectCb()
00383 {
00384 boost::lock_guard<boost::mutex> lock(connect_mutex_);
00385 bool need_ir = pub_ir_.getNumSubscribers() > 0;
00386
00387 if (need_ir && !device_->isIRStreamRunning())
00388 {
00389
00390 if (device_->isImageStreamRunning())
00391 {
00392 NODELET_ERROR("Cannot stream RGB and IR at the same time. Streaming RGB only.");
00393 }
00394 else
00395 {
00396 device_->startIRStream();
00397 time_stamp_ = ros::Time(0,0);
00398 }
00399 }
00400 else if (!need_ir)
00401 {
00402 device_->stopIRStream();
00403 }
00404 }
00405
00406
00407
00408
00409 void DriverNodelet::checkFrameCounters()
00410 {
00411 if (max(rgb_frame_counter_, max(depth_frame_counter_, ir_frame_counter_)) > config_.data_skip) {
00412
00413 rgb_frame_counter_ = 0;
00414 depth_frame_counter_ = 0;
00415 ir_frame_counter_ = 0;
00416
00417
00418 publish_rgb_ = true;
00419 publish_depth_ = true;
00420 publish_ir_ = true;
00421 }
00422 }
00423
00424 void DriverNodelet::rgbCb(const ImageBuffer& image, void* cookie)
00425 {
00426 ros::Time time = ros::Time::now () + ros::Duration(config_.image_time_offset);
00427 time_stamp_ = time;
00428
00429 bool publish = false;
00430 {
00431 boost::unique_lock<boost::mutex> counter_lock(counter_mutex_);
00432 rgb_frame_counter_++;
00433 checkFrameCounters();
00434 publish = publish_rgb_;
00435
00436 if (publish)
00437 rgb_frame_counter_ = 0;
00438 }
00439
00440 if (publish)
00441 publishRgbImage(image, time);
00442
00443 publish_rgb_ = false;
00444 }
00445
00446 void DriverNodelet::depthCb(const ImageBuffer& depth_image, void* cookie)
00447 {
00448 ros::Time time = ros::Time::now () + ros::Duration(config_.depth_time_offset);
00449 time_stamp_ = time;
00450
00451 bool publish = false;
00452 {
00453 boost::unique_lock<boost::mutex> counter_lock(counter_mutex_);
00454 depth_frame_counter_++;
00455 checkFrameCounters();
00456 publish = publish_depth_;
00457
00458 if (publish)
00459 depth_frame_counter_ = 0;
00460 }
00461
00462 if (publish)
00463 publishDepthImage(depth_image, time);
00464
00465 publish_depth_ = false;
00466 }
00467
00468 void DriverNodelet::irCb(const ImageBuffer& ir_image, void* cookie)
00469 {
00470 ros::Time time = ros::Time::now() + ros::Duration(config_.depth_time_offset);
00471 time_stamp_ = time;
00472
00473 bool publish = false;
00474 {
00475 boost::unique_lock<boost::mutex> counter_lock(counter_mutex_);
00476 ir_frame_counter_++;
00477 checkFrameCounters();
00478 publish = publish_ir_;
00479
00480 if (publish)
00481 ir_frame_counter_ = 0;
00482 }
00483
00484 if (publish)
00485 publishIrImage(ir_image, time);
00486 publish_ir_ = false;
00487 }
00488
00489 void DriverNodelet::publishRgbImage(const ImageBuffer& image, ros::Time time) const
00490 {
00491 sensor_msgs::ImagePtr rgb_msg = boost::make_shared<sensor_msgs::Image >();
00492 rgb_msg->header.stamp = time;
00493 rgb_msg->header.frame_id = rgb_frame_id_;
00494 rgb_msg->height = image.metadata.height;
00495 rgb_msg->width = image.metadata.width;
00496 switch(image.metadata.video_format) {
00497 case FREENECT_VIDEO_RGB:
00498 rgb_msg->encoding = sensor_msgs::image_encodings::RGB8;
00499 rgb_msg->step = rgb_msg->width * 3;
00500 break;
00501 case FREENECT_VIDEO_BAYER:
00502 rgb_msg->encoding = sensor_msgs::image_encodings::BAYER_GRBG8;
00503 rgb_msg->step = rgb_msg->width;
00504 break;
00505 case FREENECT_VIDEO_YUV_RGB:
00506 rgb_msg->encoding = sensor_msgs::image_encodings::YUV422;
00507 rgb_msg->step = rgb_msg->width * 2;
00508 break;
00509 default:
00510 NODELET_ERROR("Unknown RGB image format received from libfreenect");
00511
00512 return;
00513 }
00514 rgb_msg->data.resize(rgb_msg->height * rgb_msg->step);
00515 fillImage(image, reinterpret_cast<void*>(&rgb_msg->data[0]));
00516
00517 pub_rgb_.publish(rgb_msg, getRgbCameraInfo(image, time));
00518 if (enable_rgb_diagnostics_)
00519 pub_rgb_freq_->tick();
00520 }
00521
00522 void DriverNodelet::publishDepthImage(const ImageBuffer& depth, ros::Time time) const
00523 {
00524 bool registered = depth.is_registered;
00525
00526 sensor_msgs::ImagePtr depth_msg = boost::make_shared<sensor_msgs::Image>();
00527 depth_msg->header.stamp = time;
00528 depth_msg->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
00529 depth_msg->height = depth.metadata.height;
00530 depth_msg->width = depth.metadata.width;
00531 depth_msg->step = depth_msg->width * sizeof(short);
00532 depth_msg->data.resize(depth_msg->height * depth_msg->step);
00533
00534 fillImage(depth, reinterpret_cast<void*>(&depth_msg->data[0]));
00535
00536 if (z_offset_mm_ != 0)
00537 {
00538 uint16_t* data = reinterpret_cast<uint16_t*>(&depth_msg->data[0]);
00539 for (unsigned int i = 0; i < depth_msg->width * depth_msg->height; ++i)
00540 if (data[i] != 0)
00541 data[i] += z_offset_mm_;
00542 }
00543
00544 if (registered)
00545 {
00546
00547 depth_msg->header.frame_id = rgb_frame_id_;
00548 pub_depth_registered_.publish(depth_msg, getRgbCameraInfo(depth, time));
00549 }
00550 else
00551 {
00552
00553 depth_msg->header.frame_id = depth_frame_id_;
00554 pub_depth_.publish(depth_msg, getDepthCameraInfo(depth, time));
00555 }
00556 if (enable_depth_diagnostics_)
00557 pub_depth_freq_->tick();
00558
00559
00560 if (pub_projector_info_.getNumSubscribers() > 0)
00561 {
00562 pub_projector_info_.publish(getProjectorCameraInfo(depth, time));
00563 }
00564 }
00565
00566 void DriverNodelet::publishIrImage(const ImageBuffer& ir, ros::Time time) const
00567 {
00568 sensor_msgs::ImagePtr ir_msg = boost::make_shared<sensor_msgs::Image>();
00569 ir_msg->header.stamp = time;
00570 ir_msg->header.frame_id = depth_frame_id_;
00571 ir_msg->encoding = sensor_msgs::image_encodings::MONO16;
00572 ir_msg->height = ir.metadata.height;
00573 ir_msg->width = ir.metadata.width;
00574 ir_msg->step = ir_msg->width * sizeof(uint16_t);
00575 ir_msg->data.resize(ir_msg->height * ir_msg->step);
00576
00577 fillImage(ir, reinterpret_cast<void*>(&ir_msg->data[0]));
00578
00579 pub_ir_.publish(ir_msg, getIrCameraInfo(ir, time));
00580
00581 if (enable_ir_diagnostics_)
00582 pub_ir_freq_->tick();
00583 }
00584
00585 sensor_msgs::CameraInfoPtr DriverNodelet::getDefaultCameraInfo(int width, int height, double f) const
00586 {
00587 sensor_msgs::CameraInfoPtr info = boost::make_shared<sensor_msgs::CameraInfo>();
00588
00589 info->width = width;
00590 info->height = height;
00591
00592
00593 info->D.resize(5, 0.0);
00594 info->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
00595
00596
00597 info->K.assign(0.0);
00598 info->K[0] = info->K[4] = f;
00599 info->K[2] = (width / 2) - 0.5;
00600
00601
00602 info->K[5] = (width * (3./8.)) - 0.5;
00603 info->K[8] = 1.0;
00604
00605
00606 info->R.assign(0.0);
00607 info->R[0] = info->R[4] = info->R[8] = 1.0;
00608
00609
00610 info->P.assign(0.0);
00611 info->P[0] = info->P[5] = f;
00612 info->P[2] = info->K[2];
00613 info->P[6] = info->K[5];
00614 info->P[10] = 1.0;
00615
00616 return info;
00617 }
00618
00620 sensor_msgs::CameraInfoPtr DriverNodelet::getRgbCameraInfo(const ImageBuffer& image, ros::Time time) const
00621 {
00622 sensor_msgs::CameraInfoPtr info;
00623
00624 if (rgb_info_manager_->isCalibrated())
00625 {
00626 info = boost::make_shared<sensor_msgs::CameraInfo>(rgb_info_manager_->getCameraInfo());
00627 }
00628 else
00629 {
00630
00631 info = getDefaultCameraInfo(image.metadata.width, image.metadata.height, image.focal_length);
00632 }
00633
00634
00635 info->header.stamp = time;
00636 info->header.frame_id = rgb_frame_id_;
00637
00638 return info;
00639 }
00640
00641 sensor_msgs::CameraInfoPtr DriverNodelet::getIrCameraInfo(
00642 const ImageBuffer& image, ros::Time time) const {
00643 sensor_msgs::CameraInfoPtr info;
00644
00645 if (ir_info_manager_->isCalibrated())
00646 {
00647 info = boost::make_shared<sensor_msgs::CameraInfo>(ir_info_manager_->getCameraInfo());
00648 }
00649 else
00650 {
00651
00652 info = getDefaultCameraInfo(image.metadata.width, image.metadata.height, image.focal_length);
00653 }
00654
00655
00656 info->header.stamp = time;
00657 info->header.frame_id = depth_frame_id_;
00658
00659 return info;
00660 }
00661
00662 sensor_msgs::CameraInfoPtr DriverNodelet::getDepthCameraInfo(
00663 const ImageBuffer& image, ros::Time time) const {
00664
00665
00666
00667
00668 sensor_msgs::CameraInfoPtr info = getIrCameraInfo(image, time);
00669 info->K[2] -= depth_ir_offset_x_;
00670 info->K[5] -= depth_ir_offset_y_;
00671 info->P[2] -= depth_ir_offset_x_;
00672 info->P[6] -= depth_ir_offset_y_;
00673
00675 return info;
00676 }
00677
00678 sensor_msgs::CameraInfoPtr DriverNodelet::getProjectorCameraInfo(
00679 const ImageBuffer& image, ros::Time time) const {
00680
00681
00682
00683 sensor_msgs::CameraInfoPtr info = getDepthCameraInfo(image, time);
00684
00685 info->P[3] = -device_->getBaseline() * info->P[0];
00686 return info;
00687 }
00688
00689 void DriverNodelet::configCb(Config &config, uint32_t level)
00690 {
00691 depth_ir_offset_x_ = config.depth_ir_offset_x;
00692 depth_ir_offset_y_ = config.depth_ir_offset_y;
00693 z_offset_mm_ = config.z_offset_mm;
00694
00695
00696 OutputMode old_image_mode, image_mode, compatible_image_mode;
00697 if (device_->hasImageStream ())
00698 {
00699 old_image_mode = device_->getImageOutputMode ();
00700
00701
00702 image_mode = mapConfigMode2OutputMode (config.image_mode);
00703
00704 if (!device_->findCompatibleImageMode (image_mode, compatible_image_mode))
00705 {
00706 OutputMode default_mode = device_->getDefaultImageMode();
00707 NODELET_WARN("Could not find any compatible image output mode for %d. "
00708 "Falling back to default image output mode %d.",
00709 image_mode,
00710 default_mode);
00711
00712 config.image_mode = mapMode2ConfigMode(default_mode);
00713 image_mode = compatible_image_mode = default_mode;
00714 }
00715 }
00716
00717 OutputMode old_depth_mode, depth_mode, compatible_depth_mode;
00718 old_depth_mode = device_->getDepthOutputMode();
00719 depth_mode = mapConfigMode2OutputMode (config.depth_mode);
00720 if (!device_->findCompatibleDepthMode (depth_mode, compatible_depth_mode))
00721 {
00722 OutputMode default_mode = device_->getDefaultDepthMode();
00723 NODELET_WARN("Could not find any compatible depth output mode for %d. "
00724 "Falling back to default depth output mode %d.",
00725 depth_mode,
00726 default_mode);
00727
00728 config.depth_mode = mapMode2ConfigMode(default_mode);
00729 depth_mode = compatible_depth_mode = default_mode;
00730 }
00731
00732
00733 if ( (device_->hasImageStream () && compatible_image_mode != old_image_mode) ||
00734 compatible_depth_mode != old_depth_mode)
00735 {
00736
00737 stopSynchronization();
00738
00739 if (device_->hasImageStream () && compatible_image_mode != old_image_mode)
00740 device_->setImageOutputMode (compatible_image_mode);
00741
00742 if (compatible_depth_mode != old_depth_mode)
00743 device_->setDepthOutputMode (compatible_depth_mode);
00744
00745 startSynchronization ();
00746 }
00747
00748
00749 if (device_->isDepthRegistered () && !config.depth_registration)
00750 {
00751 device_->setDepthRegistration (false);
00752 }
00753 else if (!device_->isDepthRegistered () && config.depth_registration)
00754 {
00755 device_->setDepthRegistration (true);
00756 }
00757
00758
00759 config_ = config;
00760 }
00761
00762 void DriverNodelet::startSynchronization()
00763 {
00764 if (device_->isSynchronizationSupported() &&
00765 !device_->isSynchronized() &&
00766 device_->isImageStreamRunning() &&
00767 device_->isDepthStreamRunning() )
00768 {
00769 device_->setSynchronization(true);
00770 }
00771 }
00772
00773 void DriverNodelet::stopSynchronization()
00774 {
00775 if (device_->isSynchronizationSupported() &&
00776 device_->isSynchronized())
00777 {
00778 device_->setSynchronization(false);
00779 }
00780 }
00781
00782 void DriverNodelet::updateModeMaps ()
00783 {
00784 OutputMode output_mode;
00785
00786 output_mode = FREENECT_RESOLUTION_HIGH;
00787 mode2config_map_[output_mode] = Freenect_SXGA;
00788 config2mode_map_[Freenect_SXGA] = output_mode;
00789
00790 output_mode = FREENECT_RESOLUTION_MEDIUM;
00791 mode2config_map_[output_mode] = Freenect_VGA;
00792 config2mode_map_[Freenect_VGA] = output_mode;
00793 }
00794
00795 int DriverNodelet::mapMode2ConfigMode (const OutputMode& output_mode) const
00796 {
00797 std::map<OutputMode, int>::const_iterator it = mode2config_map_.find (output_mode);
00798
00799 if (it == mode2config_map_.end ())
00800 {
00801 NODELET_ERROR ("mode not be found");
00802 exit (-1);
00803 }
00804 else
00805 return it->second;
00806 }
00807
00808 OutputMode DriverNodelet::mapConfigMode2OutputMode (int mode) const
00809 {
00810 std::map<int, OutputMode>::const_iterator it = config2mode_map_.find (mode);
00811 if (it == config2mode_map_.end ())
00812 {
00813 NODELET_ERROR ("mode %d could not be found", mode);
00814 exit (-1);
00815 }
00816 else
00817 return it->second;
00818 }
00819
00820 void DriverNodelet::watchDog (const ros::TimerEvent& event)
00821 {
00823 if ( !time_stamp_.isZero() && (device_->isDepthStreamRunning() || device_->isImageStreamRunning()) )
00824 {
00825 ros::Duration duration = ros::Time::now() - time_stamp_;
00826 if (duration.toSec() >= time_out_)
00827 {
00828 NODELET_ERROR("Timeout");
00829 watch_dog_timer_.stop();
00830 throw std::runtime_error("Timeout occured in DriverNodelet");
00831 }
00832 }
00833 }
00834
00835 }
00836
00837
00838 #include <pluginlib/class_list_macros.h>
00839 PLUGINLIB_DECLARE_CLASS (freenect_camera, driver, freenect_camera::DriverNodelet, nodelet::Nodelet);