driver.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011 Willow Garage, Inc.
5  * Suat Gedikli <gedikli@willowgarage.com>
6  * Patrick Michelich <michelich@willowgarage.com>
7  * Radu Bogdan Rusu <rusu@willowgarage.com>
8  *
9  * All rights reserved.
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  *
15  * * Redistributions of source code must retain the above copyright
16  * notice, this list of conditions and the following disclaimer.
17  * * Redistributions in binary form must reproduce the above
18  * copyright notice, this list of conditions and the following
19  * disclaimer in the documentation and/or other materials provided
20  * with the distribution.
21  * * Neither the name of Willow Garage, Inc. nor the names of its
22  * contributors may be used to endorse or promote products derived
23  * from this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  *
38  */
39 #include "driver.h"
47 #include <boost/algorithm/string/replace.hpp>
48 
49 #include <log4cxx/logger.h>
50 
51 using namespace std;
52 using namespace openni_wrapper;
53 namespace openni_camera
54 {
55 inline bool operator == (const XnMapOutputMode& mode1, const XnMapOutputMode& mode2)
56 {
57  return (mode1.nXRes == mode2.nXRes && mode1.nYRes == mode2.nYRes && mode1.nFPS == mode2.nFPS);
58 }
59 inline bool operator != (const XnMapOutputMode& mode1, const XnMapOutputMode& mode2)
60 {
61  return !(mode1 == mode2);
62 }
63 
64 DriverNodelet::~DriverNodelet ()
65 {
66  // If we're still stuck in initialization (e.g. can't connect to device), break out
67  init_thread_.interrupt();
68  init_thread_.join();
69 
70  // Join OpenNI wrapper threads, which call into rgbCb() etc. Those may use device_ methods,
71  // so make sure they've finished before destroying device_.
72  if (device_)
73  device_->shutdown();
74 
77 }
78 
79 void DriverNodelet::onInit ()
80 {
81  ros::NodeHandle& param_nh = getPrivateNodeHandle();
82 
83  config_init_ = false;
84 
85  // Initialize the sensor, but don't start any streams yet. That happens in the connection callbacks.
86  updateModeMaps();
87  setupDevice();
88 
89  // Initialize dynamic reconfigure
90  reconfigure_server_.reset( new ReconfigureServer(param_nh) );
91  reconfigure_server_->setCallback(boost::bind(&DriverNodelet::configCb, this, _1, _2));
92 
93  // Setting up device can take awhile but onInit shouldn't block, so we spawn a
94  // new thread to do all the initialization
95  init_thread_ = boost::thread(boost::bind(&DriverNodelet::onInitImpl, this));
96 }
97 
98 void DriverNodelet::onInitImpl ()
99 {
100  ros::NodeHandle& nh = getNodeHandle(); // topics
101  ros::NodeHandle& param_nh = getPrivateNodeHandle(); // parameters
102 
103  // Allow remapping namespaces rgb, ir, depth, depth_registered
105  ros::NodeHandle rgb_nh(nh, "rgb");
106  image_transport::ImageTransport rgb_it(rgb_nh);
107  ros::NodeHandle ir_nh(nh, "ir");
108  image_transport::ImageTransport ir_it(ir_nh);
109  ros::NodeHandle depth_nh(nh, "depth");
110  image_transport::ImageTransport depth_it(depth_nh);
111  ros::NodeHandle depth_registered_nh(nh, "depth_registered");
112  image_transport::ImageTransport depth_registered_it(depth_registered_nh);
113  ros::NodeHandle projector_nh(nh, "projector");
114 
115  rgb_frame_counter_ = depth_frame_counter_ = ir_frame_counter_ = 0;
116  publish_rgb_ = publish_ir_ = publish_depth_ = true;
117 
118 
119  // Camera TF frames
120  param_nh.param("rgb_frame_id", rgb_frame_id_, string("/openni_rgb_optical_frame"));
121  param_nh.param("depth_frame_id", depth_frame_id_, string("/openni_depth_optical_frame"));
122  NODELET_INFO("rgb_frame_id = '%s' ", rgb_frame_id_.c_str());
123  NODELET_INFO("depth_frame_id = '%s' ", depth_frame_id_.c_str());
124 
125  // Pixel offset between depth and IR images.
126  // By default assume offset of (5,4) from 9x7 correlation window.
127  // NOTE: These are now (temporarily?) dynamically reconfigurable, to allow tweaking.
128  //param_nh.param("depth_ir_offset_x", depth_ir_offset_x_, 5.0);
129  //param_nh.param("depth_ir_offset_y", depth_ir_offset_y_, 4.0);
130 
131  // The camera names are set to [rgb|depth]_[serial#], e.g. depth_B00367707227042B.
132  // camera_info_manager substitutes this for ${NAME} in the URL.
133  std::string serial_number = device_->getSerialNumber();
134  std::string rgb_name, ir_name;
135  if (serial_number.empty())
136  {
137  rgb_name = "rgb";
138  ir_name = "depth";
139  }
140  else
141  {
142  rgb_name = "rgb_" + serial_number;
143  ir_name = "depth_" + serial_number;
144  }
145 
146  std::string rgb_info_url, ir_info_url;
147  param_nh.param("rgb_camera_info_url", rgb_info_url, std::string());
148  param_nh.param("depth_camera_info_url", ir_info_url, std::string());
149 
150  // Suppress some of the output from loading camera calibrations (kinda hacky)
151  log4cxx::LoggerPtr logger_ccp = log4cxx::Logger::getLogger("ros.camera_calibration_parsers");
152  log4cxx::LoggerPtr logger_cim = log4cxx::Logger::getLogger("ros.camera_info_manager");
153  logger_ccp->setLevel(log4cxx::Level::getFatal());
154  logger_cim->setLevel(log4cxx::Level::getWarn());
155  // Also suppress sync warnings from image_transport::CameraSubscriber. When subscribing to
156  // depth_registered/foo with OpenNI registration disabled, the rectify nodelet for depth_registered/
157  // will complain because it receives depth_registered/camera_info (from the register nodelet), but
158  // the driver is not publishing depth_registered/image_raw.
159  log4cxx::LoggerPtr logger_its = log4cxx::Logger::getLogger("ros.image_transport.sync");
160  logger_its->setLevel(log4cxx::Level::getError());
162 
163  // Load the saved calibrations, if they exist
164  rgb_info_manager_ = boost::make_shared<camera_info_manager::CameraInfoManager>(rgb_nh, rgb_name, rgb_info_url);
165  ir_info_manager_ = boost::make_shared<camera_info_manager::CameraInfoManager>(ir_nh, ir_name, ir_info_url);
166 
167  if (!rgb_info_manager_->isCalibrated())
168  NODELET_WARN("Using default parameters for RGB camera calibration.");
169  if (!ir_info_manager_->isCalibrated())
170  NODELET_WARN("Using default parameters for IR camera calibration.");
171 
172  // Advertise all published topics
173  {
174  // Prevent connection callbacks from executing until we've set all the publishers. Otherwise
175  // connectCb() can fire while we're advertising (say) "depth/image_raw", but before we actually
176  // assign to pub_depth_. Then pub_depth_.getNumSubscribers() returns 0, and we fail to start
177  // the depth generator.
178  boost::lock_guard<boost::mutex> lock(connect_mutex_);
179 
180  // Asus Xtion PRO does not have an RGB camera
181  if (device_->hasImageStream())
182  {
183  image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::rgbConnectCb, this);
184  ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::rgbConnectCb, this);
185  pub_rgb_ = rgb_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
186  }
187 
188  if (device_->hasIRStream())
189  {
190  image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::irConnectCb, this);
191  ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::irConnectCb, this);
192  pub_ir_ = ir_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
193  }
194 
195  if (device_->hasDepthStream())
196  {
197  image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::depthConnectCb, this);
198  ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::depthConnectCb, this);
199  pub_depth_ = depth_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
200  pub_projector_info_ = projector_nh.advertise<sensor_msgs::CameraInfo>("camera_info", 1, rssc, rssc);
201 
202  if (device_->isDepthRegistrationSupported())
203  pub_depth_registered_ = depth_registered_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
204  }
205  }
206 
207  // Create watch dog timer callback
208  if (param_nh.getParam("time_out", time_out_) && time_out_ > 0.0)
209  {
210  time_stamp_ = ros::Time(0,0);
211  watch_dog_timer_ = nh.createTimer(ros::Duration(time_out_), &DriverNodelet::watchDog, this);
212  }
213 }
214 
215 void DriverNodelet::setupDevice ()
216 {
217  // Initialize the openni device
218  OpenNIDriver& driver = OpenNIDriver::getInstance ();
219 
220  do {
221  driver.updateDeviceList ();
222 
223  if (driver.getNumberDevices () == 0)
224  {
225  NODELET_INFO ("No devices connected.... waiting for devices to be connected");
226  boost::this_thread::sleep(boost::posix_time::seconds(3));
227  continue;
228  }
229 
230  NODELET_INFO ("Number devices connected: %d", driver.getNumberDevices ());
231  for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
232  {
233  NODELET_INFO("%u. device on bus %03u:%02u is a %s (%03x) from %s (%03x) with serial id \'%s\'",
234  deviceIdx + 1, driver.getBus(deviceIdx), driver.getAddress(deviceIdx),
235  driver.getProductName(deviceIdx), driver.getProductID(deviceIdx),
236  driver.getVendorName(deviceIdx), driver.getVendorID(deviceIdx),
237  driver.getSerialNumber(deviceIdx));
238  }
239 
240  try {
241  string device_id;
242  int device_id_int;
243  if (!getPrivateNodeHandle().getParam("device_id", device_id) &&
244  !getPrivateNodeHandle().getParam("device_id", device_id_int))
245  {
246 
247  NODELET_WARN ("~device_id is not set! Using first device.");
248  device_ = driver.getDeviceByIndex (0);
249  }
250  else if (device_id.find ('@') != string::npos)
251  {
252  size_t pos = device_id.find ('@');
253  unsigned bus = atoi (device_id.substr (0, pos).c_str ());
254  unsigned address = atoi (device_id.substr (pos + 1, device_id.length () - pos - 1).c_str ());
255  NODELET_INFO ("Searching for device with bus@address = %d@%d", bus, address);
256  device_ = driver.getDeviceByAddress (bus, address);
257  }
258  else if (device_id[0] == '#')
259  {
260  unsigned index = atoi (device_id.c_str () + 1);
261  NODELET_INFO ("Searching for device with index = %d", index);
262  device_ = driver.getDeviceByIndex (index - 1);
263  }
264  else
265  {
266  if (device_id.empty()) // The ID passed contains only numbers
267  {
268  std::stringstream ss;
269  ss << device_id_int;
270  device_id = ss.str();
271  }
272  NODELET_INFO ("Searching for device with serial number = '%s'", device_id.c_str ());
273  device_ = driver.getDeviceBySerialNumber (device_id);
274  }
275  }
276  catch (const OpenNIException& exception)
277  {
278  if (!device_)
279  {
280  NODELET_INFO ("No matching device found.... waiting for devices. Reason: %s", exception.what ());
281  boost::this_thread::sleep(boost::posix_time::seconds(3));
282  continue;
283  }
284  else
285  {
286  NODELET_ERROR ("Could not retrieve device. Reason: %s", exception.what ());
287  exit (-1);
288  }
289  }
290  } while (!device_);
291 
292  NODELET_INFO ("Opened '%s' on bus %d:%d with serial number '%s'", device_->getProductName (),
293  device_->getBus (), device_->getAddress (), device_->getSerialNumber ());
294 
295  device_->registerImageCallback(&DriverNodelet::rgbCb, *this);
296  device_->registerDepthCallback(&DriverNodelet::depthCb, *this);
297  device_->registerIRCallback (&DriverNodelet::irCb, *this);
298 }
299 
300 void DriverNodelet::rgbConnectCb()
301 {
302  boost::lock_guard<boost::mutex> lock(connect_mutex_);
303  bool need_rgb = pub_rgb_.getNumSubscribers() > 0;
304 
305  if (need_rgb && !device_->isImageStreamRunning())
306  {
307  // Can't stream IR and RGB at the same time. Give RGB preference.
308  if (device_->isIRStreamRunning())
309  {
310  NODELET_ERROR("Cannot stream RGB and IR at the same time. Streaming RGB only.");
311  device_->stopIRStream();
312  }
313 
314  device_->startImageStream();
315  startSynchronization();
316  time_stamp_ = ros::Time(0,0); // starting an additional stream blocks for a while, could upset watchdog
317  }
318  else if (!need_rgb && device_->isImageStreamRunning())
319  {
320  stopSynchronization();
321  device_->stopImageStream();
322 
323  // Start IR if it's been blocked on RGB subscribers
324  bool need_ir = pub_ir_.getNumSubscribers() > 0;
325  if (need_ir && !device_->isIRStreamRunning())
326  {
327  device_->startIRStream();
328  time_stamp_ = ros::Time(0,0);
329  }
330  }
331 }
332 
333 void DriverNodelet::depthConnectCb()
334 {
335  boost::lock_guard<boost::mutex> lock(connect_mutex_);
337  bool need_depth =
338  device_->isDepthRegistered() ? pub_depth_registered_.getNumSubscribers() > 0 : pub_depth_.getNumSubscribers() > 0;
340 
341  if (need_depth && !device_->isDepthStreamRunning())
342  {
343  device_->startDepthStream();
344  startSynchronization();
345  time_stamp_ = ros::Time(0,0); // starting an additional stream blocks for a while, could upset watchdog
346  }
347  else if (!need_depth && device_->isDepthStreamRunning())
348  {
349  stopSynchronization();
350  device_->stopDepthStream();
351  }
352 }
353 
354 void DriverNodelet::irConnectCb()
355 {
356  boost::lock_guard<boost::mutex> lock(connect_mutex_);
357  bool need_ir = pub_ir_.getNumSubscribers() > 0;
358 
359  if (need_ir && !device_->isIRStreamRunning())
360  {
361  // Can't stream IR and RGB at the same time
362  if (device_->isImageStreamRunning())
363  {
364  NODELET_ERROR("Cannot stream RGB and IR at the same time. Streaming RGB only.");
365  }
366  else
367  {
368  device_->startIRStream();
369  time_stamp_ = ros::Time(0,0); // starting an additional stream blocks for a while, could upset watchdog
370  }
371  }
372  else if (!need_ir)
373  {
374  device_->stopIRStream();
375  }
376 }
377 
378 // If any stream is ready, publish next available image from all streams
379 // This publishes all available data with a maximum time offset of one frame between any two sources
380 // Need to have lock to call this, since callbacks can be in different threads
381 void DriverNodelet::checkFrameCounters()
382 {
383  if (max(rgb_frame_counter_, max(depth_frame_counter_, ir_frame_counter_)) > config_.data_skip) {
384  // Reset all counters after we trigger publish
385  rgb_frame_counter_ = 0;
386  depth_frame_counter_ = 0;
387  ir_frame_counter_ = 0;
388 
389  // Trigger publish on all topics
390  publish_rgb_ = true;
391  publish_depth_ = true;
392  publish_ir_ = true;
393  }
394 }
395 
396 void DriverNodelet::rgbCb(boost::shared_ptr<openni_wrapper::Image> image, void* cookie)
397 {
398  if (!config_init_)
399  return;
400 
401  ros::Time time = ros::Time::now () + ros::Duration(config_.image_time_offset);
402  time_stamp_ = time; // for watchdog
403 
404  bool publish = false;
405  {
406  boost::unique_lock<boost::mutex> counter_lock(counter_mutex_);
407  rgb_frame_counter_++;
408  checkFrameCounters();
409  publish = publish_rgb_;
410 
411  if (publish)
412  rgb_frame_counter_ = 0; // Reset counter if we publish this message to avoid under-throttling
413  }
414 
415  if (publish)
416  publishRgbImage(*image, time);
417 
418  publish_rgb_ = false;
419 }
420 
421 void DriverNodelet::depthCb(boost::shared_ptr<openni_wrapper::DepthImage> depth_image, void* cookie)
422 {
423  if (!config_init_)
424  return;
425 
426  ros::Time time = ros::Time::now () + ros::Duration(config_.depth_time_offset);
427  time_stamp_ = time; // for watchdog
428 
429  bool publish = false;
430  {
431  boost::unique_lock<boost::mutex> counter_lock(counter_mutex_);
432  depth_frame_counter_++;
433  checkFrameCounters();
434  publish = publish_depth_;
435 
436  if (publish)
437  depth_frame_counter_ = 0; // Reset counter if we publish this message to avoid under-throttling
438  }
439 
440  if (publish)
441  publishDepthImage(*depth_image, time);
442 
443  publish_depth_ = false;
444 }
445 
446 void DriverNodelet::irCb(boost::shared_ptr<openni_wrapper::IRImage> ir_image, void* cookie)
447 {
448  if (!config_init_)
449  return;
450 
451  ros::Time time = ros::Time::now() + ros::Duration(config_.depth_time_offset);
452  time_stamp_ = time; // for watchdog
453 
454  bool publish = false;
455  {
456  boost::unique_lock<boost::mutex> counter_lock(counter_mutex_);
457  ir_frame_counter_++;
458  checkFrameCounters();
459  publish = publish_ir_;
460 
461  if (publish)
462  ir_frame_counter_ = 0; // Reset counter if we publish this message to avoid under-throttling
463  }
464 
465  if (publish)
466  publishIrImage(*ir_image, time);
467  publish_ir_ = false;
468 }
469 
470 void DriverNodelet::publishRgbImage(const openni_wrapper::Image& image, ros::Time time) const
471 {
472  sensor_msgs::ImagePtr rgb_msg = boost::make_shared<sensor_msgs::Image >();
473  rgb_msg->header.stamp = time;
474  rgb_msg->header.frame_id = rgb_frame_id_;
475  bool downscale = false;
477  {
478  if (image.getWidth() == image_width_ && image.getHeight() == image_height_)
479  {
480  // image sizes match, we can copy directly
481  rgb_msg->encoding = sensor_msgs::image_encodings::BAYER_GRBG8;
482  rgb_msg->step = image_width_;
483  }
484  else
485  {
486  // image sizes missmatch, we have to downscale and de-bayer in this function
487  rgb_msg->encoding = sensor_msgs::image_encodings::RGB8;
488  rgb_msg->step = image_width_ * 3;
489  downscale = true;
490  }
491  }
492  else if (image.getEncoding() == openni_wrapper::Image::YUV422)
493  {
494  if (image.getWidth() == image_width_ && image.getHeight() == image_height_)
495  {
496  // image sizes match, we can copy directly
497  rgb_msg->encoding = sensor_msgs::image_encodings::YUV422;
498  rgb_msg->step = image_width_ * 2; // 4 bytes for 2 pixels
499  }
500  else
501  {
502  // image sizes missmatch, we have to downscale and convert in this function
503  rgb_msg->encoding = sensor_msgs::image_encodings::RGB8;
504  rgb_msg->step = image_width_ * 3;
505  downscale = true;
506  }
507  }
508  rgb_msg->height = image_height_;
509  rgb_msg->width = image_width_;
510  rgb_msg->data.resize(rgb_msg->height * rgb_msg->step);
511 
512  try
513  {
514  if (downscale)
515  {
517  {
519  bayer_image.fillRGB(image_width_, image_height_, &rgb_msg->data[0]);
520  }
521  else if (image.getEncoding() == openni_wrapper::Image::YUV422)
522  {
523  openni_wrapper::ImageYUV422 yuv_image(image.getMetaDataPtr());
524  yuv_image.fillRGB(image_width_, image_height_, &rgb_msg->data[0]);
525  }
526  }
527  else
528  image.fillRaw(&rgb_msg->data[0]);
529  }
530  catch ( OpenNIException e )
531  {
532  ROS_ERROR_THROTTLE(1,"%s",e.what());
533  }
534 
535  pub_rgb_.publish(rgb_msg, getRgbCameraInfo(rgb_msg->width,rgb_msg->height,time));
536 }
537 
538 void DriverNodelet::publishDepthImage(const openni_wrapper::DepthImage& depth, ros::Time time) const
539 {
540  bool registered = device_->isDepthRegistered();
541 
543  sensor_msgs::ImagePtr depth_msg = boost::make_shared<sensor_msgs::Image>();
544  depth_msg->header.stamp = time;
545  depth_msg->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
546  depth_msg->width = depth_width_;
547  depth_msg->height = depth_height_;
548  depth_msg->step = depth_msg->width * sizeof(short);
549  depth_msg->data.resize(depth_msg->height * depth_msg->step);
550 
551  try
552  {
553  depth.fillDepthImageRaw(depth_msg->width, depth_msg->height, reinterpret_cast<unsigned short*>(&depth_msg->data[0]),
554  depth_msg->step);
555  }
556  catch ( OpenNIException e )
557  {
558  ROS_ERROR_THROTTLE(1,"%s",e.what());
559  }
560 
561  if (fabs(z_scaling_ - 1.0) > 1e-6)
562  {
563  uint16_t* data = reinterpret_cast<uint16_t*>(&depth_msg->data[0]);
564  for (unsigned int i = 0; i < depth_msg->width * depth_msg->height; ++i)
565  if (data[i] != 0)
566  data[i] = static_cast<uint16_t>(data[i] * z_scaling_);
567  }
568 
569  if (z_offset_mm_ != 0)
570  {
571  uint16_t* data = reinterpret_cast<uint16_t*>(&depth_msg->data[0]);
572  for (unsigned int i = 0; i < depth_msg->width * depth_msg->height; ++i)
573  if (data[i] != 0)
574  data[i] += z_offset_mm_;
575  }
576 
577  if (registered)
578  {
579  // Publish RGB camera info and raw depth image to depth_registered/ ns
580  depth_msg->header.frame_id = rgb_frame_id_;
581  pub_depth_registered_.publish(depth_msg, getRgbCameraInfo(depth_msg->width, depth_msg->height, time));
582  }
583  else
584  {
585  // Publish depth camera info and raw depth image to depth/ ns
586  depth_msg->header.frame_id = depth_frame_id_;
587  pub_depth_.publish(depth_msg, getDepthCameraInfo(depth_msg->width, depth_msg->height, time));
588  }
589 
590  // Projector "info" probably only useful for working with disparity images
591  if (pub_projector_info_.getNumSubscribers() > 0)
592  {
593  pub_projector_info_.publish(getProjectorCameraInfo(depth_msg->width, depth_msg->height, time));
594  }
595 }
596 
597 void DriverNodelet::publishIrImage(const openni_wrapper::IRImage& ir, ros::Time time) const
598 {
599  sensor_msgs::ImagePtr ir_msg = boost::make_shared<sensor_msgs::Image>();
600  ir_msg->header.stamp = time;
601  ir_msg->header.frame_id = depth_frame_id_;
602  ir_msg->encoding = sensor_msgs::image_encodings::MONO16;
603  ir_msg->height = ir.getHeight();
604  ir_msg->width = ir.getWidth();
605  ir_msg->step = ir_msg->width * sizeof(uint16_t);
606  ir_msg->data.resize(ir_msg->height * ir_msg->step);
607 
608  try
609  {
610  ir.fillRaw(ir.getWidth(), ir.getHeight(), reinterpret_cast<unsigned short*>(&ir_msg->data[0]));
611  }
612  catch ( OpenNIException e )
613  {
614  ROS_ERROR_THROTTLE(1,"%s",e.what());
615  }
616 
617  pub_ir_.publish(ir_msg, getIrCameraInfo(ir.getWidth(), ir.getHeight(), time));
618 }
619 
620 sensor_msgs::CameraInfoPtr DriverNodelet::getDefaultCameraInfo(int width, int height, double f) const
621 {
622  sensor_msgs::CameraInfoPtr info = boost::make_shared<sensor_msgs::CameraInfo>();
623 
624  info->width = width;
625  info->height = height;
626 
627  // No distortion
628  info->D.resize(5, 0.0);
629  info->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
630 
631  // Simple camera matrix: square pixels (fx = fy), principal point at center
632  info->K.assign(0.0);
633  info->K[0] = info->K[4] = f;
634  info->K[2] = (width / 2) - 0.5;
635  // Aspect ratio for the camera center on Kinect (and other devices?) is 4/3
636  // This formula keeps the principal point the same in VGA and SXGA modes
637  info->K[5] = (width * (3./8.)) - 0.5;
638  info->K[8] = 1.0;
639 
640  // No separate rectified image plane, so R = I
641  info->R.assign(0.0);
642  info->R[0] = info->R[4] = info->R[8] = 1.0;
643 
644  // Then P=K(I|0) = (K|0)
645  info->P.assign(0.0);
646  info->P[0] = info->P[5] = f; // fx, fy
647  info->P[2] = info->K[2]; // cx
648  info->P[6] = info->K[5]; // cy
649  info->P[10] = 1.0;
650 
651  return info;
652 }
653 
655 sensor_msgs::CameraInfoPtr DriverNodelet::getRgbCameraInfo(int width, int height, ros::Time time) const
656 {
657  sensor_msgs::CameraInfoPtr info;
658 
659  if (rgb_info_manager_->isCalibrated())
660  {
661  info = boost::make_shared<sensor_msgs::CameraInfo>(rgb_info_manager_->getCameraInfo());
662  if ( info->width != width )
663  {
664  // Use uncalibrated values
665  ROS_WARN_ONCE("Image resolution doesn't match calibration of the RGB camera. Using default parameters.");
666  info = getDefaultCameraInfo(width, height, device_->getImageFocalLength(width));
667  }
668  }
669  else
670  {
671  // If uncalibrated, fill in default values
672  info = getDefaultCameraInfo(width, height, device_->getImageFocalLength(width));
673  }
674 
675  // Fill in header
676  info->header.stamp = time;
677  info->header.frame_id = rgb_frame_id_;
678 
679  return info;
680 }
681 
682 sensor_msgs::CameraInfoPtr DriverNodelet::getIrCameraInfo(int width, int height, ros::Time time) const
683 {
684  sensor_msgs::CameraInfoPtr info;
685 
686  if (ir_info_manager_->isCalibrated())
687  {
688  info = boost::make_shared<sensor_msgs::CameraInfo>(ir_info_manager_->getCameraInfo());
689  if ( info->width != width )
690  {
691  // Use uncalibrated values
692  ROS_WARN_ONCE("Image resolution doesn't match calibration of the IR camera. Using default parameters.");
693  info = getDefaultCameraInfo(width, height, device_->getImageFocalLength(width));
694  }
695  }
696  else
697  {
698  // If uncalibrated, fill in default values
699  info = getDefaultCameraInfo(width, height, device_->getDepthFocalLength(width));
700  }
701 
702  // Fill in header
703  info->header.stamp = time;
704  info->header.frame_id = depth_frame_id_;
705 
706  return info;
707 }
708 
709 sensor_msgs::CameraInfoPtr DriverNodelet::getDepthCameraInfo(int width, int height, ros::Time time) const
710 {
711  // The depth image has essentially the same intrinsics as the IR image, BUT the
712  // principal point is offset by half the size of the hardware correlation window
713  // (probably 9x9 or 9x7 in 640x480 mode). See http://www.ros.org/wiki/kinect_calibration/technical
714 
715  double scaling = (double)width / 640;
716 
717  sensor_msgs::CameraInfoPtr info = getIrCameraInfo(width, height, time);
718  info->K[2] -= depth_ir_offset_x_*scaling; // cx
719  info->K[5] -= depth_ir_offset_y_*scaling; // cy
720  info->P[2] -= depth_ir_offset_x_*scaling; // cx
721  info->P[6] -= depth_ir_offset_y_*scaling; // cy
722 
724  return info;
725 }
726 
727 sensor_msgs::CameraInfoPtr DriverNodelet::getProjectorCameraInfo(int width, int height, ros::Time time) const
728 {
729  // The projector info is simply the depth info with the baseline encoded in the P matrix.
730  // It's only purpose is to be the "right" camera info to the depth camera's "left" for
731  // processing disparity images.
732  sensor_msgs::CameraInfoPtr info = getDepthCameraInfo(width, height, time);
733  // Tx = -baseline * fx
734  info->P[3] = -device_->getBaseline() * info->P[0];
735  return info;
736 }
737 
738 void DriverNodelet::configCb(Config &config, uint32_t level)
739 {
740  depth_ir_offset_x_ = config.depth_ir_offset_x;
741  depth_ir_offset_y_ = config.depth_ir_offset_y;
742  z_offset_mm_ = config.z_offset_mm;
743  z_scaling_ = config.z_scaling;
744 
745  // Based on the config options, try to set the depth/image mode
746  // The device driver might decide to switch to a 'compatible mode',
747  // i.e. a higher resolution than the requested one, in which case it
748  // will later downsample the image.
749  // Only if the 'compatible mode' is different from the current one, we will
750  // need to reset the streams.
751 
752  bool depth_mode_changed = false;
753  bool image_mode_changed = false;
754 
755  XnMapOutputMode compatible_image_mode;
756  XnMapOutputMode compatible_depth_mode;
757 
758  if (device_->hasImageStream ())
759  {
760  XnMapOutputMode image_mode = mapConfigMode2XnMode (config.image_mode);
761  image_width_ = image_mode.nXRes;
762  image_height_ = image_mode.nYRes;
763 
764  // does the device support the new image mode?
765  if (!device_->findCompatibleImageMode (image_mode, compatible_image_mode))
766  {
767  XnMapOutputMode default_mode = device_->getDefaultImageMode();
768  NODELET_WARN("Could not find any compatible image output mode for %d x %d @ %d. "
769  "Falling back to default image output mode %d x %d @ %d.",
770  image_mode.nXRes, image_mode.nYRes, image_mode.nFPS,
771  default_mode.nXRes, default_mode.nYRes, default_mode.nFPS);
772 
773  config.image_mode = mapXnMode2ConfigMode(default_mode);
774  image_mode = compatible_image_mode = default_mode;
775  }
776 
777  if ( compatible_image_mode != device_->getImageOutputMode () )
778  {
779  image_mode_changed = true;
780  }
781  }
782 
783  if (device_->hasDepthStream())
784  {
785  XnMapOutputMode depth_mode = mapConfigMode2XnMode (config.depth_mode);
786  depth_width_ = depth_mode.nXRes;
787  depth_height_ = depth_mode.nYRes;
788 
789  // does the device support the new depth mode?
790  if (!device_->findCompatibleDepthMode (depth_mode, compatible_depth_mode))
791  {
792  XnMapOutputMode default_mode = device_->getDefaultDepthMode();
793  NODELET_WARN ("Could not find any compatible depth output mode for %d x %d @ %d. "
794  "Falling back to default depth output mode %d x %d @ %d.",
795  depth_mode.nXRes, depth_mode.nYRes, depth_mode.nFPS,
796  default_mode.nXRes, default_mode.nYRes, default_mode.nFPS);
797 
798  config.depth_mode = mapXnMode2ConfigMode(default_mode);
799  depth_mode = compatible_depth_mode = default_mode;
800  }
801 
802  if ( compatible_depth_mode != device_->getDepthOutputMode () )
803  {
804  depth_mode_changed = true;
805  }
806  }
807 
808  // here everything is fine. Now make the changes
809  if ( image_mode_changed || depth_mode_changed )
810  {
811  // streams need to be reset!
812  stopSynchronization();
813 
814  if ( image_mode_changed )
815  {
816  device_->setImageOutputMode (compatible_image_mode);
817  }
818 
819  if ( depth_mode_changed )
820  {
821  device_->setDepthOutputMode (compatible_depth_mode);
822  }
823 
824  startSynchronization ();
825  }
826 
828  if (device_->isDepthRegistered () && !config.depth_registration)
829  {
830  device_->setDepthRegistration (false);
831  }
832  else if (!device_->isDepthRegistered () && config.depth_registration)
833  {
834  device_->setDepthRegistration (true);
835  }
836 
837  // now we can copy
838  config_ = config;
839 
840  config_init_ = true;
841 }
842 
843 void DriverNodelet::startSynchronization()
844 {
845  if (device_->isSynchronizationSupported() &&
846  !device_->isSynchronized() &&
847  device_->getImageOutputMode().nFPS == device_->getDepthOutputMode().nFPS &&
848  device_->isImageStreamRunning() &&
849  device_->isDepthStreamRunning() )
850  {
851  device_->setSynchronization(true);
852  }
853 }
854 
855 void DriverNodelet::stopSynchronization()
856 {
857  if (device_->isSynchronizationSupported() &&
858  device_->isSynchronized())
859  {
860  device_->setSynchronization(false);
861  }
862 }
863 
864 void DriverNodelet::updateModeMaps ()
865 {
866  XnMapOutputMode output_mode;
867 
868  output_mode.nXRes = XN_SXGA_X_RES;
869  output_mode.nYRes = XN_SXGA_Y_RES;
870  output_mode.nFPS = 15;
871  xn2config_map_[output_mode] = OpenNI_SXGA_15Hz;
872  config2xn_map_[OpenNI_SXGA_15Hz] = output_mode;
873 
874  output_mode.nXRes = XN_VGA_X_RES;
875  output_mode.nYRes = XN_VGA_Y_RES;
876  output_mode.nFPS = 25;
877  xn2config_map_[output_mode] = OpenNI_VGA_25Hz;
878  config2xn_map_[OpenNI_VGA_25Hz] = output_mode;
879  output_mode.nFPS = 30;
880  xn2config_map_[output_mode] = OpenNI_VGA_30Hz;
881  config2xn_map_[OpenNI_VGA_30Hz] = output_mode;
882 
883  output_mode.nXRes = XN_QVGA_X_RES;
884  output_mode.nYRes = XN_QVGA_Y_RES;
885  output_mode.nFPS = 25;
886  xn2config_map_[output_mode] = OpenNI_QVGA_25Hz;
887  config2xn_map_[OpenNI_QVGA_25Hz] = output_mode;
888  output_mode.nFPS = 30;
889  xn2config_map_[output_mode] = OpenNI_QVGA_30Hz;
890  config2xn_map_[OpenNI_QVGA_30Hz] = output_mode;
891  output_mode.nFPS = 60;
892  xn2config_map_[output_mode] = OpenNI_QVGA_60Hz;
893  config2xn_map_[OpenNI_QVGA_60Hz] = output_mode;
894 
895  output_mode.nXRes = XN_QQVGA_X_RES;
896  output_mode.nYRes = XN_QQVGA_Y_RES;
897  output_mode.nFPS = 25;
898  xn2config_map_[output_mode] = OpenNI_QQVGA_25Hz;
899  config2xn_map_[OpenNI_QQVGA_25Hz] = output_mode;
900  output_mode.nFPS = 30;
901  xn2config_map_[output_mode] = OpenNI_QQVGA_30Hz;
902  config2xn_map_[OpenNI_QQVGA_30Hz] = output_mode;
903  output_mode.nFPS = 60;
904  xn2config_map_[output_mode] = OpenNI_QQVGA_60Hz;
905  config2xn_map_[OpenNI_QQVGA_60Hz] = output_mode;
906 }
907 
908 int DriverNodelet::mapXnMode2ConfigMode (const XnMapOutputMode& output_mode) const
909 {
910  std::map<XnMapOutputMode, int, modeComp>::const_iterator it = xn2config_map_.find (output_mode);
911 
912  if (it == xn2config_map_.end ())
913  {
914  NODELET_ERROR ("mode %dx%d@%d could not be found", output_mode.nXRes, output_mode.nYRes, output_mode.nFPS);
915  exit (-1);
916  }
917  else
918  return it->second;
919 }
920 
921 XnMapOutputMode DriverNodelet::mapConfigMode2XnMode (int mode) const
922 {
923  std::map<int, XnMapOutputMode>::const_iterator it = config2xn_map_.find (mode);
924  if (it == config2xn_map_.end ())
925  {
926  NODELET_ERROR ("mode %d could not be found", mode);
927  exit (-1);
928  }
929  else
930  return it->second;
931 }
932 
933 void DriverNodelet::watchDog (const ros::TimerEvent& event)
934 {
936  if ( !time_stamp_.isZero() && (device_->isDepthStreamRunning() || device_->isImageStreamRunning()) )
937  {
938  ros::Duration duration = ros::Time::now() - time_stamp_;
939  if (duration.toSec() >= time_out_)
940  {
941  NODELET_ERROR("Timeout");
942  watch_dog_timer_.stop();
943  throw std::runtime_error("Timeout occured in DriverNodelet");
944  }
945  }
946 }
947 
948 }
949 
950 // Register as nodelet
unsigned char getBus(unsigned index) const
const char * getSerialNumber(unsigned index) const
const std::string BAYER_GRBG8
unsigned short getVendorID(unsigned index) const
virtual Encoding getEncoding() const =0
virtual const char * what() const
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
void fillRaw(unsigned width, unsigned height, unsigned short *ir_buffer, unsigned line_step=0) const
#define NODELET_ERROR(...)
ROSCONSOLE_DECL void notifyLoggerLevelsChanged()
#define NODELET_WARN(...)
const char * getProductName(unsigned index) const
const char * getVendorName(unsigned index) const
Driver class implemented as Singleton. This class contains the xn::Context object used by all devices...
Definition: openni_driver.h:58
dynamic_reconfigure::Server< Config > ReconfigureServer
Definition: driver.h:64
boost::shared_ptr< OpenNIDevice > getDeviceBySerialNumber(const std::string &serial_number) const
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
Image class containing just a reference to image meta data. Thus this class just provides an interfac...
Definition: openni_image.h:54
General exception class.
bool operator!=(const XnMapOutputMode &mode1, const XnMapOutputMode &mode2)
Definition: driver.cpp:59
Concrete implementation of the interface Image for a YUV 422 image used by Primesense devices...
unsigned getNumberDevices() const
unsigned short getProductID(unsigned index) const
unsigned getWidth() const
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
virtual void fillRGB(unsigned width, unsigned height, unsigned char *rgb_buffer, unsigned rgb_line_step=0) const
void fillDepthImageRaw(unsigned width, unsigned height, unsigned short *depth_buffer, unsigned line_step=0) const
virtual void fillRGB(unsigned width, unsigned height, unsigned char *rgb_buffer, unsigned rgb_line_step=0) const
#define ROS_WARN_ONCE(...)
boost::shared_ptr< OpenNIDevice > getDeviceByAddress(unsigned char bus, unsigned char address) const
bool param(const std::string &param_name, T &param_val, const T &default_val) const
unsigned getWidth() const
Definition: openni_image.h:105
This class provides methods to fill a RGB or Grayscale image buffer from underlying Bayer pattern ima...
#define ROS_ERROR_THROTTLE(period,...)
const std::string TYPE_16UC1
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
boost::shared_ptr< OpenNIDevice > getDeviceByIndex(unsigned index) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
const std::string MONO16
unsigned getHeight() const
Definition: openni_image.h:110
#define NODELET_INFO(...)
const std::string YUV422
bool getParam(const std::string &key, std::string &s) const
static Time now()
void fillRaw(unsigned char *rgb_buffer) const
Definition: openni_image.h:77
This class provides methods to fill a depth or disparity image.
const boost::shared_ptr< xn::ImageMetaData > getMetaDataPtr() const
Definition: openni_image.h:130
unsigned char getAddress(unsigned index) const
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
unsigned getHeight() const
Class containing just a reference to IR meta data.


openni_camera
Author(s): Patrick Mihelich, Suat Gedikli, Radu Bogdan Rusu
autogenerated on Wed Jun 5 2019 20:15:22