camera.cpp
Go to the documentation of this file.
1 
34 #include <multisense_ros/camera.h>
35 #include <multisense_ros/RawCamConfig.h>
36 #include <multisense_ros/RawCamCal.h>
37 #include <multisense_ros/DeviceInfo.h>
38 #include <multisense_ros/Histogram.h>
39 
40 #include <sensor_msgs/distortion_models.h>
41 #include <sensor_msgs/image_encodings.h>
42 
43 #include <multisense_lib/MultiSenseChannel.hh>
44 
45 #include <opencv2/opencv.hpp>
46 #include <arpa/inet.h>
47 #include <fstream>
48 #include <turbojpeg.h>
49 
50 using namespace crl::multisense;
51 
52 namespace multisense_ros {
53 
54 namespace { // anonymous
55 
56 //
57 // All of the data sources that we control here
58 
59 const DataSource allImageSources = (Source_Luma_Left |
60  Source_Luma_Right |
61  Source_Luma_Rectified_Left |
62  Source_Luma_Rectified_Right |
63  Source_Chroma_Left |
64  Source_Disparity |
65  Source_Disparity_Right |
66  Source_Disparity_Cost |
67  Source_Jpeg_Left);
68 
69 //
70 // Packed size of point cloud structures
71 
72 const uint32_t luma_cloud_step = 16;
73 const uint32_t color_cloud_step = 16;
74 
75 //
76 // Shims for C-style driver callbacks
77 
78 void monoCB(const image::Header& header, void* userDataP)
79 { reinterpret_cast<Camera*>(userDataP)->monoCallback(header); }
80 void rectCB(const image::Header& header, void* userDataP)
81 { reinterpret_cast<Camera*>(userDataP)->rectCallback(header); }
82 void depthCB(const image::Header& header, void* userDataP)
83 { reinterpret_cast<Camera*>(userDataP)->depthCallback(header); }
84 void pointCB(const image::Header& header, void* userDataP)
85 { reinterpret_cast<Camera*>(userDataP)->pointCloudCallback(header); }
86 void rawCB(const image::Header& header, void* userDataP)
87 { reinterpret_cast<Camera*>(userDataP)->rawCamDataCallback(header); }
88 void colorCB(const image::Header& header, void* userDataP)
89 { reinterpret_cast<Camera*>(userDataP)->colorImageCallback(header); }
90 void dispCB(const image::Header& header, void* userDataP)
91 { reinterpret_cast<Camera*>(userDataP)->disparityImageCallback(header); }
92 void jpegCB(const image::Header& header, void* userDataP)
93 { reinterpret_cast<Camera*>(userDataP)->jpegImageCallback(header); }
94 void histCB(const image::Header& header, void* userDataP)
95 { reinterpret_cast<Camera*>(userDataP)->histogramCallback(header); }
96 
97 //
98 // Check for valid range points coming out of OpenCV
99 
100 bool isValidPoint(const cv::Vec3f& pt,
101  const float& maxRange)
102 {
103  //
104  // Check both for disparities explicitly marked as invalid (where
105  // OpenCV maps pt.z to MISSING_Z) and zero disparities (point
106  // mapped to infinity).
107 
108  if (image_geometry::StereoCameraModel::MISSING_Z != pt[2] && std::isfinite(pt[2])) {
109 
110  //
111  // Also filter on reasonable ranges
112 
113  const float mag = std::sqrt((pt[0]*pt[0])+(pt[1]*pt[1])+(pt[2]*pt[2]));
114 
115  if (mag < maxRange)
116  return true;
117  }
118 
119  return false;
120 }
121 
122 //
123 // Publish a point cloud, using the given storage, filtering the points,
124 // and colorizing the cloud with all available color channels.
125 //
126 // Note that the dependencies for the point cloud will be generated in
127 // different threads. This function is called each time a dependency
128 // becomes ready.
129 //
130 // The published frame ID for this point cloud type is tracked and
131 // publishing is serialized here via a mutex to prevent race conditions.
132 
133 boost::mutex point_cloud_mutex;
134 
135 bool publishPointCloud(int64_t imageFrameId,
136  int64_t pointsFrameId,
137  int64_t& cloudFrameId,
138  ros::Publisher& pub,
139  sensor_msgs::PointCloud2& cloud,
140  const uint32_t width,
141  const uint32_t height,
142  const uint32_t timeSeconds,
143  const uint32_t timeMicroSeconds,
144  const uint32_t cloudStep,
145  const std::vector<cv::Vec3f>& points,
146  const uint8_t* imageP,
147  const uint32_t colorChannels,
148  const float maxRange,
149  bool writeColorPacked,
150  bool organized)
151 {
152  boost::mutex::scoped_lock lock(point_cloud_mutex);
153 
154  if (0 == pub.getNumSubscribers() ||
155  imageFrameId != pointsFrameId ||
156  cloudFrameId >= imageFrameId) {
157  return false;
158  }
159 
160  cloudFrameId = imageFrameId;
161  const uint32_t imageSize = height * width;
162 
163  if (points.size() != imageSize)
164  return false;
165 
166  cloud.data.resize(imageSize * cloudStep);
167 
168  uint8_t *cloudP = reinterpret_cast<uint8_t*>(&cloud.data[0]);
169  const uint32_t pointSize = 3 * sizeof(float); // x, y, z
170  uint32_t validPoints = 0;
171 
172  cv::Vec3f nanPoint(std::numeric_limits<float>::quiet_NaN(),
173  std::numeric_limits<float>::quiet_NaN(),
174  std::numeric_limits<float>::quiet_NaN());
175 
176 
177  for(uint32_t i=0; i<height; ++i)
178  for(uint32_t j=0; j<width; ++j) {
179 
180  const uint32_t index = i * width + j;
181 
182  const uint32_t* pointP = reinterpret_cast<const uint32_t*>(&points[index]);
183  uint32_t* targetCloudP = reinterpret_cast<uint32_t*>(cloudP);
184 
185 
186  //
187  // When creating an organized pointcloud replace invalid points
188  // with NaN points
189 
190  if (false == isValidPoint(points[index], maxRange))
191  {
192  if (organized)
193  {
194  pointP = reinterpret_cast<const uint32_t*>(&nanPoint[0]);
195  }
196  else
197  {
198  continue;
199  }
200  }
201 
202 
203  //
204  // Directly copy points to eliminate memcpy
205 
206  targetCloudP[0] = pointP[0];
207  targetCloudP[1] = pointP[1];
208  targetCloudP[2] = pointP[2];
209 
210 
211  const uint8_t *sourceColorP = &(imageP[colorChannels * index]);
212  uint8_t *cloudColorP = (cloudP + pointSize);
213 
214  //
215  // Write the poincloud packed if specified or the color image
216  // is BGR. Copying is optimized to eliminate memcpy operations
217  // increasing overall speed
218 
219  if (writeColorPacked || colorChannels > 2)
220  {
221  switch(colorChannels)
222  {
223  case 4:
224  cloudColorP[3] = sourceColorP[3];
225  case 3:
226  cloudColorP[2] = sourceColorP[2];
227  case 2:
228  cloudColorP[1] = sourceColorP[1];
229  case 1:
230  cloudColorP[0] = sourceColorP[0];
231  }
232 
233  } else {
234  union
235  {
236  uint32_t value;
237  char bytes[sizeof(uint32_t)];
238  } color;
239 
240  color.value = 0;
241 
242  //
243  // We only need to copy 2 values since this case only
244  // applies for images with color channels of 1 or 2 bytes
245 
246  color.bytes[0] = sourceColorP[0];
247  color.bytes[1] = sourceColorP[1] & ((colorChannels > 1) * 255);
248 
249  float* floatCloudColorP = reinterpret_cast<float*>(cloudColorP);
250  floatCloudColorP[0] = static_cast<float>(color.value);
251  }
252 
253  cloudP += cloudStep;
254  validPoints ++;
255  }
256 
257  if (!organized) {
258  cloud.row_step = validPoints * cloudStep;
259  cloud.width = validPoints;
260  cloud.height = 1;
261  } else {
262  cloud.width = width;
263  cloud.height = height;
264  cloud.row_step = width * cloudStep;
265  }
266 
267  cloud.header.stamp = ros::Time(timeSeconds, 1000 * timeMicroSeconds);
268  cloud.data.resize(validPoints * cloudStep);
269  pub.publish(cloud);
270 
271  return true;
272 }
273 
274 bool savePgm(const std::string& fileName,
275  uint32_t width,
276  uint32_t height,
277  uint32_t bitsPerPixel,
278  const void *dataP)
279 {
280  std::ofstream outputStream(fileName.c_str(), std::ios::binary | std::ios::out);
281 
282  if (false == outputStream.good()) {
283  fprintf(stderr, "failed to open \"%s\"\n", fileName.c_str());
284  return false;
285  }
286 
287  const uint32_t imageSize = height * width;
288 
289  switch(bitsPerPixel) {
290  case 8:
291  {
292 
293  outputStream << "P5\n"
294  << width << " " << height << "\n"
295  << 0xFF << "\n";
296 
297  outputStream.write(reinterpret_cast<const char*>(dataP), imageSize);
298 
299  break;
300  }
301  case 16:
302  {
303  outputStream << "P5\n"
304  << width << " " << height << "\n"
305  << 0xFFFF << "\n";
306 
307  const uint16_t *imageP = reinterpret_cast<const uint16_t*>(dataP);
308 
309  for (uint32_t i=0; i<imageSize; ++i) {
310  uint16_t o = htons(imageP[i]);
311  outputStream.write(reinterpret_cast<const char*>(&o), sizeof(uint16_t));
312  }
313 
314  break;
315  }
316  }
317 
318  outputStream.close();
319  return true;
320 }
321 
322 }; // anonymous
323 
324 Camera::Camera(Channel* driver,
325  const std::string& tf_prefix) :
326  driver_(driver),
327  device_nh_(""),
328  left_nh_(device_nh_, "left"),
329  right_nh_(device_nh_, "right"),
330  left_mono_transport_(left_nh_),
331  right_mono_transport_(right_nh_),
332  left_rect_transport_(left_nh_),
333  right_rect_transport_(right_nh_),
334  left_rgb_transport_(left_nh_),
335  left_rgb_rect_transport_(left_nh_),
336  depth_transport_(device_nh_),
337  ni_depth_transport_(device_nh_),
338  disparity_left_transport_(left_nh_),
339  disparity_right_transport_(right_nh_),
340  disparity_cost_transport_(left_nh_),
341  left_mono_cam_info_(),
342  right_mono_cam_info_(),
343  left_rect_cam_info_(),
344  right_rect_cam_info_(),
345  left_rgb_rect_cam_info_(),
346  left_disp_cam_info_(),
347  right_disp_cam_info_(),
348  left_cost_cam_info_(),
349  left_rgb_cam_info_(),
350  depth_cam_info_(),
351  left_mono_cam_pub_(),
352  right_mono_cam_pub_(),
353  left_rect_cam_pub_(),
354  right_rect_cam_pub_(),
355  depth_cam_pub_(),
356  ni_depth_cam_pub_(),
357  left_rgb_cam_pub_(),
358  left_rgb_rect_cam_pub_(),
359  left_mono_cam_info_pub_(),
360  right_mono_cam_info_pub_(),
361  left_rect_cam_info_pub_(),
362  right_rect_cam_info_pub_(),
363  left_disp_cam_info_pub_(),
364  right_disp_cam_info_pub_(),
365  left_cost_cam_info_pub_(),
366  left_rgb_cam_info_pub_(),
367  left_rgb_rect_cam_info_pub_(),
368  depth_cam_info_pub_(),
369  luma_point_cloud_pub_(),
370  color_point_cloud_pub_(),
371  luma_organized_point_cloud_pub_(),
372  color_organized_point_cloud_pub_(),
373  left_disparity_pub_(),
374  right_disparity_pub_(),
375  left_disparity_cost_pub_(),
376  left_stereo_disparity_pub_(),
377  right_stereo_disparity_pub_(),
378  raw_cam_data_pub_(),
379  raw_cam_config_pub_(),
380  raw_cam_cal_pub_(),
381  device_info_pub_(),
382  histogram_pub_(),
383  left_mono_image_(),
384  right_mono_image_(),
385  left_rect_image_(),
386  right_rect_image_(),
387  depth_image_(),
388  luma_point_cloud_(),
389  color_point_cloud_(),
390  luma_organized_point_cloud_(),
391  color_organized_point_cloud_(),
392  left_luma_image_(),
393  left_rgb_image_(),
394  left_rgb_rect_image_(),
395  left_disparity_image_(),
396  left_disparity_cost_image_(),
397  right_disparity_image_(),
398  left_stereo_disparity_(),
399  right_stereo_disparity_(),
400  got_raw_cam_left_(false),
401  got_left_luma_(false),
402  left_luma_frame_id_(0),
403  left_rect_frame_id_(0),
404  left_rgb_rect_frame_id_(-1),
405  luma_point_cloud_frame_id_(-1),
406  luma_organized_point_cloud_frame_id_(-1),
407  color_point_cloud_frame_id_(-1),
408  color_organized_point_cloud_frame_id_(-1),
409  raw_cam_data_(),
410  version_info_(),
411  device_info_(),
412  image_config_(),
413  image_calibration_(),
414  cal_lock_(),
415  calibration_map_left_1_(NULL),
416  calibration_map_left_2_(NULL),
417  frame_id_left_(),
418  frame_id_right_(),
419  disparity_buff_(),
420  points_buff_(),
421  points_buff_frame_id_(-1),
422  q_matrix_(4, 4, 0.0),
423  pc_max_range_(15.0f),
424  pc_color_frame_sync_(true),
425  disparities_(0),
426  stream_lock_(),
427  stream_map_(),
428  last_frame_id_(-1),
429  luma_color_depth_(1),
430  write_pc_color_packed_(false),
431  border_clip_type_(0),
432  border_clip_value_(0.0),
433  border_clip_lock_()
434 {
435  //
436  // Query device and version information from sensor
437 
438  Status status = driver_->getVersionInfo(version_info_);
439  if (Status_Ok != status) {
440  ROS_ERROR("Camera: failed to query version info: %s",
441  Channel::statusString(status));
442  return;
443  }
444  status = driver_->getDeviceInfo(device_info_);
445  if (Status_Ok != status) {
446  ROS_ERROR("Camera: failed to query device info: %s",
447  Channel::statusString(status));
448  return;
449  }
450 
451  //
452  // Set frame ID
453 
454  frame_id_left_ = tf_prefix + "/left_camera_optical_frame";
455  frame_id_right_ = tf_prefix + "/right_camera_optical_frame";
456  ROS_INFO("camera frame id: %s", frame_id_left_.c_str());
457 
458  //
459  // Topics published for all device types
460 
461  ros::NodeHandle calibration_nh(device_nh_, "calibration");
462  device_info_pub_ = calibration_nh.advertise<multisense_ros::DeviceInfo>("device_info", 1, true);
463  raw_cam_cal_pub_ = calibration_nh.advertise<multisense_ros::RawCamCal>("raw_cam_cal", 1, true);
464  raw_cam_config_pub_ = calibration_nh.advertise<multisense_ros::RawCamConfig>("raw_cam_config", 1, true);
465  histogram_pub_ = device_nh_.advertise<multisense_ros::Histogram>("histogram", 5);
466 
467  //
468  // Change the way the luma pointcloud is published for ST21 sensors
469 
470  if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_ST21 == device_info_.hardwareRevision) {
471 
472  //
473  // Luma images are 16 bit so when copying to the point cloud
474  // structure copy 2 bytes
475 
476  luma_color_depth_ = 2;
477  }
478 
479  //
480  // Create topic publishers (TODO: color topics should not be advertised if the device can't support it)
481 
482  if (system::DeviceInfo::HARDWARE_REV_BCAM == device_info_.hardwareRevision) {
483 
484  left_mono_cam_pub_ = left_mono_transport_.advertise("image_mono", 5,
485  boost::bind(&Camera::connectStream, this, Source_Luma_Left),
486  boost::bind(&Camera::disconnectStream, this, Source_Luma_Left));
487 
488  left_rgb_cam_pub_ = left_rgb_transport_.advertise("image_color", 5,
489  boost::bind(&Camera::connectStream, this, Source_Jpeg_Left),
490  boost::bind(&Camera::disconnectStream, this, Source_Jpeg_Left));
491 
492  left_rgb_rect_cam_pub_ = left_rgb_rect_transport_.advertiseCamera("image_rect_color", 5,
493  boost::bind(&Camera::connectStream, this, Source_Jpeg_Left),
494  boost::bind(&Camera::disconnectStream, this, Source_Jpeg_Left));
495 
496  left_mono_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>("image_mono/camera_info", 1, true);
497  left_rgb_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>("image_color/camera_info", 1, true);
498  left_rgb_rect_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>("image_rect_color/camera_info", 1, true);
499 
500 
501  } else if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_M == device_info_.hardwareRevision) {
502 
503  // monocular variation
504 
505  left_mono_cam_pub_ = left_mono_transport_.advertise("image_mono", 5,
506  boost::bind(&Camera::connectStream, this, Source_Luma_Left),
507  boost::bind(&Camera::disconnectStream, this, Source_Luma_Left));
508  left_rect_cam_pub_ = left_rect_transport_.advertiseCamera("image_rect", 5,
509  boost::bind(&Camera::connectStream, this, Source_Luma_Rectified_Left),
510  boost::bind(&Camera::disconnectStream, this, Source_Luma_Rectified_Left));
511  left_rgb_cam_pub_ = left_rgb_transport_.advertise("image_color", 5,
512  boost::bind(&Camera::connectStream, this, Source_Luma_Left | Source_Chroma_Left),
513  boost::bind(&Camera::disconnectStream, this, Source_Luma_Left | Source_Chroma_Left));
514  left_rgb_rect_cam_pub_ = left_rgb_rect_transport_.advertiseCamera("image_rect_color", 5,
515  boost::bind(&Camera::connectStream, this, Source_Luma_Left | Source_Chroma_Left),
516  boost::bind(&Camera::disconnectStream, this, Source_Luma_Left | Source_Chroma_Left));
517 
518  left_mono_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>("image_mono/camera_info", 1, true);
519  left_rect_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>("image_rect/camera_info", 1, true);
520  left_rgb_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>("image_color/camera_info", 1, true);
521  left_rgb_rect_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>("image_rect_color/camera_info", 1, true);
522 
523  } else { // all other MultiSense-S* variations
524 
525 
526  left_mono_cam_pub_ = left_mono_transport_.advertise("image_mono", 5,
527  boost::bind(&Camera::connectStream, this, Source_Luma_Left),
528  boost::bind(&Camera::disconnectStream, this, Source_Luma_Left));
529  right_mono_cam_pub_ = right_mono_transport_.advertise("image_mono", 5,
530  boost::bind(&Camera::connectStream, this, Source_Luma_Right),
531  boost::bind(&Camera::disconnectStream, this, Source_Luma_Right));
532  left_rect_cam_pub_ = left_rect_transport_.advertiseCamera("image_rect", 5,
533  boost::bind(&Camera::connectStream, this, Source_Luma_Rectified_Left),
534  boost::bind(&Camera::disconnectStream, this, Source_Luma_Rectified_Left));
535  right_rect_cam_pub_ = right_rect_transport_.advertiseCamera("image_rect", 5,
536  boost::bind(&Camera::connectStream, this, Source_Luma_Rectified_Right),
537  boost::bind(&Camera::disconnectStream, this, Source_Luma_Rectified_Right));
538  depth_cam_pub_ = depth_transport_.advertise("depth", 5,
539  boost::bind(&Camera::connectStream, this, Source_Disparity),
540  boost::bind(&Camera::disconnectStream, this, Source_Disparity));
541  ni_depth_cam_pub_ = ni_depth_transport_.advertise("openni_depth", 5,
542  boost::bind(&Camera::connectStream, this, Source_Disparity),
543  boost::bind(&Camera::disconnectStream, this, Source_Disparity));
544 
545  if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_ST21 != device_info_.hardwareRevision) {
546 
547  left_rgb_cam_pub_ = left_rgb_transport_.advertise("image_color", 5,
548  boost::bind(&Camera::connectStream, this, Source_Luma_Left | Source_Chroma_Left),
549  boost::bind(&Camera::disconnectStream, this, Source_Luma_Left | Source_Chroma_Left));
550  left_rgb_rect_cam_pub_ = left_rgb_rect_transport_.advertiseCamera("image_rect_color", 5,
551  boost::bind(&Camera::connectStream, this, Source_Luma_Left | Source_Chroma_Left),
552  boost::bind(&Camera::disconnectStream, this, Source_Luma_Left | Source_Chroma_Left));
553  color_point_cloud_pub_ = device_nh_.advertise<sensor_msgs::PointCloud2>("image_points2_color", 5,
554  boost::bind(&Camera::connectStream, this,
555  Source_Disparity | Source_Luma_Left | Source_Chroma_Left),
556  boost::bind(&Camera::disconnectStream, this,
557  Source_Disparity | Source_Luma_Left | Source_Chroma_Left));
558  color_organized_point_cloud_pub_ = device_nh_.advertise<sensor_msgs::PointCloud2>("organized_image_points2_color", 5,
559  boost::bind(&Camera::connectStream, this,
560  Source_Disparity | Source_Luma_Left | Source_Chroma_Left),
561  boost::bind(&Camera::disconnectStream, this,
562  Source_Disparity | Source_Luma_Left | Source_Chroma_Left));
563 
564  left_rgb_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>("image_color/camera_info", 1, true);
565  left_rgb_rect_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>("image_rect_color/camera_info", 1, true);
566 
567  }
568 
569  luma_point_cloud_pub_ = device_nh_.advertise<sensor_msgs::PointCloud2>("image_points2", 5,
570  boost::bind(&Camera::connectStream, this, Source_Luma_Rectified_Left | Source_Disparity),
571  boost::bind(&Camera::disconnectStream, this, Source_Luma_Rectified_Left | Source_Disparity));
572 
573  luma_organized_point_cloud_pub_ = device_nh_.advertise<sensor_msgs::PointCloud2>("organized_image_points2", 5,
574  boost::bind(&Camera::connectStream, this, Source_Luma_Rectified_Left | Source_Disparity),
575  boost::bind(&Camera::disconnectStream, this, Source_Luma_Rectified_Left | Source_Disparity));
576 
577  raw_cam_data_pub_ = calibration_nh.advertise<multisense_ros::RawCamData>("raw_cam_data", 5,
578  boost::bind(&Camera::connectStream, this, Source_Luma_Rectified_Left | Source_Disparity),
579  boost::bind(&Camera::disconnectStream, this, Source_Luma_Rectified_Left | Source_Disparity));
580 
581  left_disparity_pub_ = disparity_left_transport_.advertise("disparity", 5,
582  boost::bind(&Camera::connectStream, this, Source_Disparity),
583  boost::bind(&Camera::disconnectStream, this, Source_Disparity));
584 
585  left_stereo_disparity_pub_ = left_nh_.advertise<stereo_msgs::DisparityImage>("disparity_image", 5,
586  boost::bind(&Camera::connectStream, this, Source_Disparity),
587  boost::bind(&Camera::disconnectStream, this, Source_Disparity));
588 
589  if (version_info_.sensorFirmwareVersion >= 0x0300) {
590 
591  right_disparity_pub_ = disparity_right_transport_.advertise("disparity", 5,
592  boost::bind(&Camera::connectStream, this, Source_Disparity_Right),
593  boost::bind(&Camera::disconnectStream, this, Source_Disparity_Right));
594 
595  right_stereo_disparity_pub_ = right_nh_.advertise<stereo_msgs::DisparityImage>("disparity_image", 5,
596  boost::bind(&Camera::connectStream, this, Source_Disparity_Right),
597  boost::bind(&Camera::disconnectStream, this, Source_Disparity_Right));
598 
599  left_disparity_cost_pub_ = disparity_cost_transport_.advertise("cost", 5,
600  boost::bind(&Camera::connectStream, this, Source_Disparity_Cost),
601  boost::bind(&Camera::disconnectStream, this, Source_Disparity_Cost));
602 
603  right_disp_cam_info_pub_ = right_nh_.advertise<sensor_msgs::CameraInfo>("disparity/camera_info", 1, true);
604  left_cost_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>("cost/camera_info", 1, true);
605  }
606 
607  //
608  // Camera info topic publishers
609  left_mono_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>("image_mono/camera_info", 1, true);
610  right_mono_cam_info_pub_ = right_nh_.advertise<sensor_msgs::CameraInfo>("image_mono/camera_info", 1, true);
611  left_rect_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>("image_rect/camera_info", 1, true);
612  right_rect_cam_info_pub_ = right_nh_.advertise<sensor_msgs::CameraInfo>("image_rect/camera_info", 1, true);
613  left_disp_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>("disparity/camera_info", 1, true);
614  depth_cam_info_pub_ = device_nh_.advertise<sensor_msgs::CameraInfo>("depth/camera_info", 1, true);
615  }
616 
617 
618  //
619  // All image streams off
620 
621  stop();
622 
623  //
624  // Publish device info
625 
626  multisense_ros::DeviceInfo msg;
627 
628  msg.deviceName = device_info_.name;
629  msg.buildDate = device_info_.buildDate;
630  msg.serialNumber = device_info_.serialNumber;
631  msg.deviceRevision = device_info_.hardwareRevision;
632 
633  msg.numberOfPcbs = device_info_.pcbs.size();
634  std::vector<system::PcbInfo>::const_iterator it = device_info_.pcbs.begin();
635  for(; it != device_info_.pcbs.end(); ++it) {
636  msg.pcbSerialNumbers.push_back((*it).revision);
637  msg.pcbNames.push_back((*it).name);
638  }
639 
640  msg.imagerName = device_info_.imagerName;
641  msg.imagerType = device_info_.imagerType;
642  msg.imagerWidth = device_info_.imagerWidth;
643  msg.imagerHeight = device_info_.imagerHeight;
644 
645  msg.lensName = device_info_.lensName;
646  msg.lensType = device_info_.lensType;
647  msg.nominalBaseline = device_info_.nominalBaseline;
648  msg.nominalFocalLength = device_info_.nominalFocalLength;
649  msg.nominalRelativeAperture = device_info_.nominalRelativeAperture;
650 
651  msg.lightingType = device_info_.lightingType;
652  msg.numberOfLights = device_info_.numberOfLights;
653 
654  msg.laserName = device_info_.laserName;
655  msg.laserType = device_info_.laserType;
656 
657  msg.motorName = device_info_.motorName;
658  msg.motorType = device_info_.motorType;
659  msg.motorGearReduction = device_info_.motorGearReduction;
660 
661  msg.apiBuildDate = version_info_.apiBuildDate;
662  msg.apiVersion = version_info_.apiVersion;
663  msg.firmwareBuildDate = version_info_.sensorFirmwareBuildDate;
664  msg.firmwareVersion = version_info_.sensorFirmwareVersion;
665  msg.bitstreamVersion = version_info_.sensorHardwareVersion;
666  msg.bitstreamMagic = version_info_.sensorHardwareMagic;
667  msg.fpgaDna = version_info_.sensorFpgaDna;
668 
669  device_info_pub_.publish(msg);
670 
671  //
672  // Publish image calibration
673 
674  status = driver_->getImageCalibration(image_calibration_);
675  if (Status_Ok != status)
676  ROS_ERROR("Camera: failed to query image calibration: %s",
677  Channel::statusString(status));
678  else {
679 
680  multisense_ros::RawCamCal cal;
681  const float *cP;
682 
683  cP = reinterpret_cast<const float *>(&(image_calibration_.left.M[0][0]));
684  for(uint32_t i=0; i<9; i++) cal.left_M[i] = cP[i];
685  cP = reinterpret_cast<const float *>(&(image_calibration_.left.D[0]));
686  for(uint32_t i=0; i<8; i++) cal.left_D[i] = cP[i];
687  cP = reinterpret_cast<const float *>(&(image_calibration_.left.R[0][0]));
688  for(uint32_t i=0; i<9; i++) cal.left_R[i] = cP[i];
689  cP = reinterpret_cast<const float *>(&(image_calibration_.left.P[0][0]));
690  for(uint32_t i=0; i<12; i++) cal.left_P[i] = cP[i];
691 
692  cP = reinterpret_cast<const float *>(&(image_calibration_.right.M[0][0]));
693  for(uint32_t i=0; i<9; i++) cal.right_M[i] = cP[i];
694  cP = reinterpret_cast<const float *>(&(image_calibration_.right.D[0]));
695  for(uint32_t i=0; i<8; i++) cal.right_D[i] = cP[i];
696  cP = reinterpret_cast<const float *>(&(image_calibration_.right.R[0][0]));
697  for(uint32_t i=0; i<9; i++) cal.right_R[i] = cP[i];
698  cP = reinterpret_cast<const float *>(&(image_calibration_.right.P[0][0]));
699  for(uint32_t i=0; i<12; i++) cal.right_P[i] = cP[i];
700 
701  raw_cam_cal_pub_.publish(cal);
702  }
703 
704  //
705  // Get current sensor configuration
706 
707  q_matrix_(0,0) = q_matrix_(1,1) = 1.0;
708  queryConfig();
709 
710  //
711  // Initialze point cloud data structures
712 
713  luma_point_cloud_.is_bigendian = (htonl(1) == 1);
714  luma_point_cloud_.is_dense = true;
715  luma_point_cloud_.point_step = luma_cloud_step;
716  luma_point_cloud_.height = 1;
717  luma_point_cloud_.header.frame_id = frame_id_left_;
718  luma_point_cloud_.fields.resize(4);
719  luma_point_cloud_.fields[0].name = "x";
720  luma_point_cloud_.fields[0].offset = 0;
721  luma_point_cloud_.fields[0].count = 1;
722  luma_point_cloud_.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
723  luma_point_cloud_.fields[1].name = "y";
724  luma_point_cloud_.fields[1].offset = 4;
725  luma_point_cloud_.fields[1].count = 1;
726  luma_point_cloud_.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
727  luma_point_cloud_.fields[2].name = "z";
728  luma_point_cloud_.fields[2].offset = 8;
729  luma_point_cloud_.fields[2].count = 1;
730  luma_point_cloud_.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
731  luma_point_cloud_.fields[3].name = "luminance";
732  luma_point_cloud_.fields[3].offset = 12;
733  luma_point_cloud_.fields[3].count = 1;
734  luma_point_cloud_.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
735 
736  color_point_cloud_.is_bigendian = (htonl(1) == 1);
737  color_point_cloud_.is_dense = true;
738  color_point_cloud_.point_step = color_cloud_step;
739  color_point_cloud_.height = 1;
740  color_point_cloud_.header.frame_id = frame_id_left_;
741  color_point_cloud_.fields.resize(4);
742  color_point_cloud_.fields[0].name = "x";
743  color_point_cloud_.fields[0].offset = 0;
744  color_point_cloud_.fields[0].count = 1;
745  color_point_cloud_.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
746  color_point_cloud_.fields[1].name = "y";
747  color_point_cloud_.fields[1].offset = 4;
748  color_point_cloud_.fields[1].count = 1;
749  color_point_cloud_.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
750  color_point_cloud_.fields[2].name = "z";
751  color_point_cloud_.fields[2].offset = 8;
752  color_point_cloud_.fields[2].count = 1;
753  color_point_cloud_.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
754  color_point_cloud_.fields[3].name = "rgb";
755  color_point_cloud_.fields[3].offset = 12;
756  color_point_cloud_.fields[3].count = 1;
757  color_point_cloud_.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
758 
759  luma_organized_point_cloud_.is_bigendian = (htonl(1) == 1);
760  luma_organized_point_cloud_.is_dense = false;
761  luma_organized_point_cloud_.point_step = luma_cloud_step;
762  luma_organized_point_cloud_.header.frame_id = frame_id_left_;
763  luma_organized_point_cloud_.fields.resize(4);
764  luma_organized_point_cloud_.fields[0].name = "x";
765  luma_organized_point_cloud_.fields[0].offset = 0;
766  luma_organized_point_cloud_.fields[0].count = 1;
767  luma_organized_point_cloud_.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
768  luma_organized_point_cloud_.fields[1].name = "y";
769  luma_organized_point_cloud_.fields[1].offset = 4;
770  luma_organized_point_cloud_.fields[1].count = 1;
771  luma_organized_point_cloud_.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
772  luma_organized_point_cloud_.fields[2].name = "z";
773  luma_organized_point_cloud_.fields[2].offset = 8;
774  luma_organized_point_cloud_.fields[2].count = 1;
775  luma_organized_point_cloud_.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
776  luma_organized_point_cloud_.fields[3].name = "luminance";
777  luma_organized_point_cloud_.fields[3].offset = 12;
778  luma_organized_point_cloud_.fields[3].count = 1;
779  luma_organized_point_cloud_.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
780 
781  color_organized_point_cloud_.is_bigendian = (htonl(1) == 1);
782  color_organized_point_cloud_.is_dense = false;
783  color_organized_point_cloud_.point_step = color_cloud_step;
784  color_organized_point_cloud_.header.frame_id = frame_id_left_;
785  color_organized_point_cloud_.fields.resize(4);
786  color_organized_point_cloud_.fields[0].name = "x";
787  color_organized_point_cloud_.fields[0].offset = 0;
788  color_organized_point_cloud_.fields[0].count = 1;
789  color_organized_point_cloud_.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
790  color_organized_point_cloud_.fields[1].name = "y";
791  color_organized_point_cloud_.fields[1].offset = 4;
792  color_organized_point_cloud_.fields[1].count = 1;
793  color_organized_point_cloud_.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
794  color_organized_point_cloud_.fields[2].name = "z";
795  color_organized_point_cloud_.fields[2].offset = 8;
796  color_organized_point_cloud_.fields[2].count = 1;
797  color_organized_point_cloud_.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
798  color_organized_point_cloud_.fields[3].name = "rgb";
799  color_organized_point_cloud_.fields[3].offset = 12;
800  color_organized_point_cloud_.fields[3].count = 1;
801  color_organized_point_cloud_.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
802 
803  //
804  // Add driver-level callbacks.
805  //
806  // -Driver creates individual background thread for each callback.
807  // -Images are queued (depth=5) per callback, with oldest silently dropped if not keeping up.
808  // -All images presented are backed by a referenced buffer system (no copying of image data is done.)
809 
810  if (system::DeviceInfo::HARDWARE_REV_BCAM == device_info_.hardwareRevision) {
811 
812  driver_->addIsolatedCallback(monoCB, Source_Luma_Left, this);
813  driver_->addIsolatedCallback(jpegCB, Source_Jpeg_Left, this);
814 
815  } else {
816 
817  driver_->addIsolatedCallback(monoCB, Source_Luma_Left | Source_Luma_Right, this);
818  driver_->addIsolatedCallback(rectCB, Source_Luma_Rectified_Left | Source_Luma_Rectified_Right, this);
819  driver_->addIsolatedCallback(depthCB, Source_Disparity, this);
820  driver_->addIsolatedCallback(pointCB, Source_Disparity, this);
821  driver_->addIsolatedCallback(rawCB, Source_Disparity | Source_Luma_Rectified_Left, this);
822  driver_->addIsolatedCallback(colorCB, Source_Luma_Left | Source_Chroma_Left, this);
823  driver_->addIsolatedCallback(dispCB, Source_Disparity | Source_Disparity_Right | Source_Disparity_Cost, this);
824  }
825 
826  //
827  // A common callback to publish histograms
828 
829  driver_->addIsolatedCallback(histCB, allImageSources, this);
830 
831 
832  //
833  // Disable color point cloud strict frame syncing, if desired
834 
835  const char *pcColorFrameSyncEnvStringP = getenv("MULTISENSE_ROS_PC_COLOR_FRAME_SYNC_OFF");
836  if (NULL != pcColorFrameSyncEnvStringP) {
837  pc_color_frame_sync_ = false;
838  ROS_INFO("color point cloud frame sync is disabled");
839  }
840 }
841 
842 Camera::~Camera()
843 {
844  stop();
845 
846  if (system::DeviceInfo::HARDWARE_REV_BCAM == device_info_.hardwareRevision) {
847 
848  driver_->removeIsolatedCallback(monoCB);
849  driver_->removeIsolatedCallback(jpegCB);
850 
851  } else {
852 
853  driver_->removeIsolatedCallback(monoCB);
854  driver_->removeIsolatedCallback(rectCB);
855  driver_->removeIsolatedCallback(depthCB);
856  driver_->removeIsolatedCallback(pointCB);
857  driver_->removeIsolatedCallback(rawCB);
858  driver_->removeIsolatedCallback(colorCB);
859  driver_->removeIsolatedCallback(dispCB);
860  }
861 }
862 
863 void Camera::histogramCallback(const image::Header& header)
864 {
865  if (last_frame_id_ >= header.frameId)
866  return;
867 
868  last_frame_id_ = header.frameId;
869 
870  if (histogram_pub_.getNumSubscribers() > 0) {
871  multisense_ros::Histogram rh;
872  image::Histogram mh;
873 
874  Status status = driver_->getImageHistogram(header.frameId, mh);
875  if (Status_Ok == status) {
876  rh.frame_count = header.frameId;
877  rh.time_stamp = ros::Time(header.timeSeconds,
878  1000 * header.timeMicroSeconds);
879  rh.width = header.width;
880  rh.height = header.height;
881  switch(header.source) {
882  case Source_Chroma_Left:
883  case Source_Chroma_Right:
884  rh.width *= 2;
885  rh.height *= 2;
886  }
887 
888  rh.exposure_time = header.exposure;
889  rh.gain = header.gain;
890  rh.fps = header.framesPerSecond;
891  rh.channels = mh.channels;
892  rh.bins = mh.bins;
893  rh.data = mh.data;
894  histogram_pub_.publish(rh);
895  }
896  }
897 }
898 
899 void Camera::jpegImageCallback(const image::Header& header)
900 {
901  if (Source_Jpeg_Left != header.source)
902  return;
903 
904  const uint32_t height = header.height;
905  const uint32_t width = header.width;
906  const uint32_t rgbLength = height * width * 3;
907 
908  left_rgb_image_.header.frame_id = frame_id_left_;
909  left_rgb_image_.height = height;
910  left_rgb_image_.width = width;
911  left_rgb_image_.encoding = "rgb8";
912  left_rgb_image_.is_bigendian = false;
913  left_rgb_image_.step = 3 * width;
914  left_rgb_image_.header.stamp = ros::Time(header.timeSeconds,
915  1000 * header.timeMicroSeconds);
916 
917  left_rgb_image_.data.resize(rgbLength);
918 
919  tjhandle jpegDecompressor = tjInitDecompress();
920  tjDecompress2(jpegDecompressor,
921  reinterpret_cast<unsigned char*>(const_cast<void*>(header.imageDataP)),
922  header.imageLength,
923  &(left_rgb_image_.data[0]),
924  width, 0/*pitch*/, height, TJPF_RGB, 0);
925  tjDestroy(jpegDecompressor);
926 
927  left_rgb_cam_pub_.publish(left_rgb_image_);
928  left_rgb_cam_info_.header = left_rgb_image_.header;
929  left_rgb_cam_info_pub_.publish(left_rgb_cam_info_);
930 
931  if (left_rgb_rect_cam_pub_.getNumSubscribers() > 0) {
932  boost::mutex::scoped_lock lock(cal_lock_);
933 
934  if (width != image_config_.width() ||
935  height != image_config_.height()){
936  //ROS_ERROR("calibration/image size mismatch: image=%dx%d, calibration=%dx%d",
937  //width, height, image_config_.width(), image_config_.height());
938  cal_lock_.unlock();
939  queryConfig();
940  }
941  else if (NULL == calibration_map_left_1_ || NULL == calibration_map_left_2_)
942  ROS_ERROR("Camera: undistort maps not initialized");
943  else {
944 
945  const CvScalar outlierColor = cv::Scalar_<double>(0.0);
946 
947  left_rgb_rect_image_.data.resize(rgbLength);
948 
949  IplImage *sourceImageP = cvCreateImageHeader(cvSize(width, height), IPL_DEPTH_8U, 3);
950  sourceImageP->imageData = reinterpret_cast<char*>(&(left_rgb_image_.data[0]));
951  IplImage *destImageP = cvCreateImageHeader(cvSize(width, height), IPL_DEPTH_8U, 3);
952  destImageP->imageData = reinterpret_cast<char*>(&(left_rgb_rect_image_.data[0]));
953 
954  cvRemap(sourceImageP, destImageP,
955  calibration_map_left_1_,
956  calibration_map_left_2_,
957  CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS,
958  outlierColor);
959 
960  cvReleaseImageHeader(&sourceImageP);
961  cvReleaseImageHeader(&destImageP);
962 
963  left_rgb_rect_image_.header.frame_id = frame_id_left_;
964  left_rgb_rect_image_.header.stamp = ros::Time(header.timeSeconds,
965  1000 * header.timeMicroSeconds);
966  left_rgb_rect_image_.height = height;
967  left_rgb_rect_image_.width = width;
968  left_rgb_rect_image_.encoding = "rgb8";
969  left_rgb_rect_image_.is_bigendian = false;
970  left_rgb_rect_image_.step = 3 * width;
971  left_rgb_rect_cam_info_.header = left_rgb_rect_image_.header;
972  left_rgb_rect_frame_id_ = header.frameId;
973  left_rgb_rect_cam_pub_.publish(left_rgb_rect_image_, left_rgb_rect_cam_info_);
974  left_rgb_rect_cam_info_pub_.publish(left_rgb_rect_cam_info_);
975  }
976  }
977 }
978 
979 void Camera::disparityImageCallback(const image::Header& header)
980 {
981  if (!((Source_Disparity == header.source &&
982  left_disparity_pub_.getNumSubscribers() > 0) ||
983  (Source_Disparity_Right == header.source &&
984  right_disparity_pub_.getNumSubscribers() > 0) ||
985  (Source_Disparity_Cost == header.source &&
986  left_disparity_cost_pub_.getNumSubscribers() > 0) ||
987  (Source_Disparity == header.source &&
988  left_stereo_disparity_pub_.getNumSubscribers() > 0) ||
989  (Source_Disparity_Right == header.source &&
990  right_stereo_disparity_pub_.getNumSubscribers() > 0) ))
991  return;
992 
993  const uint32_t imageSize = (header.width * header.height * header.bitsPerPixel) / 8;
994 
995  ros::Time t = ros::Time(header.timeSeconds,
996  1000 * header.timeMicroSeconds);
997 
998  switch(header.source) {
999  case Source_Disparity:
1000  case Source_Disparity_Right:
1001  {
1002  sensor_msgs::Image *imageP = NULL;
1003  sensor_msgs::CameraInfo *camInfoP = NULL;
1004  image_transport::Publisher *pubP = NULL;
1005  ros::Publisher *camInfoPubP = NULL;
1006  ros::Publisher *stereoDisparityPubP = NULL;
1007  stereo_msgs::DisparityImage *stereoDisparityImageP = NULL;
1008 
1009 
1010  if (Source_Disparity == header.source) {
1011  pubP = &left_disparity_pub_;
1012  imageP = &left_disparity_image_;
1013  imageP->header.frame_id = frame_id_left_;
1014  camInfoP = &left_disp_cam_info_;
1015  camInfoPubP = &left_disp_cam_info_pub_;
1016  stereoDisparityPubP = &left_stereo_disparity_pub_;
1017  stereoDisparityImageP = &left_stereo_disparity_;
1018  stereoDisparityImageP->header.frame_id = frame_id_left_;
1019  } else {
1020  pubP = &right_disparity_pub_;
1021  imageP = &right_disparity_image_;
1022  imageP->header.frame_id = frame_id_right_;
1023  camInfoP = &right_disp_cam_info_;
1024  camInfoPubP = &right_disp_cam_info_pub_;
1025  stereoDisparityPubP = &right_stereo_disparity_pub_;
1026  stereoDisparityImageP = &right_stereo_disparity_;
1027  stereoDisparityImageP->header.frame_id = frame_id_right_;
1028  }
1029 
1030 
1031 
1032  if (pubP->getNumSubscribers() > 0)
1033  {
1034  imageP->data.resize(imageSize);
1035  memcpy(&imageP->data[0], header.imageDataP, imageSize);
1036 
1037  imageP->header.stamp = t;
1038  imageP->height = header.height;
1039  imageP->width = header.width;
1040  imageP->is_bigendian = false;
1041 
1042  switch(header.bitsPerPixel) {
1043  case 8:
1044  imageP->encoding = sensor_msgs::image_encodings::MONO8;
1045  imageP->step = header.width;
1046  break;
1047  case 16:
1048  imageP->encoding = sensor_msgs::image_encodings::MONO16;
1049  imageP->step = header.width * 2;
1050  break;
1051  }
1052 
1053  pubP->publish(*imageP);
1054  }
1055 
1056  if (stereoDisparityPubP->getNumSubscribers() > 0)
1057  {
1058  //
1059  // If our current image resolution is using non-square pixels, i.e.
1060  // fx != fy then warn the user. This support is lacking in
1061  // stereo_msgs::DisparityImage and stereo_image_proc
1062 
1063  if (right_rect_cam_info_.P[0] != right_rect_cam_info_.P[5])
1064  {
1065  std::stringstream warning;
1066  warning << "Current camera configuration has non-square pixels (fx != fy).";
1067  warning << "The stereo_msgs/DisparityImage does not account for";
1068  warning << " this. Be careful when reprojecting to a pointcloud.";
1069  ROS_WARN("%s", warning.str().c_str());
1070  }
1071 
1072  //
1073  // Our final floating point image will be serialized into uint8_t
1074  // meaning we need to allocate 4 bytes per pixel
1075 
1076  uint32_t floatingPointImageSize = header.width * header.height * 4;
1077  stereoDisparityImageP->image.data.resize(floatingPointImageSize);
1078 
1079  stereoDisparityImageP->header.stamp = t;
1080 
1081  stereoDisparityImageP->image.height = header.height;
1082  stereoDisparityImageP->image.width = header.width;
1083  stereoDisparityImageP->image.is_bigendian = false;
1084  stereoDisparityImageP->image.header.stamp = t;
1085  stereoDisparityImageP->image.header.frame_id = stereoDisparityImageP->header.frame_id;
1086  stereoDisparityImageP->image.encoding = "32FC1";
1087  stereoDisparityImageP->image.step = 4 * header.width;
1088 
1089 
1090  //
1091  // Fx is the same for both the right and left cameras
1092 
1093  stereoDisparityImageP->f = right_rect_cam_info_.P[0];
1094 
1095  //
1096  // Our Tx is negative. The DisparityImage message expects Tx to be
1097  // positive
1098 
1099  stereoDisparityImageP->T = fabs(right_rect_cam_info_.P[3] /
1100  right_rect_cam_info_.P[0]);
1101  stereoDisparityImageP->min_disparity = 0;
1102  stereoDisparityImageP->max_disparity = disparities_;
1103  stereoDisparityImageP->delta_d = 1./16.;
1104 
1105  //
1106  // The stereo_msgs::DisparityImage message expects the disparity
1107  // image to be floating point. We will use OpenCV to perform our
1108  // element-wise division
1109 
1110 
1111  cv::Mat_<uint16_t> tmpImage(header.height,
1112  header.width,
1113  reinterpret_cast<uint16_t*>(
1114  const_cast<void*>(header.imageDataP)));
1115 
1116  //
1117  // We will copy our data directly into our output message
1118 
1119  cv::Mat_<float> floatingPointImage(header.height,
1120  header.width,
1121  reinterpret_cast<float*>(&stereoDisparityImageP->image.data[0]));
1122 
1123  //
1124  // Convert our disparity to floating point by dividing by 16 and
1125  // copy the result to the output message
1126 
1127  floatingPointImage = tmpImage / 16.0;
1128 
1129  stereoDisparityPubP->publish(*stereoDisparityImageP);
1130  }
1131 
1132  camInfoP->header = imageP->header;
1133  camInfoP->header.stamp = t;
1134  camInfoPubP->publish(*camInfoP);
1135 
1136  break;
1137  }
1138  case Source_Disparity_Cost:
1139 
1140  left_disparity_cost_image_.data.resize(imageSize);
1141  memcpy(&left_disparity_cost_image_.data[0], header.imageDataP, imageSize);
1142 
1143  left_disparity_cost_image_.header.frame_id = frame_id_left_;
1144  left_disparity_cost_image_.header.stamp = t;
1145  left_disparity_cost_image_.height = header.height;
1146  left_disparity_cost_image_.width = header.width;
1147 
1148  left_disparity_cost_image_.encoding = sensor_msgs::image_encodings::MONO8;
1149  left_disparity_cost_image_.is_bigendian = false;
1150  left_disparity_cost_image_.step = header.width;
1151 
1152  left_disparity_cost_pub_.publish(left_disparity_cost_image_);
1153 
1154  left_cost_cam_info_.header = left_disparity_cost_image_.header;
1155  left_cost_cam_info_pub_.publish(left_cost_cam_info_);
1156 
1157  break;
1158  }
1159 }
1160 
1161 void Camera::monoCallback(const image::Header& header)
1162 {
1163  if (Source_Luma_Left != header.source &&
1164  Source_Luma_Right != header.source) {
1165 
1166  ROS_ERROR("Camera: unexpected image source: 0x%x", header.source);
1167  return;
1168  }
1169 
1170  ros::Time t = ros::Time(header.timeSeconds,
1171  1000 * header.timeMicroSeconds);
1172 
1173  switch(header.source) {
1174  case Source_Luma_Left:
1175 
1176  left_mono_image_.data.resize(header.imageLength);
1177  memcpy(&left_mono_image_.data[0], header.imageDataP, header.imageLength);
1178 
1179  left_mono_image_.header.frame_id = frame_id_left_;
1180  left_mono_image_.header.stamp = t;
1181  left_mono_image_.height = header.height;
1182  left_mono_image_.width = header.width;
1183 
1184  switch(header.bitsPerPixel) {
1185  case 8:
1186  left_mono_image_.encoding = sensor_msgs::image_encodings::MONO8;
1187  left_mono_image_.step = header.width;
1188  break;
1189  case 16:
1190  left_mono_image_.encoding = sensor_msgs::image_encodings::MONO16;
1191  left_mono_image_.step = header.width * 2;
1192  break;
1193  }
1194 
1195  left_mono_image_.is_bigendian = false;
1196 
1197  left_mono_cam_pub_.publish(left_mono_image_);
1198 
1199  //
1200  // Publish a specific camera info message for the left mono image
1201  left_mono_cam_info_.header= left_mono_image_.header;
1202  left_mono_cam_info_pub_.publish(left_mono_cam_info_);
1203 
1204  break;
1205  case Source_Luma_Right:
1206 
1207  right_mono_image_.data.resize(header.imageLength);
1208  memcpy(&right_mono_image_.data[0], header.imageDataP, header.imageLength);
1209 
1210  right_mono_image_.header.frame_id = frame_id_right_;
1211  right_mono_image_.header.stamp = t;
1212  right_mono_image_.height = header.height;
1213  right_mono_image_.width = header.width;
1214 
1215  switch(header.bitsPerPixel) {
1216  case 8:
1217  right_mono_image_.encoding = sensor_msgs::image_encodings::MONO8;
1218  right_mono_image_.step = header.width;
1219  break;
1220  case 16:
1221  right_mono_image_.encoding = sensor_msgs::image_encodings::MONO16;
1222  right_mono_image_.step = header.width * 2;
1223  break;
1224  }
1225  right_mono_image_.is_bigendian = false;
1226 
1227  right_mono_cam_pub_.publish(right_mono_image_);
1228 
1229  //
1230  // Publish a specific camera info message for the left mono image
1231  right_mono_cam_info_.header= right_mono_image_.header;
1232  right_mono_cam_info_pub_.publish(right_mono_cam_info_);
1233 
1234  break;
1235  }
1236 }
1237 
1238 void Camera::rectCallback(const image::Header& header)
1239 {
1240  if (Source_Luma_Rectified_Left != header.source &&
1241  Source_Luma_Rectified_Right != header.source) {
1242 
1243  ROS_ERROR("Camera: unexpected image source: 0x%x", header.source);
1244  return;
1245  }
1246 
1247 
1248  ros::Time t = ros::Time(header.timeSeconds,
1249  1000 * header.timeMicroSeconds);
1250 
1251  switch(header.source) {
1252  case Source_Luma_Rectified_Left:
1253 
1254  left_rect_image_.data.resize(header.imageLength);
1255  memcpy(&left_rect_image_.data[0], header.imageDataP, header.imageLength);
1256 
1257  left_rect_image_.header.frame_id = frame_id_left_;
1258  left_rect_image_.header.stamp = t;
1259  left_rect_image_.height = header.height;
1260  left_rect_image_.width = header.width;
1261 
1262  left_rect_frame_id_ = header.frameId;
1263 
1264 
1265  switch(header.bitsPerPixel) {
1266  case 8:
1267  left_rect_image_.encoding = sensor_msgs::image_encodings::MONO8;
1268  left_rect_image_.step = header.width;
1269 
1270  break;
1271  case 16:
1272  left_rect_image_.encoding = sensor_msgs::image_encodings::MONO16;
1273  left_rect_image_.step = header.width * 2;
1274 
1275  break;
1276  }
1277 
1278  left_rect_image_.is_bigendian = false;
1279 
1280  left_rect_cam_info_.header = left_rect_image_.header;
1281 
1282  //
1283  // Continue to publish the rect camera info on the
1284  // <namespace>/left/camera_info topic for backward compatibility with
1285  // older versions of the driver
1286  left_rect_cam_pub_.publish(left_rect_image_, left_rect_cam_info_);
1287 
1288  left_rect_cam_info_pub_.publish(left_rect_cam_info_);
1289 
1290  publishPointCloud(left_rect_frame_id_,
1291  points_buff_frame_id_,
1292  luma_point_cloud_frame_id_,
1293  luma_point_cloud_pub_,
1294  luma_point_cloud_,
1295  header.width,
1296  header.height,
1297  header.timeSeconds,
1298  header.timeMicroSeconds,
1299  luma_cloud_step,
1300  points_buff_,
1301  &(left_rect_image_.data[0]), luma_color_depth_,
1302  pc_max_range_,
1303  write_pc_color_packed_,
1304  false);
1305 
1306  publishPointCloud(left_rect_frame_id_,
1307  points_buff_frame_id_,
1308  luma_organized_point_cloud_frame_id_,
1309  luma_organized_point_cloud_pub_,
1310  luma_organized_point_cloud_,
1311  header.width,
1312  header.height,
1313  header.timeSeconds,
1314  header.timeMicroSeconds,
1315  luma_cloud_step,
1316  points_buff_,
1317  &(left_rect_image_.data[0]), luma_color_depth_,
1318  pc_max_range_,
1319  write_pc_color_packed_,
1320  true);
1321 
1322  break;
1323  case Source_Luma_Rectified_Right:
1324 
1325  right_rect_image_.data.resize(header.imageLength);
1326  memcpy(&right_rect_image_.data[0], header.imageDataP, header.imageLength);
1327 
1328  right_rect_image_.header.frame_id = frame_id_left_;
1329  right_rect_image_.header.stamp = t;
1330  right_rect_image_.height = header.height;
1331  right_rect_image_.width = header.width;
1332 
1333  switch(header.bitsPerPixel) {
1334  case 8:
1335  right_rect_image_.encoding = sensor_msgs::image_encodings::MONO8;
1336  right_rect_image_.step = header.width;
1337  break;
1338  case 16:
1339  right_rect_image_.encoding = sensor_msgs::image_encodings::MONO16;
1340  right_rect_image_.step = header.width * 2;
1341  break;
1342  }
1343 
1344  right_rect_image_.is_bigendian = false;
1345 
1346  right_rect_cam_info_.header = right_rect_image_.header;
1347 
1348  //
1349  // Continue to publish the rect camera info on the
1350  // <namespace>/right/camera_info topic for backward compatibility with
1351  // older versions of the driver
1352  right_rect_cam_pub_.publish(right_rect_image_, right_rect_cam_info_);
1353 
1354  right_rect_cam_info_pub_.publish(right_rect_cam_info_);
1355 
1356  break;
1357  }
1358 }
1359 
1360 void Camera::depthCallback(const image::Header& header)
1361 {
1362  if (Source_Disparity != header.source) {
1363 
1364  ROS_ERROR("Camera: unexpected image source: 0x%x", header.source);
1365  return;
1366  }
1367 
1368  uint32_t niDepthSubscribers = ni_depth_cam_pub_.getNumSubscribers();
1369  uint32_t depthSubscribers = depth_cam_pub_.getNumSubscribers();
1370  if (0 == niDepthSubscribers &&
1371  0 == depthSubscribers)
1372  return;
1373 
1374  const float bad_point = std::numeric_limits<float>::quiet_NaN();
1375  const uint32_t depthSize = header.height * header.width * sizeof(float);
1376  const uint32_t niDepthSize = header.height * header.width * sizeof(uint16_t);
1377  const uint32_t imageSize = header.width * header.height;
1378 
1379  depth_image_.header.stamp = ros::Time(header.timeSeconds,
1380  1000 * header.timeMicroSeconds);
1381  depth_image_.header.frame_id = frame_id_left_;
1382  depth_image_.height = header.height;
1383  depth_image_.width = header.width;
1384  depth_image_.is_bigendian = (htonl(1) == 1);
1385 
1386  ni_depth_image_ = depth_image_;
1387 
1388  ni_depth_image_.encoding = sensor_msgs::image_encodings::MONO16;
1389  ni_depth_image_.step = header.width * 2;
1390 
1391  depth_image_.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
1392  depth_image_.step = header.width * 4;
1393 
1394  depth_image_.data.resize(depthSize);
1395  ni_depth_image_.data.resize(niDepthSize);
1396 
1397  float *depthImageP = reinterpret_cast<float*>(&depth_image_.data[0]);
1398  uint16_t *niDepthImageP = reinterpret_cast<uint16_t*>(&ni_depth_image_.data[0]);
1399 
1400  //
1401  // Disparity is in 32-bit floating point
1402 
1403  if (32 == header.bitsPerPixel) {
1404 
1405  //
1406  // Depth = focal_length*baseline/disparity
1407  // From the Q matrix used to reproject disparity images using non-isotropic
1408  // pixels we see that z = (fx*fy*Tx). Normalizing z so that
1409  // the scale factor on the homogeneous cartesian coordinate is 1 results
1410  // in z = (fx*fy*Tx)/(-fy*d) or z = (fx*Tx)/(-d).
1411  // The 4th element of the right camera projection matrix is defined
1412  // as fx*Tx.
1413 
1414  const double scale = (-right_rect_cam_info_.P[3]);
1415 
1416  const float *disparityImageP = reinterpret_cast<const float*>(header.imageDataP);
1417 
1418  for (uint32_t i = 0 ; i < imageSize ; ++i)
1419  {
1420  if (0.0 >= disparityImageP[i])
1421  {
1422  depthImageP[i] = bad_point;
1423  niDepthImageP[i] = 0;
1424  }
1425  else
1426  {
1427  depthImageP[i] = scale / disparityImageP[i];
1428  niDepthImageP[i] = static_cast<uint16_t>(depthImageP[i] * 1000);
1429  }
1430  }
1431 
1432  //
1433  // Disparity is in 1/16th pixel, unsigned integer
1434 
1435  } else if (16 == header.bitsPerPixel) {
1436 
1437  //
1438  // Depth = focal_length*baseline/disparity
1439  // From the Q matrix used to reproject disparity images using non-isotropic
1440  // pixels we see that z = (fx*fy*Tx). Normalizing z so that
1441  // the scale factor on the homogeneous cartesian coordinate is 1 results
1442  // in z = (fx*fy*Tx)/(-fy*d) or z = (fx*Tx)/(-d). Because our disparity
1443  // image is 16 bits we must also divide by 16 making z = (fx*Tx*16)/(-d)
1444  // The 4th element of the right camera projection matrix is defined
1445  // as fx*Tx.
1446 
1447 
1448  const float scale = (right_rect_cam_info_.P[3] * -16.0f);
1449 
1450  const uint16_t *disparityImageP = reinterpret_cast<const uint16_t*>(header.imageDataP);
1451 
1452  for (uint32_t i = 0 ; i < imageSize ; ++i)
1453  {
1454  if (0 == disparityImageP[i])
1455  {
1456  depthImageP[i] = bad_point;
1457  niDepthImageP[i] = 0;
1458  }
1459  else
1460  {
1461  depthImageP[i] = scale / disparityImageP[i];
1462  niDepthImageP[i] = static_cast<uint16_t>(depthImageP[i] * 1000);
1463  }
1464  }
1465 
1466  } else {
1467  ROS_ERROR("Camera: unsupported disparity bpp: %d", header.bitsPerPixel);
1468  return;
1469  }
1470 
1471  if (0 != niDepthSubscribers)
1472  {
1473  ni_depth_cam_pub_.publish(ni_depth_image_);
1474  }
1475 
1476  if (0 != depthSubscribers)
1477  {
1478  depth_cam_pub_.publish(depth_image_);
1479  }
1480 
1481  depth_cam_info_.header = depth_image_.header;
1482  depth_cam_info_pub_.publish(depth_cam_info_);
1483 }
1484 
1485 void Camera::pointCloudCallback(const image::Header& header)
1486 {
1487  if (Source_Disparity != header.source) {
1488 
1489  ROS_ERROR("Camera: unexpected image source: 0x%x", header.source);
1490  return;
1491  }
1492 
1493  if (0 == luma_point_cloud_pub_.getNumSubscribers() &&
1494  0 == color_point_cloud_pub_.getNumSubscribers() &&
1495  0 == luma_organized_point_cloud_pub_.getNumSubscribers() &&
1496  0 == color_organized_point_cloud_pub_.getNumSubscribers())
1497  return;
1498 
1499  boost::mutex::scoped_lock lock(border_clip_lock_);
1500 
1501  const bool handle_missing = true;
1502  const uint32_t imageSize = header.height * header.width;
1503 
1504  //
1505  // Resize buffers
1506 
1507  points_buff_.resize(imageSize);
1508  disparity_buff_.resize(imageSize);
1509 
1510  //
1511  // Allocate buffer for reprojection output
1512 
1513  cv::Mat_<cv::Vec3f> points(header.height, header.width, &(points_buff_[0]));
1514 
1515  //
1516  // Image is already 32-bit floating point
1517 
1518  if (32 == header.bitsPerPixel) {
1519 
1520  cv::Mat_<float> disparity(header.height, header.width,
1521  const_cast<float*>(reinterpret_cast<const float*>(header.imageDataP)));
1522 
1523  cv::reprojectImageTo3D(disparity, points, q_matrix_, handle_missing);
1524 
1525  //
1526  // Convert CRL 1/16th pixel disparity to floating point
1527 
1528  } else if (16 == header.bitsPerPixel) {
1529 
1530  cv::Mat_<uint16_t> disparityOrigP(header.height, header.width,
1531  const_cast<uint16_t*>(reinterpret_cast<const uint16_t*>(header.imageDataP)));
1532  cv::Mat_<float> disparity(header.height, header.width, &(disparity_buff_[0]));
1533  disparity = disparityOrigP / 16.0f;
1534 
1535  cv::reprojectImageTo3D(disparity, points, q_matrix_, handle_missing);
1536 
1537  } else {
1538  ROS_ERROR("Camera: unsupported disparity bpp: %d", header.bitsPerPixel);
1539  return;
1540  }
1541 
1542  //
1543  // Apply the border clip mask making all the points in the border clip region
1544  // invalid. Only do this if we have selected a border clip value
1545 
1546  if ( border_clip_value_ > 0)
1547  {
1548  points.setTo(cv::Vec3f(10000.0, 10000.0, 10000.0), border_clip_mask_);
1549  }
1550 
1551 
1552  //
1553  // Store the disparity frame ID
1554 
1555  points_buff_frame_id_ = header.frameId;
1556 
1557  //
1558  // Publish the point clouds if desired/possible
1559 
1560  publishPointCloud(left_rect_frame_id_,
1561  points_buff_frame_id_,
1562  luma_point_cloud_frame_id_,
1563  luma_point_cloud_pub_,
1564  luma_point_cloud_,
1565  header.width,
1566  header.height,
1567  header.timeSeconds,
1568  header.timeMicroSeconds,
1569  luma_cloud_step,
1570  points_buff_,
1571  &(left_rect_image_.data[0]), luma_color_depth_,
1572  pc_max_range_,
1573  write_pc_color_packed_,
1574  false);
1575 
1576  publishPointCloud(left_rgb_rect_frame_id_,
1577  points_buff_frame_id_,
1578  color_point_cloud_frame_id_,
1579  color_point_cloud_pub_,
1580  color_point_cloud_,
1581  header.width,
1582  header.height,
1583  header.timeSeconds,
1584  header.timeMicroSeconds,
1585  color_cloud_step,
1586  points_buff_,
1587  &(left_rgb_rect_image_.data[0]), 3,
1588  pc_max_range_,
1589  write_pc_color_packed_,
1590  false);
1591 
1592  publishPointCloud(left_rect_frame_id_,
1593  points_buff_frame_id_,
1594  luma_organized_point_cloud_frame_id_,
1595  luma_organized_point_cloud_pub_,
1596  luma_organized_point_cloud_,
1597  header.width,
1598  header.height,
1599  header.timeSeconds,
1600  header.timeMicroSeconds,
1601  luma_cloud_step,
1602  points_buff_,
1603  &(left_rect_image_.data[0]), luma_color_depth_,
1604  pc_max_range_,
1605  write_pc_color_packed_,
1606  true);
1607 
1608  publishPointCloud(left_rgb_rect_frame_id_,
1609  points_buff_frame_id_,
1610  color_organized_point_cloud_frame_id_,
1611  color_organized_point_cloud_pub_,
1612  color_organized_point_cloud_,
1613  header.width,
1614  header.height,
1615  header.timeSeconds,
1616  header.timeMicroSeconds,
1617  color_cloud_step,
1618  points_buff_,
1619  &(left_rgb_rect_image_.data[0]), 3,
1620  pc_max_range_,
1621  write_pc_color_packed_,
1622  true);
1623 }
1624 
1625 void Camera::rawCamDataCallback(const image::Header& header)
1626 {
1627  if (0 == raw_cam_data_pub_.getNumSubscribers()) {
1628  got_raw_cam_left_ = false;
1629  return;
1630  }
1631 
1632  const uint32_t imageSize = header.width * header.height;
1633 
1634  //
1635  // The left-rectified image is currently published before
1636  // the matching disparity image.
1637 
1638  if (false == got_raw_cam_left_) {
1639 
1640  if (Source_Luma_Rectified_Left == header.source) {
1641 
1642  raw_cam_data_.gray_scale_image.resize(imageSize);
1643  memcpy(&(raw_cam_data_.gray_scale_image[0]),
1644  header.imageDataP,
1645  imageSize * sizeof(uint8_t));
1646 
1647  raw_cam_data_.frames_per_second = header.framesPerSecond;
1648  raw_cam_data_.gain = header.gain;
1649  raw_cam_data_.exposure_time = header.exposure;
1650  raw_cam_data_.frame_count = header.frameId;
1651  raw_cam_data_.time_stamp = ros::Time(header.timeSeconds,
1652  1000 * header.timeMicroSeconds);
1653  raw_cam_data_.width = header.width;
1654  raw_cam_data_.height = header.height;
1655 
1656  got_raw_cam_left_ = true;
1657  }
1658 
1659  } else if (Source_Disparity == header.source) {
1660 
1661  const uint32_t imageSize = header.width * header.height;
1662 
1663  if (header.frameId == raw_cam_data_.frame_count) {
1664 
1665  raw_cam_data_.disparity_image.resize(imageSize);
1666  memcpy(&(raw_cam_data_.disparity_image[0]),
1667  header.imageDataP, imageSize * sizeof(uint16_t));
1668 
1669  raw_cam_data_pub_.publish(raw_cam_data_);
1670  }
1671 
1672  got_raw_cam_left_ = false;
1673  }
1674 }
1675 
1676 void Camera::colorImageCallback(const image::Header& header)
1677 {
1678  if (0 == left_rgb_cam_pub_.getNumSubscribers() &&
1679  0 == left_rgb_rect_cam_pub_.getNumSubscribers() &&
1680  0 == color_point_cloud_pub_.getNumSubscribers() &&
1681  0 == color_organized_point_cloud_pub_.getNumSubscribers()) {
1682  got_left_luma_ = false;
1683  return;
1684  }
1685 
1686  //
1687  // The left-luma image is currently published before
1688  // the matching chroma image.
1689 
1690  if (false == got_left_luma_) {
1691 
1692  if (Source_Luma_Left == header.source) {
1693 
1694  const uint32_t imageSize = header.width * header.height;
1695 
1696  left_luma_image_.data.resize(imageSize);
1697  memcpy(&left_luma_image_.data[0], header.imageDataP, imageSize);
1698 
1699  left_luma_image_.height = header.height;
1700  left_luma_image_.width = header.width;
1701 
1702  left_luma_frame_id_ = header.frameId;
1703  got_left_luma_ = true;
1704  }
1705 
1706  } else if (Source_Chroma_Left == header.source) {
1707 
1708  if (header.frameId == left_luma_frame_id_) {
1709 
1710  const uint32_t height = left_luma_image_.height;
1711  const uint32_t width = left_luma_image_.width;
1712  const uint32_t imageSize = 3 * height * width;
1713 
1714  left_rgb_image_.data.resize(imageSize);
1715 
1716  left_rgb_image_.header.frame_id = frame_id_left_;
1717  left_rgb_image_.header.stamp = ros::Time(header.timeSeconds,
1718  1000 * header.timeMicroSeconds);
1719  left_rgb_image_.height = height;
1720  left_rgb_image_.width = width;
1721 
1722  left_rgb_image_.encoding = "bgr8";
1723  left_rgb_image_.is_bigendian = false;
1724  left_rgb_image_.step = 3 * width;
1725 
1726  //
1727  // Convert YCbCr 4:2:0 to RGB
1728  // TODO: speed this up
1729 
1730  const uint8_t *lumaP = reinterpret_cast<const uint8_t*>(&(left_luma_image_.data[0]));
1731  const uint8_t *chromaP = reinterpret_cast<const uint8_t*>(header.imageDataP);
1732  uint8_t *bgrP = reinterpret_cast<uint8_t*>(&(left_rgb_image_.data[0]));
1733  const uint32_t rgbStride = width * 3;
1734 
1735  for(uint32_t y=0; y<height; y++) {
1736  for(uint32_t x=0; x<width; x++) {
1737 
1738  const uint32_t lumaOffset = (y * width) + x;
1739  const uint32_t chromaOffset = 2 * (((y/2) * (width/2)) + (x/2));
1740 
1741  const float px_y = static_cast<float>(lumaP[lumaOffset]);
1742  const float px_cb = static_cast<float>(chromaP[chromaOffset+0]) - 128.0f;
1743  const float px_cr = static_cast<float>(chromaP[chromaOffset+1]) - 128.0f;
1744 
1745  float px_r = px_y + 1.402f * px_cr;
1746  float px_g = px_y - 0.34414f * px_cb - 0.71414f * px_cr;
1747  float px_b = px_y + 1.772f * px_cb;
1748 
1749  if (px_r < 0.0f) px_r = 0.0f;
1750  else if (px_r > 255.0f) px_r = 255.0f;
1751  if (px_g < 0.0f) px_g = 0.0f;
1752  else if (px_g > 255.0f) px_g = 255.0f;
1753  if (px_b < 0.0f) px_b = 0.0f;
1754  else if (px_b > 255.0f) px_b = 255.0f;
1755 
1756  const uint32_t rgbOffset = (y * rgbStride) + (3 * x);
1757 
1758  bgrP[rgbOffset + 0] = static_cast<uint8_t>(px_b);
1759  bgrP[rgbOffset + 1] = static_cast<uint8_t>(px_g);
1760  bgrP[rgbOffset + 2] = static_cast<uint8_t>(px_r);
1761  }
1762  }
1763 
1764  if (0 != left_rgb_cam_pub_.getNumSubscribers()) {
1765  left_rgb_cam_pub_.publish(left_rgb_image_);
1766 
1767  left_rgb_cam_info_.header = left_rgb_image_.header;
1768  left_rgb_cam_info_pub_.publish(left_rgb_cam_info_);
1769  }
1770 
1771  if (left_rgb_rect_cam_pub_.getNumSubscribers() > 0 ||
1772  color_point_cloud_pub_.getNumSubscribers() > 0 ||
1773  color_organized_point_cloud_pub_.getNumSubscribers() > 0) {
1774  boost::mutex::scoped_lock lock(cal_lock_);
1775 
1776  if (width != image_config_.width() ||
1777  height != image_config_.height())
1778  //ROS_ERROR("calibration/image size mismatch: image=%dx%d, calibration=%dx%d",
1779  //width, height, image_config_.width(), image_config_.height());
1780  ;
1781  else if (NULL == calibration_map_left_1_ || NULL == calibration_map_left_2_)
1782  ROS_ERROR("Camera: undistort maps not initialized");
1783  else {
1784 
1785  const CvScalar outlierColor = cv::Scalar_<double>(0.0);
1786 
1787  left_rgb_rect_image_.data.resize(imageSize);
1788 
1789  IplImage *sourceImageP = cvCreateImageHeader(cvSize(width, height), IPL_DEPTH_8U, 3);
1790  sourceImageP->imageData = reinterpret_cast<char*>(&(left_rgb_image_.data[0]));
1791  IplImage *destImageP = cvCreateImageHeader(cvSize(width, height), IPL_DEPTH_8U, 3);
1792  destImageP->imageData = reinterpret_cast<char*>(&(left_rgb_rect_image_.data[0]));
1793 
1794  cvRemap(sourceImageP, destImageP,
1795  calibration_map_left_1_,
1796  calibration_map_left_2_,
1797  CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS,
1798  outlierColor);
1799 
1800  cvReleaseImageHeader(&sourceImageP);
1801  cvReleaseImageHeader(&destImageP);
1802 
1803  left_rgb_rect_image_.header.frame_id = frame_id_left_;
1804  left_rgb_rect_image_.header.stamp = ros::Time(header.timeSeconds,
1805  1000 * header.timeMicroSeconds);
1806  left_rgb_rect_image_.height = height;
1807  left_rgb_rect_image_.width = width;
1808 
1809  left_rgb_rect_image_.encoding = "bgr8";
1810  left_rgb_rect_image_.is_bigendian = false;
1811  left_rgb_rect_image_.step = 3 * width;
1812 
1813  left_rgb_rect_cam_info_.header = left_rgb_rect_image_.header;
1814  left_rgb_rect_frame_id_ = header.frameId;
1815 
1816  if (left_rgb_rect_cam_pub_.getNumSubscribers() > 0) {
1817  left_rgb_rect_cam_pub_.publish(left_rgb_rect_image_, left_rgb_rect_cam_info_);
1818 
1819  left_rgb_rect_cam_info_.header = left_rgb_rect_image_.header;
1820  left_rgb_rect_cam_info_pub_.publish(left_rgb_rect_cam_info_);
1821  }
1822 
1823  //
1824  // Publish the color point cloud if desired/possible
1825 
1826  publishPointCloud(left_rgb_rect_frame_id_,
1827  points_buff_frame_id_,
1828  color_point_cloud_frame_id_,
1829  color_point_cloud_pub_,
1830  color_point_cloud_,
1831  left_luma_image_.width,
1832  left_luma_image_.height,
1833  header.timeSeconds,
1834  header.timeMicroSeconds,
1835  color_cloud_step,
1836  points_buff_,
1837  &(left_rgb_rect_image_.data[0]), 3,
1838  pc_max_range_,
1839  write_pc_color_packed_,
1840  false);
1841 
1842  publishPointCloud(left_rgb_rect_frame_id_,
1843  points_buff_frame_id_,
1844  color_organized_point_cloud_frame_id_,
1845  color_organized_point_cloud_pub_,
1846  color_organized_point_cloud_,
1847  left_luma_image_.width,
1848  left_luma_image_.height,
1849  header.timeSeconds,
1850  header.timeMicroSeconds,
1851  color_cloud_step,
1852  points_buff_,
1853  &(left_rgb_rect_image_.data[0]), 3,
1854  pc_max_range_,
1855  write_pc_color_packed_,
1856  true);
1857  }
1858  }
1859  }
1860 
1861  got_left_luma_ = false;
1862  }
1863 }
1864 
1865 void Camera::queryConfig()
1866 {
1867  boost::mutex::scoped_lock lock(cal_lock_);
1868 
1869  //
1870  // Get the camera config
1871 
1872  Status status = driver_->getImageConfig(image_config_);
1873  if (Status_Ok != status) {
1874  ROS_ERROR("Camera: failed to query sensor configuration: %s",
1875  Channel::statusString(status));
1876  return;
1877  }
1878 
1879  //
1880  // For convenience
1881 
1882  const image::Config& c = image_config_;
1883 
1884  disparities_ = c.disparities();
1885 
1886  //
1887  // Frame IDs must match for the rectified images
1888 
1889  left_rect_cam_info_.header.frame_id = frame_id_left_;
1890  left_rect_cam_info_.header.stamp = ros::Time::now();
1891  right_rect_cam_info_.header = left_rect_cam_info_.header;
1892 
1893  left_rect_cam_info_.width = c.width();
1894  left_rect_cam_info_.height = c.height();
1895 
1896  right_rect_cam_info_.width = c.width();
1897  right_rect_cam_info_.height = c.height();
1898 
1899  //
1900  // Calibration from sensor is for native resolution
1901 
1902  image::Calibration cal = image_calibration_;
1903 
1904  const float x_scale = 1.0f / ((static_cast<float>(device_info_.imagerWidth) /
1905  static_cast<float>(c.width())));
1906  const float y_scale = 1.0f / ((static_cast<float>(device_info_.imagerHeight) /
1907  static_cast<float>(c.height())));
1908 
1909  //
1910  // Populate our rectified camera info topics
1911 
1912  updateCameraInfo(left_rect_cam_info_, cal.left.M, cal.left.R, cal.left.P, cal.left.D, x_scale, y_scale);
1913  updateCameraInfo(right_rect_cam_info_, cal.right.M, cal.right.R, cal.right.P, cal.right.D, x_scale, y_scale);
1914 
1915  //
1916  // Copy our rectified camera info topics to populate the other camera info.
1917 
1918  left_disp_cam_info_ = left_rect_cam_info_;
1919  left_cost_cam_info_ = left_rect_cam_info_;
1920  left_rgb_rect_cam_info_ = left_rect_cam_info_;
1921  left_mono_cam_info_ = left_rect_cam_info_;
1922  left_rgb_cam_info_ = left_rect_cam_info_;
1923  depth_cam_info_ = left_rect_cam_info_;
1924 
1925  right_mono_cam_info_ = right_rect_cam_info_;
1926  right_disp_cam_info_ = right_rect_cam_info_;
1927 
1928  //
1929  // Update the frame ID's for the unrectified right image topics
1930 
1931  right_mono_cam_info_.header.frame_id = frame_id_right_;
1932  right_disp_cam_info_.header.frame_id = frame_id_right_;
1933 
1934  //
1935  // Compute the Q matrix here, as image_geometery::StereoCameraModel does
1936  // not allow for non-square pixels.
1937  //
1938  // FyTx 0 0 -FyCxTx
1939  // 0 FxTx 0 -FxCyTx
1940  // 0 0 0 FxFyTx
1941  // 0 0 -Fy Fy(Cx - Cx')
1942 
1943  q_matrix_(0,0) = c.fy() * c.tx();
1944  q_matrix_(1,1) = c.fx() * c.tx();
1945  q_matrix_(0,3) = -c.fy() * c.cx() * c.tx();
1946  q_matrix_(1,3) = -c.fx() * c.cy() * c.tx();
1947  q_matrix_(2,3) = c.fx() * c.fy() * c.tx();
1948  q_matrix_(3,2) = -c.fy();
1949  q_matrix_(3,3) = c.fy() * (right_rect_cam_info_.P[2] - left_rect_cam_info_.P[2]);
1950 
1951  //
1952  // Create rectification maps for local rectification of color images
1953 
1954  if (calibration_map_left_1_)
1955  cvReleaseMat(&calibration_map_left_1_);
1956  if (calibration_map_left_2_)
1957  cvReleaseMat(&calibration_map_left_2_);
1958 
1959  calibration_map_left_1_ = cvCreateMat(c.height(), c.width(), CV_32F);
1960  calibration_map_left_2_ = cvCreateMat(c.height(), c.width(), CV_32F);
1961 
1962  cal.left.M[0][0] *= x_scale; cal.left.M[1][1] *= y_scale;
1963  cal.left.M[0][2] *= x_scale; cal.left.M[1][2] *= y_scale;
1964  cal.right.M[0][0] *= x_scale; cal.right.M[1][1] *= y_scale;
1965  cal.right.M[0][2] *= x_scale; cal.right.M[1][2] *= y_scale;
1966  cal.left.P[0][0] *= x_scale; cal.left.P[1][1] *= y_scale;
1967  cal.left.P[0][2] *= x_scale; cal.left.P[1][2] *= y_scale;
1968  cal.left.P[0][3] *= x_scale; cal.left.P[1][3] *= y_scale;
1969 
1970  CvMat M1 = cvMat(3, 3, CV_32F, &cal.left.M);
1971  CvMat D1 = cvMat(1, 8, CV_32F, &cal.left.D);
1972  CvMat R1 = cvMat(3, 3, CV_32F, &cal.left.R);
1973  CvMat P1 = cvMat(3, 4, CV_32F, &cal.left.P);
1974 
1975  cvInitUndistortRectifyMap(&M1, &D1, &R1, &P1,
1976  calibration_map_left_1_,
1977  calibration_map_left_2_);
1978 
1979 
1980  //
1981  // Publish the "raw" config message
1982 
1983  multisense_ros::RawCamConfig cfg;
1984 
1985  cfg.width = c.width();
1986  cfg.height = c.height();
1987  cfg.frames_per_second = c.fps();
1988  cfg.gain = c.gain();
1989  cfg.exposure_time = c.exposure();
1990 
1991  cfg.fx = c.fx();
1992  cfg.fy = c.fy();
1993  cfg.cx = c.cx();
1994  cfg.cy = c.cy();
1995  cfg.tx = c.tx();
1996  cfg.ty = c.ty();
1997  cfg.tz = c.tz();
1998  cfg.roll = c.roll();
1999  cfg.pitch = c.pitch();
2000  cfg.yaw = c.yaw();
2001 
2002  raw_cam_config_pub_.publish(cfg);
2003 
2004  //
2005  // Update the border clipping mask since the resolution changed
2006 
2007  generateBorderClip(border_clip_type_, border_clip_value_, c.height(), c.width());
2008 
2009  //
2010  // Republish our camera info topics since the resolution changed
2011 
2012  publishAllCameraInfo();
2013 }
2014 
2015 void Camera::updateCameraInfo(sensor_msgs::CameraInfo& cameraInfo,
2016  const float M[3][3],
2017  const float R[3][3],
2018  const float P[3][4],
2019  const float D[8],
2020  double xScale,
2021  double yScale
2022 )
2023 {
2024  cameraInfo.P[0] = P[0][0] * xScale; cameraInfo.P[1] = P[0][1];
2025  cameraInfo.P[2] = P[0][2] * xScale; cameraInfo.P[3] = P[0][3] * xScale;
2026  cameraInfo.P[4] = P[1][0]; cameraInfo.P[5] = P[1][1] * yScale;
2027  cameraInfo.P[6] = P[1][2] * yScale; cameraInfo.P[7] = P[1][3];
2028  cameraInfo.P[8] = P[2][0]; cameraInfo.P[9] = P[2][1];
2029  cameraInfo.P[10] = P[2][2]; cameraInfo.P[11] = P[2][3];
2030 
2031  cameraInfo.K[0] = M[0][0] * xScale; cameraInfo.K[1] = M[0][1];
2032  cameraInfo.K[2] = M[0][2] * xScale; cameraInfo.K[3] = M[1][0];
2033  cameraInfo.K[4] = M[1][1] * yScale; cameraInfo.K[5] = M[1][2] * yScale;
2034  cameraInfo.K[6] = M[2][0]; cameraInfo.K[7] = M[2][1];
2035  cameraInfo.K[8] = M[2][2];
2036 
2037  cameraInfo.R[0] = R[0][0]; cameraInfo.R[1] = R[0][1];
2038  cameraInfo.R[2] = R[0][2]; cameraInfo.R[3] = R[1][0];
2039  cameraInfo.R[4] = R[1][1]; cameraInfo.R[5] = R[1][2];
2040  cameraInfo.R[6] = R[2][0]; cameraInfo.R[7] = R[2][1];
2041  cameraInfo.R[8] = R[2][2];
2042 
2043  //
2044  // Distortion coefficients follow OpenCV's convention.
2045  // k1, k2, p1, p2, k3, k4, k5, k6
2046 
2047  cameraInfo.D.resize(8);
2048  for (uint8_t i=0 ; i < 8 ; ++i) {
2049  cameraInfo.D[i] = D[i];
2050  }
2051 
2052  //
2053  // MultiSense cameras support both the full 8 parameter rational_polynomial
2054  // model and the simplified 5 parameter plum_bob model. If the last 3
2055  // parameters of the distortion model are 0 then the camera is using
2056  // the simplified plumb_bob model
2057 
2058  if (D[7] == 0.0 && D[6] == 0.0 && D[5] == 0.0) {
2059  cameraInfo.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
2060  } else {
2061  cameraInfo.distortion_model = sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL;
2062  }
2063 
2064 }
2065 
2066 
2067 void Camera::publishAllCameraInfo()
2068 {
2069 
2070  //
2071  // Republish camera info messages outside of image callbacks.
2072  // The camera info publishers are latching so the messages
2073  // will persist until a new message is published in one of the image
2074  // callbacks. This makes it easier when a user is trying access a camera_info
2075  // for a topic which they are not subscribed to
2076 
2077  if (system::DeviceInfo::HARDWARE_REV_BCAM == device_info_.hardwareRevision) {
2078 
2079  left_mono_cam_info_pub_.publish(left_mono_cam_info_);
2080  left_rgb_cam_info_pub_.publish(left_rgb_cam_info_);
2081  left_rgb_rect_cam_info_pub_.publish(left_rgb_rect_cam_info_);
2082 
2083  } else if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_M == device_info_.hardwareRevision) {
2084 
2085  left_mono_cam_info_pub_.publish(left_mono_cam_info_);
2086  left_rect_cam_info_pub_.publish(left_rect_cam_info_);
2087  left_rgb_cam_info_pub_.publish(left_rgb_cam_info_);
2088  left_rgb_rect_cam_info_pub_.publish(left_rgb_rect_cam_info_);
2089 
2090  } else { // all other MultiSense-S* variations
2091 
2092  if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_ST21 != device_info_.hardwareRevision) {
2093 
2094  left_rgb_cam_info_pub_.publish(left_rgb_cam_info_);
2095  left_rgb_rect_cam_info_pub_.publish(left_rgb_rect_cam_info_);
2096  }
2097 
2098  if (version_info_.sensorFirmwareVersion >= 0x0300) {
2099 
2100  right_disp_cam_info_pub_.publish(right_disp_cam_info_);
2101  left_cost_cam_info_pub_.publish(left_cost_cam_info_);
2102  }
2103 
2104  left_mono_cam_info_pub_.publish(left_mono_cam_info_);
2105  left_rect_cam_info_pub_.publish(left_rect_cam_info_);
2106  right_mono_cam_info_pub_.publish(right_mono_cam_info_);
2107  right_rect_cam_info_pub_.publish(right_rect_cam_info_);
2108  left_disp_cam_info_pub_.publish(left_disp_cam_info_);
2109  depth_cam_info_pub_.publish(depth_cam_info_);
2110 
2111  }
2112 }
2113 
2114 void Camera::borderClipChanged(int borderClipType, double borderClipValue)
2115 {
2116  //
2117  // This assumes the image resolution did not change and will just use
2118  // the current mask size as width and height arguments
2119 
2120  generateBorderClip(borderClipType, borderClipValue, border_clip_mask_.rows, border_clip_mask_.cols);
2121 
2122 }
2123 
2124 void Camera::generateBorderClip(int borderClipType, double borderClipValue, uint32_t height, uint32_t width)
2125 {
2126 
2127  boost::mutex::scoped_lock lock(border_clip_lock_);
2128 
2129  border_clip_type_ = borderClipType;
2130  border_clip_value_ = borderClipValue;
2131 
2132  //
2133  // Reset the border clip mask
2134 
2135  border_clip_mask_ = cv::Mat_<uint8_t>(height, width, static_cast<uint8_t>(255));
2136 
2137  //
2138  // Manually generate our disparity border clipping mask. Points with
2139  // a value of 255 are excluded from the pointcloud. Points with a value of 0
2140  // are included
2141 
2142  double halfWidth = static_cast<double>(width)/2.0;
2143  double halfHeight = static_cast<double>(height)/2.0;
2144 
2145  //
2146  // Precompute the maximum radius from the center of the image for a point
2147  // to be considered in the circle
2148 
2149  double radius = sqrt( pow( halfWidth, 2) + pow( halfHeight, 2) );
2150  radius -= borderClipValue;
2151 
2152  for (uint32_t u = 0 ; u < width ; ++u)
2153  {
2154  for (uint32_t v = 0 ; v < height ; ++v)
2155  {
2156  switch (borderClipType)
2157  {
2158  case RECTANGULAR:
2159  {
2160  if ( u >= borderClipValue && u <= width - borderClipValue &&
2161  v >= borderClipValue && v <= height - borderClipValue)
2162  {
2163  border_clip_mask_(v, u) = 0;
2164  }
2165 
2166  break;
2167  }
2168  case CIRCULAR:
2169  {
2170  double vector = sqrt( pow( halfWidth - u, 2) +
2171  pow( halfHeight - v, 2) );
2172 
2173  if ( vector < radius)
2174  {
2175  border_clip_mask_(v, u) = 0;
2176  }
2177 
2178  break;
2179  }
2180  default:
2181  {
2182  ROS_WARN("Unknown border clip type.");
2183  return;
2184  }
2185  }
2186  }
2187  }
2188 }
2189 
2190 void Camera::stop()
2191 {
2192  boost::mutex::scoped_lock lock(stream_lock_);
2193 
2194  stream_map_.clear();
2195 
2196  Status status = driver_->stopStreams(allImageSources);
2197  if (Status_Ok != status)
2198  ROS_ERROR("Camera: failed to stop all streams: %s",
2199  Channel::statusString(status));
2200 }
2201 
2202 void Camera::connectStream(DataSource enableMask)
2203 {
2204  boost::mutex::scoped_lock lock(stream_lock_);
2205 
2206  DataSource notStarted = 0;
2207 
2208  for(uint32_t i=0; i<32; i++)
2209  if ((1<<i) & enableMask && 0 == stream_map_[(1<<i)]++)
2210  notStarted |= (1<<i);
2211 
2212  if (0 != notStarted) {
2213 
2214  Status status = driver_->startStreams(notStarted);
2215  if (Status_Ok != status)
2216  ROS_ERROR("Camera: failed to start streams 0x%x: %s",
2217  notStarted, Channel::statusString(status));
2218  }
2219 }
2220 
2221 void Camera::disconnectStream(DataSource disableMask)
2222 {
2223  boost::mutex::scoped_lock lock(stream_lock_);
2224 
2225  DataSource notStopped = 0;
2226 
2227  for(uint32_t i=0; i<32; i++)
2228  if ((1<<i) & disableMask && 0 == --stream_map_[(1<<i)])
2229  notStopped |= (1<<i);
2230 
2231  if (0 != notStopped) {
2232  Status status = driver_->stopStreams(notStopped);
2233  if (Status_Ok != status)
2234  ROS_ERROR("Camera: failed to stop streams 0x%x: %s\n",
2235  notStopped, Channel::statusString(status));
2236  }
2237 }
2238 
2239 
2240 
2241 } // namespace
crl::multisense::Channel::startStreams
virtual Status startStreams(DataSource mask)=0
multisense_ros::Camera::left_rgb_cam_info_pub_
ros::Publisher left_rgb_cam_info_pub_
Definition: camera.h:169
crl::multisense::Source_Disparity_Cost
static CRL_CONSTEXPR DataSource Source_Disparity_Cost
multisense_ros::Camera::right_disparity_pub_
image_transport::Publisher right_disparity_pub_
Definition: camera.h:180
multisense_ros::Camera::luma_organized_point_cloud_pub_
ros::Publisher luma_organized_point_cloud_pub_
Definition: camera.h:176
multisense_ros::Camera::pointCloudCallback
void pointCloudCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:1485
multisense_ros::Camera::rawCamDataCallback
void rawCamDataCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:1625
multisense_ros::Camera::disparity_cost_transport_
image_transport::ImageTransport disparity_cost_transport_
Definition: camera.h:137
multisense_ros::Camera::left_mono_cam_pub_
image_transport::Publisher left_mono_cam_pub_
Definition: camera.h:153
multisense_ros::Camera::right_rect_image_
sensor_msgs::Image right_rect_image_
Definition: camera.h:201
image_transport::CameraPublisher::publish
void publish(const sensor_msgs::Image &image, const sensor_msgs::CameraInfo &info) const
sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL
const std::string RATIONAL_POLYNOMIAL
multisense_ros::Camera::disparityImageCallback
void disparityImageCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:979
multisense_ros::Camera::device_nh_
ros::NodeHandle device_nh_
Definition: camera.h:120
multisense_ros::Camera::luma_color_depth_
uint8_t luma_color_depth_
Definition: camera.h:283
multisense_ros::Camera::image_calibration_
crl::multisense::image::Calibration image_calibration_
Definition: camera.h:237
crl::multisense::Source_Disparity_Right
static CRL_CONSTEXPR DataSource Source_Disparity_Right
ros::NodeHandle
multisense_ros::Camera::right_stereo_disparity_pub_
ros::Publisher right_stereo_disparity_pub_
Definition: camera.h:184
multisense_ros::Camera::right_rect_cam_info_pub_
ros::Publisher right_rect_cam_info_pub_
Definition: camera.h:165
multisense_ros::Camera::pc_color_frame_sync_
bool pc_color_frame_sync_
Definition: camera.h:260
multisense_ros::Camera::colorImageCallback
void colorImageCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:1676
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
crl::multisense::Source_Jpeg_Left
static CRL_CONSTEXPR DataSource Source_Jpeg_Left
multisense_ros::Camera::left_rect_transport_
image_transport::ImageTransport left_rect_transport_
Definition: camera.h:129
multisense_ros::Camera::ni_depth_image_
sensor_msgs::Image ni_depth_image_
Definition: camera.h:203
crl::multisense::Source_Disparity
static CRL_CONSTEXPR DataSource Source_Disparity
multisense_ros::Camera::luma_organized_point_cloud_
sensor_msgs::PointCloud2 luma_organized_point_cloud_
Definition: camera.h:206
f
f
multisense_ros::Camera::luma_point_cloud_
sensor_msgs::PointCloud2 luma_point_cloud_
Definition: camera.h:204
multisense_ros::Camera::monoCallback
void monoCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:1161
multisense_ros::Camera::stop
void stop()
Definition: camera.cpp:2190
multisense_ros::Camera::left_disparity_cost_pub_
image_transport::Publisher left_disparity_cost_pub_
Definition: camera.h:181
multisense_ros::Camera::left_mono_cam_info_
sensor_msgs::CameraInfo left_mono_cam_info_
Definition: camera.h:142
multisense_ros::Camera::CIRCULAR
Definition: camera.h:294
multisense_ros::Camera::color_point_cloud_
sensor_msgs::PointCloud2 color_point_cloud_
Definition: camera.h:205
multisense_ros::Camera::left_stereo_disparity_pub_
ros::Publisher left_stereo_disparity_pub_
Definition: camera.h:183
multisense_ros::Camera::luma_point_cloud_pub_
ros::Publisher luma_point_cloud_pub_
Definition: camera.h:173
crl::multisense::Source_Chroma_Left
static CRL_CONSTEXPR DataSource Source_Chroma_Left
multisense_ros::Camera::got_raw_cam_left_
bool got_raw_cam_left_
Definition: camera.h:220
multisense_ros::Camera::depth_cam_info_pub_
ros::Publisher depth_cam_info_pub_
Definition: camera.h:171
multisense_ros::Camera::border_clip_lock_
boost::mutex border_clip_lock_
Definition: camera.h:304
crl::multisense::Channel::stopStreams
virtual Status stopStreams(DataSource mask)=0
multisense_ros::Camera::histogram_pub_
ros::Publisher histogram_pub_
Definition: camera.h:193
image_transport::CameraPublisher::getNumSubscribers
uint32_t getNumSubscribers() const
multisense_ros::Camera::left_mono_cam_info_pub_
ros::Publisher left_mono_cam_info_pub_
Definition: camera.h:162
multisense_ros::Camera::right_disparity_image_
sensor_msgs::Image right_disparity_image_
Definition: camera.h:215
image_transport::ImageTransport::advertise
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
crl::multisense::Channel::getImageHistogram
virtual Status getImageHistogram(int64_t frameId, image::Histogram &histogram)=0
multisense_ros::Camera::borderClipChanged
void borderClipChanged(int borderClipType, double borderClipValue)
Definition: camera.cpp:2114
multisense_ros::Camera::border_clip_value_
double border_clip_value_
Definition: camera.h:297
multisense_ros::Camera::depth_image_
sensor_msgs::Image depth_image_
Definition: camera.h:202
multisense_ros::Camera::disconnectStream
void disconnectStream(crl::multisense::DataSource disableMask)
Definition: camera.cpp:2221
multisense_ros::Camera::frame_id_right_
std::string frame_id_right_
Definition: camera.h:250
crl::multisense::Source_Luma_Left
static CRL_CONSTEXPR DataSource Source_Luma_Left
multisense_ros::Camera::depthCallback
void depthCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:1360
crl::multisense::image::Config::disparities
uint32_t disparities() const
multisense_ros::Camera::queryConfig
void queryConfig()
Definition: camera.cpp:1865
multisense_ros::Camera::connectStream
void connectStream(crl::multisense::DataSource enableMask)
Definition: camera.cpp:2202
multisense_ros::Camera::raw_cam_config_pub_
ros::Publisher raw_cam_config_pub_
Definition: camera.h:190
multisense_ros::Camera::left_disparity_cost_image_
sensor_msgs::Image left_disparity_cost_image_
Definition: camera.h:214
y
TFSIMD_FORCE_INLINE const tfScalar & y() const
multisense_ros::Camera::right_rect_cam_info_
sensor_msgs::CameraInfo right_rect_cam_info_
Definition: camera.h:145
ROS_WARN
#define ROS_WARN(...)
multisense_ros::Camera::left_luma_frame_id_
int64_t left_luma_frame_id_
Definition: camera.h:222
crl::multisense::Channel::getVersionInfo
virtual Status getVersionInfo(system::VersionInfo &v)=0
multisense_ros::Camera::right_mono_transport_
image_transport::ImageTransport right_mono_transport_
Definition: camera.h:128
image_geometry::StereoCameraModel::MISSING_Z
static const double MISSING_Z
multisense_ros::Camera::left_rgb_transport_
image_transport::ImageTransport left_rgb_transport_
Definition: camera.h:131
multisense_ros::Camera::points_buff_
std::vector< cv::Vec3f > points_buff_
Definition: camera.h:256
multisense_ros::Camera::disparities_
uint32_t disparities_
Definition: camera.h:266
multisense_ros::Camera::left_cost_cam_info_
sensor_msgs::CameraInfo left_cost_cam_info_
Definition: camera.h:149
image_transport::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
multisense_ros::Camera::pc_max_range_
float pc_max_range_
Definition: camera.h:259
multisense_ros::Camera::right_mono_cam_info_pub_
ros::Publisher right_mono_cam_info_pub_
Definition: camera.h:163
multisense_ros::Camera::left_stereo_disparity_
stereo_msgs::DisparityImage left_stereo_disparity_
Definition: camera.h:217
multisense_ros::Camera::frame_id_left_
std::string frame_id_left_
Definition: camera.h:249
crl::multisense::image::Histogram::data
std::vector< uint32_t > data
multisense_ros::Camera::right_mono_cam_pub_
image_transport::Publisher right_mono_cam_pub_
Definition: camera.h:154
multisense_ros::Camera::stream_map_
StreamMapType stream_map_
Definition: camera.h:273
multisense_ros::Camera::device_info_
crl::multisense::system::DeviceInfo device_info_
Definition: camera.h:235
image_transport::ImageTransport::advertiseCamera
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
multisense_ros::Camera::generateBorderClip
void generateBorderClip(int borderClipType, double borderClipValue, uint32_t width, uint32_t height)
Definition: camera.cpp:2124
multisense_ros::Camera::luma_point_cloud_frame_id_
int64_t luma_point_cloud_frame_id_
Definition: camera.h:225
crl::multisense::Source_Luma_Right
static CRL_CONSTEXPR DataSource Source_Luma_Right
multisense_ros::Camera::left_mono_image_
sensor_msgs::Image left_mono_image_
Definition: camera.h:198
multisense_ros::Camera::ni_depth_cam_pub_
image_transport::Publisher ni_depth_cam_pub_
Definition: camera.h:158
multisense_ros::Camera::rectCallback
void rectCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:1238
crl::multisense::Channel::removeIsolatedCallback
virtual Status removeIsolatedCallback(image::Callback callback)=0
crl::multisense::Channel::addIsolatedCallback
virtual Status addIsolatedCallback(image::Callback callback, DataSource imageSourceMask, void *userDataP=NULL)=0
multisense_ros::Camera::left_disparity_pub_
image_transport::Publisher left_disparity_pub_
Definition: camera.h:179
multisense_ros::Camera::raw_cam_data_pub_
ros::Publisher raw_cam_data_pub_
Definition: camera.h:189
multisense_ros::Camera::left_rgb_rect_cam_pub_
image_transport::CameraPublisher left_rgb_rect_cam_pub_
Definition: camera.h:160
multisense_ros::Camera::left_disp_cam_info_pub_
ros::Publisher left_disp_cam_info_pub_
Definition: camera.h:166
multisense_ros::Camera::depth_cam_pub_
image_transport::Publisher depth_cam_pub_
Definition: camera.h:157
image_transport::Publisher::publish
void publish(const sensor_msgs::Image &message) const
multisense_ros::Camera::image_config_
crl::multisense::image::Config image_config_
Definition: camera.h:236
ROS_INFO
#define ROS_INFO(...)
crl::multisense::Channel::getImageCalibration
virtual Status getImageCalibration(image::Calibration &c)=0
multisense_ros::Camera::disparity_buff_
std::vector< float > disparity_buff_
Definition: camera.h:255
sensor_msgs::image_encodings::TYPE_32FC1
const std::string TYPE_32FC1
multisense_ros::Camera::right_nh_
ros::NodeHandle right_nh_
Definition: camera.h:122
multisense_ros::Camera::right_rect_cam_pub_
image_transport::CameraPublisher right_rect_cam_pub_
Definition: camera.h:156
multisense_ros::Camera::left_mono_transport_
image_transport::ImageTransport left_mono_transport_
Definition: camera.h:127
sensor_msgs::distortion_models::PLUMB_BOB
const std::string PLUMB_BOB
sensor_msgs::image_encodings::MONO8
const std::string MONO8
multisense_ros::Camera::raw_cam_cal_pub_
ros::Publisher raw_cam_cal_pub_
Definition: camera.h:191
multisense_ros::Camera::depth_cam_info_
sensor_msgs::CameraInfo depth_cam_info_
Definition: camera.h:151
multisense_ros::Camera::left_rect_cam_pub_
image_transport::CameraPublisher left_rect_cam_pub_
Definition: camera.h:155
multisense_ros::Camera::left_rect_image_
sensor_msgs::Image left_rect_image_
Definition: camera.h:200
multisense_ros::Camera::right_disp_cam_info_pub_
ros::Publisher right_disp_cam_info_pub_
Definition: camera.h:167
distortion_models.h
multisense_ros::Camera::driver_
crl::multisense::Channel * driver_
Definition: camera.h:115
multisense_ros::Camera::left_luma_image_
sensor_msgs::Image left_luma_image_
Definition: camera.h:209
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
multisense_ros::Camera::device_info_pub_
ros::Publisher device_info_pub_
Definition: camera.h:192
multisense_ros::Camera::left_rgb_cam_pub_
image_transport::Publisher left_rgb_cam_pub_
Definition: camera.h:159
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
multisense_ros::Status
Definition: status.h:45
sensor_msgs::image_encodings::MONO16
const std::string MONO16
multisense_ros::Camera::left_rgb_rect_transport_
image_transport::ImageTransport left_rgb_rect_transport_
Definition: camera.h:132
multisense_ros::Camera::left_rect_cam_info_
sensor_msgs::CameraInfo left_rect_cam_info_
Definition: camera.h:144
multisense_ros::Camera::write_pc_color_packed_
bool write_pc_color_packed_
Definition: camera.h:289
multisense_ros::Camera::color_organized_point_cloud_frame_id_
int64_t color_organized_point_cloud_frame_id_
Definition: camera.h:228
multisense_ros::Camera::left_rgb_image_
sensor_msgs::Image left_rgb_image_
Definition: camera.h:210
multisense_ros::Camera::right_mono_image_
sensor_msgs::Image right_mono_image_
Definition: camera.h:199
multisense_ros::Camera::depth_transport_
image_transport::ImageTransport depth_transport_
Definition: camera.h:133
multisense_ros::Camera::left_rgb_rect_cam_info_pub_
ros::Publisher left_rgb_rect_cam_info_pub_
Definition: camera.h:170
multisense_ros::Camera::histogramCallback
void histogramCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:863
multisense_ros::Camera::right_stereo_disparity_
stereo_msgs::DisparityImage right_stereo_disparity_
Definition: camera.h:218
multisense_ros::Camera::calibration_map_left_2_
CvMat * calibration_map_left_2_
Definition: camera.h:244
multisense_ros::Camera::points_buff_frame_id_
int64_t points_buff_frame_id_
Definition: camera.h:257
multisense_ros::Camera::left_disp_cam_info_
sensor_msgs::CameraInfo left_disp_cam_info_
Definition: camera.h:147
DataSource
uint32_t DataSource
multisense_ros::Camera::left_nh_
ros::NodeHandle left_nh_
Definition: camera.h:121
multisense_ros::Camera::color_point_cloud_frame_id_
int64_t color_point_cloud_frame_id_
Definition: camera.h:227
multisense_ros::Camera::right_rect_transport_
image_transport::ImageTransport right_rect_transport_
Definition: camera.h:130
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
multisense_ros::Camera::left_cost_cam_info_pub_
ros::Publisher left_cost_cam_info_pub_
Definition: camera.h:168
multisense_ros::Camera::right_disp_cam_info_
sensor_msgs::CameraInfo right_disp_cam_info_
Definition: camera.h:148
multisense_ros::Camera::stream_lock_
boost::mutex stream_lock_
Definition: camera.h:272
multisense_ros::Camera::left_rect_frame_id_
int64_t left_rect_frame_id_
Definition: camera.h:223
multisense_ros::Camera::luma_organized_point_cloud_frame_id_
int64_t luma_organized_point_cloud_frame_id_
Definition: camera.h:226
multisense_ros::Camera::calibration_map_left_1_
CvMat * calibration_map_left_1_
Definition: camera.h:243
multisense_ros::Camera::border_clip_type_
int border_clip_type_
Definition: camera.h:296
multisense_ros::Camera::left_rgb_cam_info_
sensor_msgs::CameraInfo left_rgb_cam_info_
Definition: camera.h:150
multisense_ros::Camera::ni_depth_transport_
image_transport::ImageTransport ni_depth_transport_
Definition: camera.h:134
multisense_ros::Camera::disparity_right_transport_
image_transport::ImageTransport disparity_right_transport_
Definition: camera.h:136
multisense_ros::Camera::left_rect_cam_info_pub_
ros::Publisher left_rect_cam_info_pub_
Definition: camera.h:164
ros::Time::now
static Time now()
crl::multisense::Channel::getDeviceInfo
virtual Status getDeviceInfo(system::DeviceInfo &info)=0
multisense_ros::Camera::right_mono_cam_info_
sensor_msgs::CameraInfo right_mono_cam_info_
Definition: camera.h:143
multisense_ros::Camera::jpegImageCallback
void jpegImageCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:899
multisense_ros::Camera::left_disparity_image_
sensor_msgs::Image left_disparity_image_
Definition: camera.h:213
image_transport::Publisher
multisense_ros::Camera::color_organized_point_cloud_pub_
ros::Publisher color_organized_point_cloud_pub_
Definition: camera.h:177
crl::multisense::Source_Luma_Rectified_Right
static CRL_CONSTEXPR DataSource Source_Luma_Rectified_Right
multisense_ros::Camera::disparity_left_transport_
image_transport::ImageTransport disparity_left_transport_
Definition: camera.h:135
image_encodings.h
multisense_ros::Camera::color_organized_point_cloud_
sensor_msgs::PointCloud2 color_organized_point_cloud_
Definition: camera.h:207
multisense_ros::Camera::version_info_
crl::multisense::system::VersionInfo version_info_
Definition: camera.h:234
multisense_ros::Camera::q_matrix_
cv::Mat_< double > q_matrix_
Definition: camera.h:258
multisense_ros::Camera::left_rgb_rect_cam_info_
sensor_msgs::CameraInfo left_rgb_rect_cam_info_
Definition: camera.h:146
multisense_ros::Camera::left_rgb_rect_frame_id_
int64_t left_rgb_rect_frame_id_
Definition: camera.h:224
multisense_ros::Camera::left_rgb_rect_image_
sensor_msgs::Image left_rgb_rect_image_
Definition: camera.h:211
multisense_ros::Camera::publishAllCameraInfo
void publishAllCameraInfo()
Definition: camera.cpp:2067
multisense_ros::Camera::got_left_luma_
bool got_left_luma_
Definition: camera.h:221
ROS_ERROR
#define ROS_ERROR(...)
multisense_ros::Camera::raw_cam_data_
multisense_ros::RawCamData raw_cam_data_
Definition: camera.h:229
multisense_ros
Definition: camera.h:51
multisense_ros::Camera::color_point_cloud_pub_
ros::Publisher color_point_cloud_pub_
Definition: camera.h:174
multisense_ros::Camera::border_clip_mask_
cv::Mat_< uint8_t > border_clip_mask_
Definition: camera.h:302
crl::multisense::Channel::getImageConfig
virtual Status getImageConfig(image::Config &c)=0
multisense_ros::Camera::last_frame_id_
int64_t last_frame_id_
Definition: camera.h:278
multisense_ros::Camera::~Camera
~Camera()
Definition: camera.cpp:842
multisense_ros::Camera::cal_lock_
boost::mutex cal_lock_
Definition: camera.h:242
multisense_ros::Camera::updateCameraInfo
void updateCameraInfo(sensor_msgs::CameraInfo &cameraInfo, const float M[3][3], const float R[3][3], const float P[3][4], const float D[8], double xScale=1, double yScale=1)
Definition: camera.cpp:2015
multisense_ros::Camera::RECTANGULAR
Definition: camera.h:294
crl::multisense::Source_Luma_Rectified_Left
static CRL_CONSTEXPR DataSource Source_Luma_Rectified_Left

multisense_ros
Author(s):
autogenerated on Sat Apr 6 2019 02:16:53