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 #include "openni2_camera/openni2_driver.h"
00033 #include "openni2_camera/openni2_exception.h"
00034
00035 #include <sensor_msgs/image_encodings.h>
00036 #include <sensor_msgs/distortion_models.h>
00037
00038 #include <boost/date_time/posix_time/posix_time.hpp>
00039 #include <boost/thread/thread.hpp>
00040
00041 namespace openni2_wrapper
00042 {
00043
00044 OpenNI2Driver::OpenNI2Driver(ros::NodeHandle& n, ros::NodeHandle& pnh) :
00045 nh_(n),
00046 pnh_(pnh),
00047 device_manager_(OpenNI2DeviceManager::getSingelton()),
00048 config_init_(false),
00049 data_skip_ir_counter_(0),
00050 data_skip_color_counter_(0),
00051 data_skip_depth_counter_ (0),
00052 ir_subscribers_(false),
00053 color_subscribers_(false),
00054 depth_subscribers_(false),
00055 depth_raw_subscribers_(false)
00056 {
00057
00058 genVideoModeTableMap();
00059
00060 readConfigFromParameterServer();
00061
00062 initDevice();
00063
00064
00065 reconfigure_server_.reset(new ReconfigureServer(pnh_));
00066 reconfigure_server_->setCallback(boost::bind(&OpenNI2Driver::configCb, this, _1, _2));
00067
00068 while (!config_init_)
00069 {
00070 ROS_DEBUG("Waiting for dynamic reconfigure configuration.");
00071 boost::this_thread::sleep(boost::posix_time::seconds(0.1));
00072 }
00073 ROS_DEBUG("Dynamic reconfigure configuration received.");
00074
00075 advertiseROSTopics();
00076
00077 }
00078
00079 void OpenNI2Driver::advertiseROSTopics()
00080 {
00081
00082
00083 ros::NodeHandle color_nh(nh_, "rgb");
00084 image_transport::ImageTransport color_it(color_nh);
00085 ros::NodeHandle ir_nh(nh_, "ir");
00086 image_transport::ImageTransport ir_it(ir_nh);
00087 ros::NodeHandle depth_nh(nh_, "depth");
00088 image_transport::ImageTransport depth_it(depth_nh);
00089 ros::NodeHandle depth_raw_nh(nh_, "depth");
00090 image_transport::ImageTransport depth_raw_it(depth_raw_nh);
00091
00092
00093
00094
00095
00096
00097 boost::lock_guard<boost::mutex> lock(connect_mutex_);
00098
00099
00100 if (device_->hasColorSensor())
00101 {
00102 image_transport::SubscriberStatusCallback itssc = boost::bind(&OpenNI2Driver::colorConnectCb, this);
00103 ros::SubscriberStatusCallback rssc = boost::bind(&OpenNI2Driver::colorConnectCb, this);
00104 pub_color_ = color_it.advertiseCamera("image", 1, itssc, itssc, rssc, rssc);
00105 }
00106
00107 if (device_->hasIRSensor())
00108 {
00109 image_transport::SubscriberStatusCallback itssc = boost::bind(&OpenNI2Driver::irConnectCb, this);
00110 ros::SubscriberStatusCallback rssc = boost::bind(&OpenNI2Driver::irConnectCb, this);
00111 pub_ir_ = ir_it.advertiseCamera("image", 1, itssc, itssc, rssc, rssc);
00112 }
00113
00114 if (device_->hasDepthSensor())
00115 {
00116 image_transport::SubscriberStatusCallback itssc = boost::bind(&OpenNI2Driver::depthConnectCb, this);
00117 ros::SubscriberStatusCallback rssc = boost::bind(&OpenNI2Driver::depthConnectCb, this);
00118 pub_depth_raw_ = depth_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
00119 pub_depth_ = depth_raw_it.advertiseCamera("image", 1, itssc, itssc, rssc, rssc);
00120 }
00121
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132 std::string serial_number = device_->getStringID();
00133 std::string color_name, ir_name;
00134
00135 color_name = "rgb_" + serial_number;
00136 ir_name = "depth_" + serial_number;
00137
00138
00139 color_info_manager_ = boost::make_shared<camera_info_manager::CameraInfoManager>(color_nh, color_name, color_info_url_);
00140 ir_info_manager_ = boost::make_shared<camera_info_manager::CameraInfoManager>(ir_nh, ir_name, ir_info_url_);
00141
00142 }
00143
00144 void OpenNI2Driver::configCb(Config &config, uint32_t level)
00145 {
00146 bool stream_reset = false;
00147
00148 depth_ir_offset_x_ = config.depth_ir_offset_x;
00149 depth_ir_offset_y_ = config.depth_ir_offset_y;
00150 z_offset_mm_ = config.z_offset_mm;
00151 z_scaling_ = config.z_scaling;
00152
00153 ir_time_offset_ = ros::Duration(config.ir_time_offset);
00154 color_time_offset_ = ros::Duration(config.color_time_offset);
00155 depth_time_offset_ = ros::Duration(config.depth_time_offset);
00156
00157 if (lookupVideoModeFromDynConfig(config.ir_mode, ir_video_mode_)<0)
00158 {
00159 ROS_ERROR("Undefined IR video mode received from dynamic reconfigure");
00160 exit(-1);
00161 }
00162
00163 if (lookupVideoModeFromDynConfig(config.color_mode, color_video_mode_)<0)
00164 {
00165 ROS_ERROR("Undefined color video mode received from dynamic reconfigure");
00166 exit(-1);
00167 }
00168
00169 if (lookupVideoModeFromDynConfig(config.depth_mode, depth_video_mode_)<0)
00170 {
00171 ROS_ERROR("Undefined depth video mode received from dynamic reconfigure");
00172 exit(-1);
00173 }
00174
00175
00176
00177 ir_video_mode_.pixel_format_ = PIXEL_FORMAT_GRAY16;
00178 color_video_mode_.pixel_format_ = PIXEL_FORMAT_RGB888;
00179 depth_video_mode_.pixel_format_ = PIXEL_FORMAT_DEPTH_1_MM;
00180
00181 color_depth_synchronization_ = config.color_depth_synchronization;
00182 depth_registration_ = config.depth_registration;
00183
00184 auto_exposure_ = config.auto_exposure;
00185 auto_white_balance_ = config.auto_white_balance;
00186
00187 use_device_time_ = config.use_device_time;
00188
00189 data_skip_ = config.data_skip+1;
00190
00191 applyConfigToOpenNIDevice();
00192
00193 config_init_ = true;
00194
00195 old_config_ = config;
00196 }
00197
00198 void OpenNI2Driver::setIRVideoMode(const OpenNI2VideoMode& ir_video_mode)
00199 {
00200 if (device_->isIRVideoModeSupported(ir_video_mode))
00201 {
00202 if (ir_video_mode != device_->getIRVideoMode())
00203 {
00204 device_->setIRVideoMode(ir_video_mode);
00205 }
00206
00207 }
00208 else
00209 {
00210 ROS_ERROR_STREAM("Unsupported IR video mode - " << ir_video_mode);
00211 }
00212 }
00213 void OpenNI2Driver::setColorVideoMode(const OpenNI2VideoMode& color_video_mode)
00214 {
00215 if (device_->isColorVideoModeSupported(color_video_mode))
00216 {
00217 if (color_video_mode != device_->getColorVideoMode())
00218 {
00219 device_->setColorVideoMode(color_video_mode);
00220 }
00221 }
00222 else
00223 {
00224 ROS_ERROR_STREAM("Unsupported color video mode - " << color_video_mode);
00225 }
00226 }
00227 void OpenNI2Driver::setDepthVideoMode(const OpenNI2VideoMode& depth_video_mode)
00228 {
00229 if (device_->isDepthVideoModeSupported(depth_video_mode))
00230 {
00231 if (depth_video_mode != device_->getDepthVideoMode())
00232 {
00233 device_->setDepthVideoMode(depth_video_mode);
00234 }
00235 }
00236 else
00237 {
00238 ROS_ERROR_STREAM("Unsupported depth video mode - " << depth_video_mode);
00239 }
00240 }
00241
00242 void OpenNI2Driver::applyConfigToOpenNIDevice()
00243 {
00244
00245 data_skip_ir_counter_ = 0;
00246 data_skip_color_counter_= 0;
00247 data_skip_depth_counter_ = 0;
00248
00249 setIRVideoMode(ir_video_mode_);
00250 setColorVideoMode(color_video_mode_);
00251 setDepthVideoMode(depth_video_mode_);
00252
00253 if (device_->isImageRegistrationModeSupported())
00254 {
00255 try
00256 {
00257 if (!config_init_ || (old_config_.depth_registration != depth_registration_))
00258 device_->setImageRegistrationMode(depth_registration_);
00259 }
00260 catch (const OpenNI2Exception& exception)
00261 {
00262 ROS_ERROR("Could not set image registration. Reason: %s", exception.what());
00263 }
00264 }
00265
00266 try
00267 {
00268 if (!config_init_ || (old_config_.color_depth_synchronization != color_depth_synchronization_))
00269 device_->setDepthColorSync(color_depth_synchronization_);
00270 }
00271 catch (const OpenNI2Exception& exception)
00272 {
00273 ROS_ERROR("Could not set color depth synchronization. Reason: %s", exception.what());
00274 }
00275
00276 try
00277 {
00278 if (!config_init_ || (old_config_.auto_exposure != auto_exposure_))
00279 device_->setAutoExposure(auto_exposure_);
00280 }
00281 catch (const OpenNI2Exception& exception)
00282 {
00283 ROS_ERROR("Could not set auto exposure. Reason: %s", exception.what());
00284 }
00285
00286 try
00287 {
00288 if (!config_init_ || (old_config_.auto_white_balance != auto_white_balance_))
00289 device_->setAutoWhiteBalance(auto_white_balance_);
00290 }
00291 catch (const OpenNI2Exception& exception)
00292 {
00293 ROS_ERROR("Could not set auto white balance. Reason: %s", exception.what());
00294 }
00295
00296 device_->setUseDeviceTimer(use_device_time_);
00297
00298 }
00299
00300 void OpenNI2Driver::colorConnectCb()
00301 {
00302 boost::lock_guard<boost::mutex> lock(connect_mutex_);
00303
00304 color_subscribers_ = pub_color_.getNumSubscribers() > 0;
00305
00306 if (color_subscribers_ && !device_->isColorStreamStarted())
00307 {
00308
00309 if (device_->isIRStreamStarted())
00310 {
00311 ROS_ERROR("Cannot stream RGB and IR at the same time. Streaming RGB only.");
00312 ROS_INFO("Stopping IR stream.");
00313 device_->stopIRStream();
00314 }
00315
00316 device_->setColorFrameCallback(boost::bind(&OpenNI2Driver::newColorFrameCallback, this, _1));
00317
00318 ROS_INFO("Starting color stream.");
00319 device_->startColorStream();
00320
00321 }
00322 else if (!color_subscribers_ && device_->isColorStreamStarted())
00323 {
00324 ROS_INFO("Stopping color stream.");
00325 device_->stopColorStream();
00326
00327
00328 bool need_ir = pub_ir_.getNumSubscribers() > 0;
00329 if (need_ir && !device_->isIRStreamStarted())
00330 {
00331 device_->setIRFrameCallback(boost::bind(&OpenNI2Driver::newIRFrameCallback, this, _1));
00332
00333 ROS_INFO("Starting IR stream.");
00334 device_->startIRStream();
00335 }
00336 }
00337 }
00338
00339 void OpenNI2Driver::depthConnectCb()
00340 {
00341 boost::lock_guard<boost::mutex> lock(connect_mutex_);
00342
00343 depth_subscribers_ = pub_depth_.getNumSubscribers() > 0;
00344 depth_raw_subscribers_ = pub_depth_raw_.getNumSubscribers() > 0;
00345
00346 bool need_depth = depth_subscribers_ || depth_raw_subscribers_;
00347
00348 if (need_depth && !device_->isDepthStreamStarted())
00349 {
00350 device_->setDepthFrameCallback(boost::bind(&OpenNI2Driver::newDepthFrameCallback, this, _1));
00351
00352 ROS_INFO("Starting depth stream.");
00353 device_->startDepthStream();
00354 }
00355 else if (!need_depth && device_->isDepthStreamStarted())
00356 {
00357 ROS_INFO("Stopping depth stream.");
00358 device_->stopDepthStream();
00359 }
00360 }
00361
00362 void OpenNI2Driver::irConnectCb()
00363 {
00364 boost::lock_guard<boost::mutex> lock(connect_mutex_);
00365
00366 ir_subscribers_ = pub_ir_.getNumSubscribers() > 0;
00367
00368 if (ir_subscribers_ && !device_->isIRStreamStarted())
00369 {
00370
00371 if (device_->isColorStreamStarted())
00372 {
00373 ROS_ERROR("Cannot stream RGB and IR at the same time. Streaming RGB only.");
00374 }
00375 else
00376 {
00377 device_->setIRFrameCallback(boost::bind(&OpenNI2Driver::newIRFrameCallback, this, _1));
00378
00379 ROS_INFO("Starting IR stream.");
00380 device_->startIRStream();
00381 }
00382 }
00383 else if (!ir_subscribers_ && device_->isIRStreamStarted())
00384 {
00385 ROS_INFO("Stopping IR stream.");
00386 device_->stopIRStream();
00387 }
00388 }
00389
00390 void OpenNI2Driver::newIRFrameCallback(sensor_msgs::ImagePtr image)
00391 {
00392 if ((++data_skip_ir_counter_)%data_skip_==0)
00393 {
00394 data_skip_ir_counter_ = 0;
00395
00396 if (ir_subscribers_)
00397 {
00398 image->header.frame_id = ir_frame_id_;
00399 image->header.stamp = image->header.stamp + ir_time_offset_;
00400
00401 pub_ir_.publish(image, getIRCameraInfo(image->width, image->height, image->header.stamp));
00402 }
00403 }
00404 }
00405
00406 void OpenNI2Driver::newColorFrameCallback(sensor_msgs::ImagePtr image)
00407 {
00408 if ((++data_skip_color_counter_)%data_skip_==0)
00409 {
00410 data_skip_color_counter_ = 0;
00411
00412 if (color_subscribers_)
00413 {
00414 image->header.frame_id = color_frame_id_;
00415 image->header.stamp = image->header.stamp + color_time_offset_;
00416
00417 pub_color_.publish(image, getColorCameraInfo(image->width, image->height, image->header.stamp));
00418 }
00419 }
00420 }
00421
00422 void OpenNI2Driver::newDepthFrameCallback(sensor_msgs::ImagePtr image)
00423 {
00424 if ((++data_skip_depth_counter_)%data_skip_==0)
00425 {
00426
00427 data_skip_depth_counter_ = 0;
00428
00429 if (depth_raw_subscribers_||depth_subscribers_)
00430 {
00431 image->header.stamp = image->header.stamp + depth_time_offset_;
00432
00433 if (z_offset_mm_ != 0)
00434 {
00435 uint16_t* data = reinterpret_cast<uint16_t*>(&image->data[0]);
00436 for (unsigned int i = 0; i < image->width * image->height; ++i)
00437 if (data[i] != 0)
00438 data[i] += z_offset_mm_;
00439 }
00440
00441 if (fabs(z_scaling_ - 1.0) > 1e-6)
00442 {
00443 uint16_t* data = reinterpret_cast<uint16_t*>(&image->data[0]);
00444 for (unsigned int i = 0; i < image->width * image->height; ++i)
00445 if (data[i] != 0)
00446 data[i] = static_cast<uint16_t>(data[i] * z_scaling_);
00447 }
00448
00449 sensor_msgs::CameraInfoPtr cam_info;
00450
00451 if (depth_registration_)
00452 {
00453 image->header.frame_id = color_frame_id_;
00454 cam_info = getColorCameraInfo(image->width,image->height, image->header.stamp);
00455 } else
00456 {
00457 image->header.frame_id = depth_frame_id_;
00458 cam_info = getDepthCameraInfo(image->width,image->height, image->header.stamp);
00459 }
00460
00461 if (depth_raw_subscribers_)
00462 {
00463 pub_depth_raw_.publish(image, cam_info);
00464 }
00465
00466 if (depth_subscribers_ )
00467 {
00468 sensor_msgs::ImageConstPtr floating_point_image = rawToFloatingPointConversion(image);
00469 pub_depth_.publish(floating_point_image, cam_info);
00470 }
00471 }
00472 }
00473 }
00474
00475
00476 sensor_msgs::CameraInfoPtr OpenNI2Driver::getDefaultCameraInfo(int width, int height, double f) const
00477 {
00478 sensor_msgs::CameraInfoPtr info = boost::make_shared<sensor_msgs::CameraInfo>();
00479
00480 info->width = width;
00481 info->height = height;
00482
00483
00484 info->D.resize(5, 0.0);
00485 info->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
00486
00487
00488 info->K.assign(0.0);
00489 info->K[0] = info->K[4] = f;
00490 info->K[2] = (width / 2) - 0.5;
00491
00492
00493 info->K[5] = (width * (3./8.)) - 0.5;
00494 info->K[8] = 1.0;
00495
00496
00497 info->R.assign(0.0);
00498 info->R[0] = info->R[4] = info->R[8] = 1.0;
00499
00500
00501 info->P.assign(0.0);
00502 info->P[0] = info->P[5] = f;
00503 info->P[2] = info->K[2];
00504 info->P[6] = info->K[5];
00505 info->P[10] = 1.0;
00506
00507 return info;
00508 }
00509
00511 sensor_msgs::CameraInfoPtr OpenNI2Driver::getColorCameraInfo(int width, int height, ros::Time time) const
00512 {
00513 sensor_msgs::CameraInfoPtr info;
00514
00515 if (color_info_manager_->isCalibrated())
00516 {
00517 info = boost::make_shared<sensor_msgs::CameraInfo>(color_info_manager_->getCameraInfo());
00518 if ( info->width != width )
00519 {
00520
00521 ROS_WARN_ONCE("Image resolution doesn't match calibration of the RGB camera. Using default parameters.");
00522 info = getDefaultCameraInfo(width, height, device_->getColorFocalLength(height));
00523 }
00524 }
00525 else
00526 {
00527
00528 info = getDefaultCameraInfo(width, height, device_->getColorFocalLength(height));
00529 }
00530
00531
00532 info->header.stamp = time;
00533 info->header.frame_id = color_frame_id_;
00534
00535 return info;
00536 }
00537
00538
00539 sensor_msgs::CameraInfoPtr OpenNI2Driver::getIRCameraInfo(int width, int height, ros::Time time) const
00540 {
00541 sensor_msgs::CameraInfoPtr info;
00542
00543 if (ir_info_manager_->isCalibrated())
00544 {
00545 info = boost::make_shared<sensor_msgs::CameraInfo>(ir_info_manager_->getCameraInfo());
00546 if ( info->width != width )
00547 {
00548
00549 ROS_WARN_ONCE("Image resolution doesn't match calibration of the IR camera. Using default parameters.");
00550 info = getDefaultCameraInfo(width, height, device_->getIRFocalLength(height));
00551 }
00552 }
00553 else
00554 {
00555
00556 info = getDefaultCameraInfo(width, height, device_->getDepthFocalLength(height));
00557 }
00558
00559
00560 info->header.stamp = time;
00561 info->header.frame_id = depth_frame_id_;
00562
00563 return info;
00564 }
00565
00566 sensor_msgs::CameraInfoPtr OpenNI2Driver::getDepthCameraInfo(int width, int height, ros::Time time) const
00567 {
00568
00569
00570
00571
00572 double scaling = (double)width / 640;
00573
00574 sensor_msgs::CameraInfoPtr info = getIRCameraInfo(width, height, time);
00575 info->K[2] -= depth_ir_offset_x_*scaling;
00576 info->K[5] -= depth_ir_offset_y_*scaling;
00577 info->P[2] -= depth_ir_offset_x_*scaling;
00578 info->P[6] -= depth_ir_offset_y_*scaling;
00579
00581 return info;
00582 }
00583
00584 void OpenNI2Driver::readConfigFromParameterServer()
00585 {
00586 if (!pnh_.getParam("device_id", device_id_))
00587 {
00588 ROS_WARN ("~device_id is not set! Using first device.");
00589 device_id_ = "#1";
00590 }
00591
00592
00593 pnh_.param("ir_frame_id", ir_frame_id_, std::string("/openni_ir_optical_frame"));
00594 pnh_.param("rgb_frame_id", color_frame_id_, std::string("/openni_rgb_optical_frame"));
00595 pnh_.param("depth_frame_id", depth_frame_id_, std::string("/openni_depth_optical_frame"));
00596
00597 ROS_DEBUG("ir_frame_id = '%s' ", ir_frame_id_.c_str());
00598 ROS_DEBUG("rgb_frame_id = '%s' ", color_frame_id_.c_str());
00599 ROS_DEBUG("depth_frame_id = '%s' ", depth_frame_id_.c_str());
00600
00601 pnh_.param("rgb_camera_info_url", color_info_url_, std::string());
00602 pnh_.param("depth_camera_info_url", ir_info_url_, std::string());
00603
00604 }
00605
00606 std::string OpenNI2Driver::resolveDeviceURI(const std::string& device_id) throw(OpenNI2Exception)
00607 {
00608 std::string device_URI;
00609 boost::shared_ptr<std::vector<std::string> > available_device_URIs =
00610 device_manager_->getConnectedDeviceURIs();
00611
00612
00613 if (device_id_.size() > 1 && device_id_[0] == '#')
00614 {
00615 std::istringstream device_number_str(device_id_.substr(1));
00616 int device_number;
00617 device_number_str >> device_number;
00618 int device_index = device_number - 1;
00619 if (device_index >= available_device_URIs->size() || device_index < 0)
00620 {
00621 THROW_OPENNI_EXCEPTION(
00622 "Invalid device number %i, there are %zu devices connected.",
00623 device_number, available_device_URIs->size());
00624 }
00625 else
00626 {
00627 return available_device_URIs->at(device_index);
00628 }
00629 }
00630
00631
00632
00633
00634 else if (device_id_.size() > 1 && device_id_.find('@') != std::string::npos)
00635 {
00636
00637 size_t index = device_id_.find('@');
00638 if (index <= 0)
00639 {
00640 THROW_OPENNI_EXCEPTION(
00641 "%s is not a valid device URI, you must give the bus number before the @.",
00642 device_id_.c_str());
00643 }
00644 if (index >= device_id_.size() - 1)
00645 {
00646 THROW_OPENNI_EXCEPTION(
00647 "%s is not a valid device URI, you must give a number after the @, specify 0 for first device",
00648 device_id_.c_str());
00649 }
00650
00651
00652 std::istringstream device_number_str(device_id_.substr(index+1));
00653 int device_number;
00654 device_number_str >> device_number;
00655
00656
00657 std::string bus = device_id_.substr(0, index);
00658 bus.insert(0, "@");
00659
00660 for (size_t i = 0; i < available_device_URIs->size(); ++i)
00661 {
00662 std::string s = (*available_device_URIs)[i];
00663 if (s.find(bus) != std::string::npos)
00664 {
00665
00666 --device_number;
00667 if (device_number <= 0)
00668 return s;
00669 }
00670 }
00671
00672 THROW_OPENNI_EXCEPTION("Device not found %s", device_id_.c_str());
00673 }
00674 else
00675 {
00676
00677 boost::shared_ptr<std::vector<OpenNI2DeviceInfo> > dev_infos =
00678 device_manager_->getConnectedDeviceInfos();
00679
00680 for(std::vector<OpenNI2DeviceInfo>::iterator it = dev_infos->begin();
00681 it != dev_infos->end(); ++it)
00682 {
00683 if (device_id.size()>0 && device_id == it->serial_)
00684 return it->uri_;
00685 }
00686
00687
00688 return device_id_;
00689 }
00690 }
00691
00692 void OpenNI2Driver::initDevice()
00693 {
00694 while (ros::ok() && !device_)
00695 {
00696 try
00697 {
00698 std::string device_URI = resolveDeviceURI(device_id_);
00699 device_ = device_manager_->getDevice(device_URI);
00700 }
00701 catch (const OpenNI2Exception& exception)
00702 {
00703 if (!device_)
00704 {
00705 ROS_INFO("No matching device found.... waiting for devices. Reason: %s", exception.what());
00706 boost::this_thread::sleep(boost::posix_time::seconds(3));
00707 continue;
00708 }
00709 else
00710 {
00711 ROS_ERROR("Could not retrieve device. Reason: %s", exception.what());
00712 exit(-1);
00713 }
00714 }
00715 }
00716
00717 while (ros::ok() && !device_->isValid())
00718 {
00719 ROS_DEBUG("Waiting for device initialization..");
00720 boost::this_thread::sleep(boost::posix_time::seconds(0.1));
00721 }
00722
00723 }
00724
00725 void OpenNI2Driver::genVideoModeTableMap()
00726 {
00727
00728
00729
00730
00731
00732
00733
00734
00735
00736
00737
00738
00739
00740
00741
00742
00743
00744 video_modes_lookup_.clear();
00745
00746 OpenNI2VideoMode video_mode;
00747
00748
00749 video_mode.x_resolution_ = 1280;
00750 video_mode.y_resolution_ = 1024;
00751 video_mode.frame_rate_ = 30;
00752
00753 video_modes_lookup_[1] = video_mode;
00754
00755
00756 video_mode.x_resolution_ = 1280;
00757 video_mode.y_resolution_ = 1024;
00758 video_mode.frame_rate_ = 15;
00759
00760 video_modes_lookup_[2] = video_mode;
00761
00762
00763 video_mode.x_resolution_ = 1280;
00764 video_mode.y_resolution_ = 720;
00765 video_mode.frame_rate_ = 30;
00766
00767 video_modes_lookup_[3] = video_mode;
00768
00769
00770 video_mode.x_resolution_ = 1280;
00771 video_mode.y_resolution_ = 720;
00772 video_mode.frame_rate_ = 15;
00773
00774 video_modes_lookup_[4] = video_mode;
00775
00776
00777 video_mode.x_resolution_ = 640;
00778 video_mode.y_resolution_ = 480;
00779 video_mode.frame_rate_ = 30;
00780
00781 video_modes_lookup_[5] = video_mode;
00782
00783
00784 video_mode.x_resolution_ = 640;
00785 video_mode.y_resolution_ = 480;
00786 video_mode.frame_rate_ = 25;
00787
00788 video_modes_lookup_[6] = video_mode;
00789
00790
00791 video_mode.x_resolution_ = 320;
00792 video_mode.y_resolution_ = 240;
00793 video_mode.frame_rate_ = 25;
00794
00795 video_modes_lookup_[7] = video_mode;
00796
00797
00798 video_mode.x_resolution_ = 320;
00799 video_mode.y_resolution_ = 240;
00800 video_mode.frame_rate_ = 30;
00801
00802 video_modes_lookup_[8] = video_mode;
00803
00804
00805 video_mode.x_resolution_ = 320;
00806 video_mode.y_resolution_ = 240;
00807 video_mode.frame_rate_ = 60;
00808
00809 video_modes_lookup_[9] = video_mode;
00810
00811
00812 video_mode.x_resolution_ = 160;
00813 video_mode.y_resolution_ = 120;
00814 video_mode.frame_rate_ = 25;
00815
00816 video_modes_lookup_[10] = video_mode;
00817
00818
00819 video_mode.x_resolution_ = 160;
00820 video_mode.y_resolution_ = 120;
00821 video_mode.frame_rate_ = 30;
00822
00823 video_modes_lookup_[11] = video_mode;
00824
00825
00826 video_mode.x_resolution_ = 160;
00827 video_mode.y_resolution_ = 120;
00828 video_mode.frame_rate_ = 60;
00829
00830 video_modes_lookup_[12] = video_mode;
00831
00832 }
00833
00834 int OpenNI2Driver::lookupVideoModeFromDynConfig(int mode_nr, OpenNI2VideoMode& video_mode)
00835 {
00836 int ret = -1;
00837
00838 std::map<int, OpenNI2VideoMode>::const_iterator it;
00839
00840 it = video_modes_lookup_.find(mode_nr);
00841
00842 if (it!=video_modes_lookup_.end())
00843 {
00844 video_mode = it->second;
00845 ret = 0;
00846 }
00847
00848 return ret;
00849 }
00850
00851 sensor_msgs::ImageConstPtr OpenNI2Driver::rawToFloatingPointConversion(sensor_msgs::ImageConstPtr raw_image)
00852 {
00853 static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
00854
00855 sensor_msgs::ImagePtr new_image = boost::make_shared<sensor_msgs::Image>();
00856
00857 new_image->header = raw_image->header;
00858 new_image->width = raw_image->width;
00859 new_image->height = raw_image->height;
00860 new_image->is_bigendian = 0;
00861 new_image->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00862 new_image->step = sizeof(float)*raw_image->width;
00863
00864 std::size_t data_size = new_image->width*new_image->height;
00865 new_image->data.resize(data_size*sizeof(float));
00866
00867 const unsigned short* in_ptr = reinterpret_cast<const unsigned short*>(&raw_image->data[0]);
00868 float* out_ptr = reinterpret_cast<float*>(&new_image->data[0]);
00869
00870 for (std::size_t i = 0; i<data_size; ++i, ++in_ptr, ++out_ptr)
00871 {
00872 if (*in_ptr==0 || *in_ptr==0x7FF)
00873 {
00874 *out_ptr = bad_point;
00875 } else
00876 {
00877 *out_ptr = static_cast<float>(*in_ptr)/1000.0f;
00878 }
00879 }
00880
00881 return new_image;
00882 }
00883
00884 }