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
00040
00041 #include "kinect_camera/kinect.h"
00042 #include <sensor_msgs/image_encodings.h>
00043 #include <cv_bridge/CvBridge.h>
00044 #include <boost/make_shared.hpp>
00045
00046 namespace kinect_camera
00047 {
00048
00049 const double KinectDriver::SHIFT_SCALE = 0.125;
00050
00052 KinectDriver::KinectDriver (ros::NodeHandle comm_nh, ros::NodeHandle param_nh)
00053 : reconfigure_server_(param_nh),
00054 width_ (640), height_ (480),
00055 f_ctx_(NULL), f_dev_(NULL),
00056 started_(false),
00057 depth_sent_ (true), rgb_sent_ (true)
00058 {
00059
00060 ReconfigureServer::CallbackType f = boost::bind(&KinectDriver::configCb, this, _1, _2);
00061 reconfigure_server_.setCallback(f);
00062
00063
00064 std::string kinect_depth_frame;
00065 param_nh.param ("kinect_depth_frame", kinect_depth_frame, std::string ("/kinect_depth"));
00066 cloud_.header.frame_id = cloud2_.header.frame_id = kinect_depth_frame;
00067 cloud_.channels.resize (1);
00068 cloud_.channels[0].name = "rgb";
00069 cloud_.channels[0].values.resize (width_ * height_);
00071 cloud_rgb_.channels.resize (1);
00072 cloud_rgb_.channels[0].name = "rgb";
00073 cloud_rgb_.channels[0].values.resize (width_ * height_);
00074
00075 cloud2_.height = height_;
00076 cloud2_.width = width_;
00077 cloud2_.fields.resize (4);
00078 cloud2_.fields[0].name = "x";
00079 cloud2_.fields[1].name = "y";
00080 cloud2_.fields[2].name = "z";
00081 cloud2_.fields[3].name = "rgb";
00082
00083
00084 int offset = 0;
00085 for (size_t s = 0; s < cloud2_.fields.size (); ++s, offset += 4)
00086 {
00087 cloud2_.fields[s].offset = offset;
00088 cloud2_.fields[s].count = 1;
00089 cloud2_.fields[s].datatype = sensor_msgs::PointField::FLOAT32;
00090 }
00091
00092 cloud2_.point_step = offset;
00093 cloud2_.row_step = cloud2_.point_step * cloud2_.width;
00094 cloud2_.data.resize (cloud2_.row_step * cloud2_.height);
00095 cloud2_.is_dense = true;
00096
00097
00098 cloud2_rgb_.height = height_;
00099 cloud2_rgb_.width = width_;
00100 cloud2_rgb_.fields.resize (4);
00101 cloud2_rgb_.fields[0].name = "x";
00102 cloud2_rgb_.fields[1].name = "y";
00103 cloud2_rgb_.fields[2].name = "z";
00104 cloud2_rgb_.fields[3].name = "rgb";
00105
00106
00107 offset = 0;
00108 for (size_t s = 0; s < cloud2_rgb_.fields.size (); ++s, offset += 4)
00109 {
00110 cloud2_rgb_.fields[s].offset = offset;
00111 cloud2_rgb_.fields[s].count = 1;
00112 cloud2_rgb_.fields[s].datatype = sensor_msgs::PointField::FLOAT32;
00113 }
00114
00115 cloud2_rgb_.point_step = offset;
00116 cloud2_rgb_.row_step = cloud2_rgb_.point_step * cloud2_rgb_.width;
00117 cloud2_rgb_.data.resize (cloud2_rgb_.row_step * cloud2_rgb_.height);
00118 cloud2_rgb_.is_dense = true;
00119
00120
00121 depth_image_.header.frame_id = kinect_depth_frame;
00122 depth_image_.height = height_;
00123 depth_image_.width = width_;
00124 depth_image_.encoding = "mono8";
00125 depth_image_.step = width_;
00126 depth_image_.data.resize(width_ * height_);
00127
00128
00129 std::string kinect_RGB_frame;
00130 param_nh.param ("kinect_rgb_frame", kinect_RGB_frame, std::string ("/kinect_rgb"));
00131 rgb_image_.header.frame_id = rgb_info_.header.frame_id = kinect_RGB_frame;
00132 rgb_image_.height = height_;
00133 rgb_image_.width = width_;
00134 cloud_rgb_.header.frame_id = cloud2_rgb_.header.frame_id = kinect_RGB_frame;
00135
00136
00137 std::string cam_name, rgb_info_url, depth_info_url;
00138 param_nh.param ("camera_name", cam_name, std::string("camera"));
00139 param_nh.param ("rgb/camera_info_url", rgb_info_url, std::string("auto"));
00140 param_nh.param ("depth/camera_info_url", depth_info_url, std::string("auto"));
00141 if (rgb_info_url.compare("auto") == 0)
00142 rgb_info_url = std::string("file://")+ros::package::getPath(ROS_PACKAGE_NAME)+std::string("/info/calibration_rgb.yaml");
00143 if (depth_info_url.compare("auto") == 0)
00144 depth_info_url = std::string("file://")+ros::package::getPath(ROS_PACKAGE_NAME)+std::string("/info/calibration_depth.yaml");
00145 ROS_INFO ("[KinectDriver] Calibration URLs:\n\tRGB: %s\n\tDepth: %s",
00146 rgb_info_url.c_str (), depth_info_url.c_str ());
00147
00148 rgb_info_manager_ = boost::make_shared<CameraInfoManager> (ros::NodeHandle(comm_nh, "rgb"),
00149 cam_name, rgb_info_url);
00150 depth_info_manager_ = boost::make_shared<CameraInfoManager> (ros::NodeHandle(comm_nh, "depth"),
00151 cam_name, depth_info_url);
00152 rgb_info_ = rgb_info_manager_->getCameraInfo ();
00153 depth_info_ = depth_info_manager_->getCameraInfo ();
00154 rgb_info_.header.frame_id = kinect_RGB_frame;
00155 depth_info_.header.frame_id = kinect_depth_frame;
00156 rgb_model_.fromCameraInfo(rgb_info_);
00157 depth_model_.fromCameraInfo(depth_info_);
00158
00160
00161 param_nh.param ("shift_offset", shift_offset_, 1084.0);
00162 param_nh.param ("projector_depth_baseline", baseline_, 0.075);
00163
00164
00165
00166 Eigen::Matrix4d Q;
00167 Q << 1, 0, 0, -depth_model_.cx(),
00168 0, 1, 0, -depth_model_.cy(),
00169 0, 0, 0, depth_model_.fx(),
00170 0, 0, 1.0 / baseline_, 0;
00171
00172
00173
00174 XmlRpc::XmlRpcValue rot, trans;
00175 if (param_nh.getParam("depth_rgb_rotation", rot) &&
00176 param_nh.getParam("depth_rgb_translation", trans) &&
00177 rot.size() == 9 && trans.size() == 3)
00178 {
00179 S << rot[0], rot[1], rot[2], trans[0],
00180 rot[3], rot[4], rot[5], trans[1],
00181 rot[6], rot[7], rot[8], trans[2],
00182 0, 0, 0, 1;
00183 }
00184 else
00185 {
00186 ROS_WARN("Transform between depth and RGB cameras is not calibrated");
00187 S << 1, 0, 0, -0.025,
00188 0, 1, 0, 0,
00189 0, 0, 1, 0,
00190 0, 0, 0, 1;
00191 }
00192
00193
00194 Eigen::Matrix<double, 3, 4> P;
00195 P << rgb_model_.fx(), 0, rgb_model_.cx(), 0,
00196 0, rgb_model_.fy(), rgb_model_.cy(), 0,
00197 0, 0, 1, 0;
00198
00199
00200 depth_to_rgb_ = P*S*Q;
00201
00202 std::cout << "Transform matrix:" << std::endl << depth_to_rgb_ << std::endl << std::endl;
00203
00204
00205 image_transport::ImageTransport it(comm_nh);
00206 pub_rgb_ = it.advertiseCamera ("rgb/image_raw", 1);
00207 pub_rgb_rect_ = it.advertise("rgb/image_rect_color", 1);
00208 pub_depth_ = it.advertiseCamera ("depth/image_raw", 1);
00209 pub_ir_ = it.advertiseCamera ("ir/image_raw", 1);
00210 pub_depth_points_ = comm_nh.advertise<sensor_msgs::PointCloud> ("depth/points", 15);
00211 pub_depth_points2_ = comm_nh.advertise<sensor_msgs::PointCloud2>("depth/points2", 15);
00212 pub_rgb_points_ = comm_nh.advertise<sensor_msgs::PointCloud> ("rgb/points", 15);
00213 pub_rgb_points2_ = comm_nh.advertise<sensor_msgs::PointCloud2>("rgb/points2", 15);
00214 pub_imu_ = comm_nh.advertise<sensor_msgs::Imu>("imu", 15);
00215 }
00216
00220 bool
00221 KinectDriver::init (int index)
00222 {
00223
00224 if (freenect_init (&f_ctx_, NULL) < 0)
00225 {
00226 ROS_ERROR ("[KinectDriver::init] Initialization failed!");
00227 return (false);
00228 }
00229
00230
00231 int nr_devices = freenect_num_devices (f_ctx_);
00232 if (nr_devices < 1)
00233 {
00234 ROS_WARN ("[KinectDriver::init] No devices found!");
00235 return (false);
00236 }
00237 ROS_DEBUG ("[KinectDriver::init] Number of devices found: %d", nr_devices);
00238 if (nr_devices <= index)
00239 {
00240 ROS_WARN ("[KinectDriver::init] Desired device index (%d) out of bounds (%d)!", index, nr_devices);
00241 return (false);
00242 }
00243
00244
00245 if (freenect_open_device (f_ctx_, &f_dev_, index) < 0)
00246 {
00247 ROS_ERROR ("[KinectDriver::init] Could not open device with index (%d)!", index);
00248 return (false);
00249 }
00250
00251
00252 freenect_set_user(f_dev_, this);
00253 freenect_set_depth_callback (f_dev_, &KinectDriver::depthCbInternal);
00254 freenect_set_rgb_callback (f_dev_, &KinectDriver::rgbCbInternal);
00255 freenect_set_ir_callback (f_dev_, &KinectDriver::irCbInternal);
00256
00257 updateDeviceSettings();
00258
00259 return (true);
00260 }
00261
00262 void KinectDriver::depthCbInternal (freenect_device *dev, void *buf, uint32_t timestamp)
00263 {
00264 freenect_depth* tbuf = reinterpret_cast<freenect_depth*>(buf);
00265 KinectDriver* driver = reinterpret_cast<KinectDriver*>(freenect_get_user(dev));
00266 driver->depthCb(dev, tbuf, timestamp);
00267 }
00268
00269 void KinectDriver::rgbCbInternal (freenect_device *dev, freenect_pixel *buf, uint32_t timestamp)
00270 {
00271 KinectDriver* driver = reinterpret_cast<KinectDriver*>(freenect_get_user(dev));
00272 driver->rgbCb(dev, buf, timestamp);
00273 }
00274
00275 void KinectDriver::irCbInternal (freenect_device *dev, freenect_pixel_ir *buf, uint32_t timestamp)
00276 {
00277 KinectDriver* driver = reinterpret_cast<KinectDriver*>(freenect_get_user(dev));
00278 driver->irCb(dev, buf, timestamp);
00279 }
00280
00282 KinectDriver::~KinectDriver ()
00283 {
00284 freenect_close_device (f_dev_);
00285 freenect_shutdown (f_ctx_);
00286 }
00287
00289 void
00290 KinectDriver::start ()
00291 {
00292 freenect_start_depth (f_dev_);
00293 if (config_.color_format == FREENECT_FORMAT_IR)
00294 freenect_start_ir (f_dev_);
00295 else
00296 freenect_start_rgb (f_dev_);
00297
00298 started_ = true;
00299 }
00300
00302 void
00303 KinectDriver::stop ()
00304 {
00305 freenect_stop_depth (f_dev_);
00306 if (config_.color_format == FREENECT_FORMAT_IR)
00307 freenect_stop_ir (f_dev_);
00308 else
00309 freenect_stop_rgb (f_dev_);
00310
00311 started_ = false;
00312 }
00313
00319 void
00320 KinectDriver::depthCb (freenect_device *dev, freenect_depth *buf, uint32_t timestamp)
00321 {
00322 boost::mutex::scoped_lock lock (buffer_mutex_);
00323
00324 depth_buf_ = buf;
00325 depth_sent_ = false;
00326
00327
00328 if (!rgb_sent_)
00329 processRgbAndDepth ();
00330 }
00331
00337 void
00338 KinectDriver::rgbCb (freenect_device *dev, freenect_pixel *rgb, uint32_t timestamp)
00339 {
00340 boost::mutex::scoped_lock lock (buffer_mutex_);
00341
00342 rgb_buf_ = rgb;
00343 rgb_sent_ = false;
00344
00345
00346 if (!depth_sent_) {
00347 processRgbAndDepth ();
00348 }
00349 }
00350
00351 void
00352 KinectDriver::irCb (freenect_device *dev, freenect_pixel_ir *rgb, uint32_t timestamp)
00353 {
00354 boost::mutex::scoped_lock lock (buffer_mutex_);
00355
00356
00357 rgb_buf_ = reinterpret_cast<freenect_pixel*>(rgb);
00358 rgb_sent_ = false;
00359
00360 if (!depth_sent_) {
00361 processRgbAndDepth ();
00362 }
00363 }
00364
00365 void KinectDriver::processRgbAndDepth()
00366 {
00368
00369
00370 if (pub_rgb_.getNumSubscribers () > 0 &&
00371 config_.color_format != FREENECT_FORMAT_IR)
00372 {
00373
00374 memcpy (&rgb_image_.data[0], &rgb_buf_[0], rgb_image_.data.size());
00375
00376
00378 if (rgb_info_.height != (size_t)rgb_image_.height || rgb_info_.width != (size_t)rgb_image_.width)
00379 ROS_DEBUG_THROTTLE (60, "[KinectDriver::rgbCb] Uncalibrated Camera");
00380 }
00381
00382 if (pub_ir_.getNumSubscribers () > 0 &&
00383 config_.color_format == FREENECT_FORMAT_IR)
00384 {
00386 freenect_pixel_ir *ir = reinterpret_cast<freenect_pixel_ir*>(rgb_buf_);
00387 for (int i = 0; i < FREENECT_FRAME_PIX; ++i) {
00388 int val = ir[i];
00389 rgb_image_.data[i] = (val >> 2) & 0xff;
00390 }
00391 }
00392
00393 bool have_rgb = config_.color_format == FREENECT_FORMAT_RGB;
00394
00395
00397 cv::Mat rgb_raw(height_, width_, CV_8UC3, rgb_buf_);
00398
00399 if (pub_rgb_rect_.getNumSubscribers () > 0 ||
00400 pub_depth_points_.getNumSubscribers () > 0 ||
00401 pub_depth_points2_.getNumSubscribers () > 0 ||
00402 pub_rgb_points_.getNumSubscribers () > 0 ||
00403 pub_rgb_points2_.getNumSubscribers () > 0)
00404 {
00405 if (have_rgb)
00406 rgb_model_.rectifyImage(rgb_raw, rgb_rect_);
00407 else
00408 ROS_WARN_THROTTLE(60, "Color format must be RGB to publish point clouds");
00409 }
00410 double fT = depth_model_.fx() * baseline_;
00411
00413
00414 if (pub_depth_points_.getNumSubscribers () > 0 && have_rgb)
00415 {
00416
00417 cloud_.points.resize (0);
00418 cloud_.channels[0].values.resize(0);
00419 int k = 0;
00420 for (int v = 0; v < height_; ++v)
00421 {
00422 for (int u = 0; u < width_; ++u, ++k)
00423 {
00424 double d = SHIFT_SCALE * (shift_offset_ - depth_buf_[k]);
00425 if (d <= 0.0)
00426 continue;
00427
00428
00429 geometry_msgs::Point32 pt;
00430 pt.z = fT / d;
00431 pt.x = ((u - depth_model_.cx()) / depth_model_.fx()) * pt.z;
00432 pt.y = ((v - depth_model_.cy()) / depth_model_.fy()) * pt.z;
00433 cloud_.points.push_back(pt);
00434
00435
00436 Eigen::Vector4d uvd1;
00437 uvd1 << u, v, d, 1;
00438 Eigen::Vector3d uvw;
00439 uvw = depth_to_rgb_ * uvd1;
00440 int u_rgb = uvw[0]/uvw[2] + 0.5;
00441 int v_rgb = uvw[1]/uvw[2] + 0.5;
00442
00443 int32_t rgb_packed = 0;
00444 if (u_rgb >= 0 && u_rgb < width_ && v_rgb >= 0 && v_rgb < height_) {
00445 cv::Vec3b rgb = rgb_rect_.at<cv::Vec3b>(v_rgb, u_rgb);
00446 rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2];
00447 }
00448 cloud_.channels[0].values.push_back(*(float*)(&rgb_packed));
00449 }
00450 }
00451 }
00452
00453 if (pub_depth_points2_.getNumSubscribers () > 0 && have_rgb)
00454 {
00455
00456 float bad_point = std::numeric_limits<float>::quiet_NaN ();
00457 int k = 0;
00458 for (int v = 0; v < height_; ++v)
00459 {
00460 for (int u = 0; u < width_; ++u, ++k)
00461 {
00462 float* pt_data = reinterpret_cast<float*>(&cloud2_.data[0] + k * cloud2_.point_step);
00464
00465 double d = SHIFT_SCALE * (shift_offset_ - depth_buf_[k]);
00466 if (d <= 0.0) {
00467
00468 pt_data[0] = bad_point;
00469 pt_data[1] = bad_point;
00470 pt_data[2] = bad_point;
00471
00472 pt_data[3] = 0;
00473 continue;
00474 }
00475
00476
00477 double Z = fT / d;
00478 double X = ((u - depth_model_.cx()) / depth_model_.fx()) * Z;
00479 double Y = ((v - depth_model_.cy()) / depth_model_.fy()) * Z;
00480 pt_data[0] = X;
00481 pt_data[1] = Y;
00482 pt_data[2] = Z;
00483
00484
00485 Eigen::Vector4d uvd1;
00486 uvd1 << u, v, d, 1;
00487 Eigen::Vector3d uvw;
00488 uvw = depth_to_rgb_ * uvd1;
00489 int u_rgb = uvw[0]/uvw[2] + 0.5;
00490 int v_rgb = uvw[1]/uvw[2] + 0.5;
00491
00492
00493 int32_t rgb_packed = 0;
00494 if (u_rgb >= 0 && u_rgb < width_ && v_rgb >= 0 && v_rgb < height_) {
00495 cv::Vec3b rgb = rgb_rect_.at<cv::Vec3b>(v_rgb, u_rgb);
00496 rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2];
00497 }
00498 memcpy(&pt_data[3], &rgb_packed, sizeof(int32_t));
00499 }
00500 }
00501 }
00502
00503 if (pub_rgb_points_.getNumSubscribers () > 0 && have_rgb)
00504 {
00505
00506 int16_t MAX_READING = std::numeric_limits<int16_t>::max();
00507 cv::Mat z_buffer(height_, width_, CV_32SC2, cv::Scalar(MAX_READING, -1));
00508
00509 cloud_rgb_.points.resize (0);
00510 cloud_rgb_.channels[0].values.resize(0);
00511 int k = 0;
00512 for (int v = 0; v < height_; ++v)
00513 {
00514 for (int u = 0; u < width_; ++u, ++k)
00515 {
00516 int r = depth_buf_[k];
00517 double d = SHIFT_SCALE * (shift_offset_ - r);
00518 if (d <= 0.0)
00519 continue;
00520
00521
00522 double Z = fT / d;
00523 double X = ((u - depth_model_.cx()) / depth_model_.fx()) * Z;
00524 double Y = ((v - depth_model_.cy()) / depth_model_.fy()) * Z;
00525
00526
00527 Eigen::Vector4d xyz_depth, xyz_rgb;
00528 xyz_depth << X, Y, Z, 1;
00529 xyz_rgb = S * xyz_depth;
00530
00531
00532 int u_rgb = (xyz_rgb.x() / xyz_rgb.z()) * rgb_model_.fx() + rgb_model_.cx() + 0.5;
00533 int v_rgb = (xyz_rgb.y() / xyz_rgb.z()) * rgb_model_.fy() + rgb_model_.cy() + 0.5;
00534 if (u_rgb < 0 || u_rgb >= width_ || v_rgb < 0 || v_rgb >= height_)
00535 continue;
00536
00537
00538 geometry_msgs::Point32 pt;
00539 pt.x = xyz_rgb.x();
00540 pt.y = xyz_rgb.y();
00541 pt.z = xyz_rgb.z();
00542
00543
00544
00545 cv::Vec2i &reading = z_buffer.at<cv::Vec2i>(v_rgb, u_rgb);
00546 int32_t rgb_packed = 0;
00548
00549 if (r < reading[0]) {
00550 cv::Vec3b rgb = rgb_rect_.at<cv::Vec3b>(v_rgb, u_rgb);
00551 rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2];
00552
00553 int index = reading[1];
00554 if (index >= 0) {
00555 int32_t black = 0;
00556 cloud_rgb_.channels[0].values[index] = *(float*)(&black);
00557 }
00558
00559 reading[0] = r;
00560 }
00561 reading[1] = cloud_rgb_.points.size();
00562 cloud_rgb_.points.push_back(pt);
00563 cloud_rgb_.channels[0].values.push_back(*(float*)(&rgb_packed));
00564 }
00565 }
00566 }
00567
00568 if (pub_rgb_points2_.getNumSubscribers () > 0 && have_rgb)
00569 {
00570 float bad_point = std::numeric_limits<float>::quiet_NaN ();
00571 float* pt_data = reinterpret_cast<float*>(&cloud2_rgb_.data[0]);
00572 std::fill_n(pt_data, height_ * width_ * 4, bad_point);
00573 int k = 0;
00574 for (int v = 0; v < height_; ++v)
00575 {
00576 for (int u = 0; u < width_; ++u, ++k)
00577 {
00578 double d = SHIFT_SCALE * (shift_offset_ - depth_buf_[k]);
00579 if (d <= 0.0)
00580 continue;
00581
00582
00583 double Z = fT / d;
00584 double X = ((u - depth_model_.cx()) / depth_model_.fx()) * Z;
00585 double Y = ((v - depth_model_.cy()) / depth_model_.fy()) * Z;
00586
00587
00588 Eigen::Vector4d xyz_depth, xyz_rgb;
00589 xyz_depth << X, Y, Z, 1;
00590 xyz_rgb = S * xyz_depth;
00591
00592
00593 int u_rgb = (xyz_rgb.x() / xyz_rgb.z()) * rgb_model_.fx() + rgb_model_.cx() + 0.5;
00594 int v_rgb = (xyz_rgb.y() / xyz_rgb.z()) * rgb_model_.fy() + rgb_model_.cy() + 0.5;
00595 if (u_rgb < 0 || u_rgb >= width_ || v_rgb < 0 || v_rgb >= height_)
00596 continue;
00597
00598
00599 cv::Vec3b rgb = rgb_rect_.at<cv::Vec3b>(v_rgb, u_rgb);
00600 int32_t rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2];
00601 float* pt = pt_data + (v_rgb*width_ + u_rgb)*4;
00602
00603 if (!(pt[2] < xyz_rgb.z())) {
00604 pt[0] = xyz_rgb.x();
00605 pt[1] = xyz_rgb.y();
00606 pt[2] = xyz_rgb.z();
00607 memcpy(&pt[3], &rgb_packed, sizeof(int32_t));
00608 }
00609 }
00610 }
00611 }
00612
00613 if (pub_depth_.getNumSubscribers () > 0)
00614 {
00615
00616 depthBufferTo8BitImage(depth_buf_);
00617 }
00618
00619 publish();
00620 }
00621
00622 void
00623 KinectDriver::publish ()
00624 {
00626
00627 ros::Time time = ros::Time::now ();
00628 cloud_.header.stamp = cloud2_.header.stamp = cloud_rgb_.header.stamp = cloud2_rgb_.header.stamp = time;
00629 rgb_image_.header.stamp = rgb_info_.header.stamp = time;
00630 depth_image_.header.stamp = depth_info_.header.stamp = time;
00631
00632
00633 bool have_rgb = config_.color_format == FREENECT_FORMAT_RGB;
00634 if (config_.color_format == FREENECT_FORMAT_IR)
00635 {
00636 if (pub_ir_.getNumSubscribers() > 0)
00637 pub_ir_.publish (boost::make_shared<const sensor_msgs::Image> (rgb_image_),
00638 boost::make_shared<const sensor_msgs::CameraInfo> (depth_info_));
00639 }
00640 else
00641 {
00642 if (pub_rgb_.getNumSubscribers () > 0)
00643 pub_rgb_.publish (boost::make_shared<const sensor_msgs::Image> (rgb_image_),
00644 boost::make_shared<const sensor_msgs::CameraInfo> (rgb_info_));
00645 if (pub_rgb_rect_.getNumSubscribers () > 0 && have_rgb)
00646 {
00647 IplImage ipl = rgb_rect_;
00648 sensor_msgs::ImagePtr msg_ptr = sensor_msgs::CvBridge::cvToImgMsg(&ipl, "rgb8");
00649 msg_ptr->header.stamp = time;
00650 msg_ptr->header.frame_id = rgb_info_.header.frame_id;
00651 pub_rgb_rect_.publish(msg_ptr);
00652 }
00653 }
00654
00655
00656 if (pub_depth_.getNumSubscribers () > 0)
00657 pub_depth_.publish (boost::make_shared<const sensor_msgs::Image> (depth_image_), boost::make_shared<const sensor_msgs::CameraInfo> (depth_info_));
00658
00659
00660 if (pub_depth_points_.getNumSubscribers () > 0 && have_rgb)
00661 pub_depth_points_.publish (boost::make_shared<const sensor_msgs::PointCloud> (cloud_));
00662 if (pub_depth_points2_.getNumSubscribers () > 0 && have_rgb)
00663 pub_depth_points2_.publish (boost::make_shared<const sensor_msgs::PointCloud2> (cloud2_));
00664 if (pub_rgb_points_.getNumSubscribers () > 0 && have_rgb)
00665 pub_rgb_points_.publish (boost::make_shared<const sensor_msgs::PointCloud> (cloud_rgb_));
00666 if (pub_rgb_points2_.getNumSubscribers () > 0 && have_rgb)
00667 pub_rgb_points2_.publish (boost::make_shared<const sensor_msgs::PointCloud2> (cloud2_rgb_));
00668
00669 rgb_sent_ = true;
00670 depth_sent_ = true;
00671 }
00672
00673 void KinectDriver::publishImu()
00674 {
00675 imu_msg_.header.stamp = ros::Time::now();
00676 imu_msg_.linear_acceleration.x = accel_x_;
00677 imu_msg_.linear_acceleration.y = accel_y_;
00678 imu_msg_.linear_acceleration.z = accel_z_;
00679 imu_msg_.linear_acceleration_covariance[0] = imu_msg_.linear_acceleration_covariance[4]
00680 = imu_msg_.linear_acceleration_covariance[8] = 0.01;
00681 imu_msg_.angular_velocity_covariance[0] = -1;
00682 imu_msg_.orientation_covariance[0] = -1;
00683 if (pub_imu_.getNumSubscribers() > 0)
00684 pub_imu_.publish(imu_msg_);
00685 }
00686
00687 void KinectDriver::configCb (Config &config, uint32_t level)
00688 {
00690
00691
00693 if (config.color_format == FREENECT_FORMAT_RGB) {
00694 rgb_image_.encoding = sensor_msgs::image_encodings::RGB8;
00695 rgb_image_.data.resize(FREENECT_RGB_SIZE);
00696 rgb_image_.step = FREENECT_FRAME_W * 3;
00697 }
00698 else if (config.color_format == FREENECT_FORMAT_BAYER) {
00699 rgb_image_.encoding = sensor_msgs::image_encodings::BAYER_GRBG8;
00700 rgb_image_.data.resize(FREENECT_BAYER_SIZE);
00701 rgb_image_.step = FREENECT_FRAME_W;
00702 }
00703 else if (config.color_format == FREENECT_FORMAT_IR) {
00704 rgb_image_.encoding = sensor_msgs::image_encodings::MONO8;
00705 rgb_image_.data.resize(FREENECT_BAYER_SIZE);
00706 rgb_image_.step = FREENECT_FRAME_W;
00707 }
00708 else {
00709 ROS_ERROR("Unknown color format code %d", config.color_format);
00710 }
00711
00712 config_ = config;
00713 updateDeviceSettings();
00714 }
00715
00716 void KinectDriver::updateDeviceSettings()
00717 {
00718 if (f_dev_) {
00719 freenect_set_rgb_format(f_dev_, (freenect_rgb_format)config_.color_format);
00720 freenect_set_led(f_dev_, (freenect_led_options)config_.led);
00721 freenect_set_tilt_degs(f_dev_, config_.tilt);
00722
00723 if (started_) {
00724 if (config_.color_format == FREENECT_FORMAT_IR) {
00725 freenect_stop_rgb(f_dev_);
00726 freenect_start_ir(f_dev_);
00727 }
00728 else {
00729 freenect_stop_ir(f_dev_);
00730 freenect_start_rgb(f_dev_);
00731 }
00732 }
00733 }
00734 }
00735
00736
00737
00738
00739
00740
00741
00742
00743
00744
00745 void KinectDriver::depthBufferTo8BitImage(const freenect_depth * buf)
00746 {
00747 double fT = depth_model_.fx() * baseline_;
00748
00749 for (int y=0; y<height_; ++y)
00750 for (int x=0; x<width_; ++x)
00751 {
00752 int index(y*width_ + x);
00753 int reading = buf[index];
00754 double range = fT / (shift_offset_ - reading);
00755
00756 uint8_t color;
00757
00758 if (range > config_.max_range || range < 0)
00759 color = 255;
00760 else
00761 color = 255 * range / config_.max_range;
00762
00763 depth_image_.data[index] = color;
00764 }
00765 }
00766
00767 }