camera.cpp
Go to the documentation of this file.
1 
34 #include <arpa/inet.h>
35 #include <fstream>
36 #include <turbojpeg.h>
37 
38 #include <Eigen/Geometry>
39 #include <opencv2/opencv.hpp>
40 
41 #include <sensor_msgs/distortion_models.h>
42 #include <sensor_msgs/image_encodings.h>
43 #include <tf2/LinearMath/Transform.h>
44 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
45 
46 #include <multisense_lib/MultiSenseChannel.hh>
47 #include <multisense_ros/camera.h>
48 #include <multisense_ros/RawCamConfig.h>
49 #include <multisense_ros/RawCamCal.h>
50 #include <multisense_ros/DeviceInfo.h>
51 #include <multisense_ros/Histogram.h>
52 #include <multisense_ros/point_cloud_utilities.h>
53 
54 using namespace crl::multisense;
55 
56 namespace multisense_ros {
57 
58 namespace { // anonymous
59 
60 tf2::Matrix3x3 toRotation(float R[3][3])
61 {
62  return tf2::Matrix3x3{R[0][0],
63  R[0][1],
64  R[0][2],
65  R[1][0],
66  R[1][1],
67  R[1][2],
68  R[2][0],
69  R[2][1],
70  R[2][2]};
71 }
72 
73 //
74 // All of the data sources that we control here
75 
76 constexpr DataSource allImageSources = (Source_Luma_Left |
77  Source_Luma_Right |
78  Source_Luma_Aux |
79  Source_Luma_Rectified_Left |
80  Source_Luma_Rectified_Right |
81  Source_Luma_Rectified_Aux |
82  Source_Chroma_Rectified_Aux |
83  Source_Chroma_Left |
84  Source_Chroma_Aux |
85  Source_Disparity |
86  Source_Disparity_Right |
87  Source_Disparity_Cost |
88  Source_Jpeg_Left);
89 
90 //
91 // Shims for C-style driver callbacks
92 
93 void monoCB(const image::Header& header, void* userDataP)
94 { reinterpret_cast<Camera*>(userDataP)->monoCallback(header); }
95 void rectCB(const image::Header& header, void* userDataP)
96 { reinterpret_cast<Camera*>(userDataP)->rectCallback(header); }
97 void depthCB(const image::Header& header, void* userDataP)
98 { reinterpret_cast<Camera*>(userDataP)->depthCallback(header); }
99 void pointCB(const image::Header& header, void* userDataP)
100 { reinterpret_cast<Camera*>(userDataP)->pointCloudCallback(header); }
101 void rawCB(const image::Header& header, void* userDataP)
102 { reinterpret_cast<Camera*>(userDataP)->rawCamDataCallback(header); }
103 void colorCB(const image::Header& header, void* userDataP)
104 { reinterpret_cast<Camera*>(userDataP)->colorImageCallback(header); }
105 void dispCB(const image::Header& header, void* userDataP)
106 { reinterpret_cast<Camera*>(userDataP)->disparityImageCallback(header); }
107 void jpegCB(const image::Header& header, void* userDataP)
108 { reinterpret_cast<Camera*>(userDataP)->jpegImageCallback(header); }
109 void histCB(const image::Header& header, void* userDataP)
110 { reinterpret_cast<Camera*>(userDataP)->histogramCallback(header); }
111 void colorizeCB(const image::Header& header, void* userDataP)
112 { reinterpret_cast<Camera*>(userDataP)->colorizeCallback(header); }
113 void groundSurfaceCB(const image::Header& header, void* userDataP)
114 { reinterpret_cast<Camera*>(userDataP)->groundSurfaceCallback(header); }
115 void groundSurfaceSplineCB(const ground_surface::Header& header, void* userDataP)
116 { reinterpret_cast<Camera*>(userDataP)->groundSurfaceSplineCallback(header); }
117 
118 
119 bool isValidReprojectedPoint(const Eigen::Vector3f& pt, float squared_max_range)
120 {
121  return pt[2] > 0.0f && std::isfinite(pt[2]) && pt.squaredNorm() < squared_max_range;
122 }
123 
124 void writePoint(sensor_msgs::PointCloud2 &pointcloud, size_t index, const Eigen::Vector3f &point, uint32_t color)
125 {
126  float* cloudP = reinterpret_cast<float*>(&(pointcloud.data[index * pointcloud.point_step]));
127  cloudP[0] = point[0];
128  cloudP[1] = point[1];
129  cloudP[2] = point[2];
130 
131  uint32_t* colorP = reinterpret_cast<uint32_t*>(&(cloudP[3]));
132  colorP[0] = color;
133 }
134 
135 void writePoint(sensor_msgs::PointCloud2 &pointcloud,
136  size_t pointcloud_index,
137  const Eigen::Vector3f &point,
138  size_t image_index,
139  const image::Header &image)
140 {
141  switch (image.bitsPerPixel)
142  {
143  case 8:
144  {
145  const uint32_t luma = static_cast<uint32_t>(reinterpret_cast<const uint8_t*>(image.imageDataP)[image_index]);
146  return writePoint(pointcloud, pointcloud_index, point, luma);
147  }
148  case 16:
149  {
150  const uint32_t luma = static_cast<uint32_t>(reinterpret_cast<const uint16_t*>(image.imageDataP)[image_index]);
151  return writePoint(pointcloud, pointcloud_index, point, luma);
152  }
153  case 32:
154  {
155  const uint32_t luma = reinterpret_cast<const uint32_t*>(image.imageDataP)[image_index];
156  return writePoint(pointcloud, pointcloud_index, point, luma);
157  }
158  }
159 }
160 
161 bool clipPoint(const BorderClip& borderClipType,
162  double borderClipValue,
163  size_t height,
164  size_t width,
165  size_t u,
166  size_t v)
167 {
168  switch (borderClipType)
169  {
170  case BorderClip::NONE:
171  {
172  return false;
173  }
174  case BorderClip::RECTANGULAR:
175  {
176  return !( u >= borderClipValue && u <= width - borderClipValue &&
177  v >= borderClipValue && v <= height - borderClipValue);
178  }
179  case BorderClip::CIRCULAR:
180  {
181  const double halfWidth = static_cast<double>(width)/2.0;
182  const double halfHeight = static_cast<double>(height)/2.0;
183 
184  const double radius = sqrt( halfWidth * halfWidth + halfHeight * halfHeight ) - borderClipValue;
185 
186  return !(Eigen::Vector2d{halfWidth - u, halfHeight - v}.norm() < radius);
187  }
188  default:
189  {
190  ROS_WARN("Camera: Unknown border clip type.");
191  break;
192  }
193  }
194 
195  return true;
196 }
197 
198 cv::Vec3b interpolate_color(const Eigen::Vector2f &pixel, const cv::Mat &image)
199 {
200  const float width = image.cols;
201  const float height = image.rows;
202 
203  const float &u = pixel(0);
204  const float &v = pixel(1);
205 
206  //
207  // Implement a basic bileinar interpolation scheme
208  // https://en.wikipedia.org/wiki/Bilinear_interpolation
209  //
210  const size_t min_u = static_cast<size_t>(std::min(std::max(std::floor(u), 0.f), width - 1.f));
211  const size_t max_u = static_cast<size_t>(std::min(std::max(std::floor(u) + 1, 0.f), width - 1.f));
212  const size_t min_v = static_cast<size_t>(std::min(std::max(std::floor(v), 0.f), height - 1.f));
213  const size_t max_v = static_cast<size_t>(std::min(std::max(std::floor(v) + 1, 0.f), height - 1.f));
214 
215  const cv::Vec3d element00 = image.at<cv::Vec3b>(width * min_v + min_u);
216  const cv::Vec3d element01 = image.at<cv::Vec3b>(width * min_v + max_u);
217  const cv::Vec3d element10 = image.at<cv::Vec3b>(width * max_v + min_u);
218  const cv::Vec3d element11 = image.at<cv::Vec3b>(width * max_v + max_u);
219 
220  const size_t delta_u = max_u - min_u;
221  const size_t delta_v = max_v - min_v;
222 
223  const double u_ratio = delta_u == 0 ? 1. : (static_cast<double>(max_u) - u) / static_cast<double>(delta_u);
224  const double v_ratio = delta_v == 0 ? 1. : (static_cast<double>(max_v) - v) / static_cast<double>(delta_v);
225 
226  const cv::Vec3b f_xy0 = element00 * u_ratio + element01 * (1. - u_ratio);
227  const cv::Vec3b f_xy1 = element10 * u_ratio + element11 * (1. - u_ratio);
228 
229  return (f_xy0 * v_ratio + f_xy1 * (1. - v_ratio));
230 }
231 
232 } // anonymous
233 
234 //
235 // Provide compiler with definition of the static members
236 
237 constexpr char Camera::LEFT[];
238 constexpr char Camera::RIGHT[];
239 constexpr char Camera::AUX[];
240 constexpr char Camera::CALIBRATION[];
241 constexpr char Camera::GROUND_SURFACE[];
242 
243 constexpr char Camera::ORIGIN_FRAME[];
244 constexpr char Camera::HEAD_FRAME[];
245 constexpr char Camera::LEFT_CAMERA_FRAME[];
246 constexpr char Camera::RIGHT_CAMERA_FRAME[];
247 constexpr char Camera::LEFT_RECTIFIED_FRAME[];
248 constexpr char Camera::RIGHT_RECTIFIED_FRAME[];
249 constexpr char Camera::AUX_CAMERA_FRAME[];
250 constexpr char Camera::AUX_RECTIFIED_FRAME[];
251 
252 constexpr char Camera::DEVICE_INFO_TOPIC[];
253 constexpr char Camera::RAW_CAM_CAL_TOPIC[];
254 constexpr char Camera::RAW_CAM_CONFIG_TOPIC[];
255 constexpr char Camera::RAW_CAM_DATA_TOPIC[];
256 constexpr char Camera::HISTOGRAM_TOPIC[];
257 constexpr char Camera::MONO_TOPIC[];
258 constexpr char Camera::RECT_TOPIC[];
259 constexpr char Camera::DISPARITY_TOPIC[];
260 constexpr char Camera::DISPARITY_IMAGE_TOPIC[];
261 constexpr char Camera::DEPTH_TOPIC[];
262 constexpr char Camera::OPENNI_DEPTH_TOPIC[];
263 constexpr char Camera::COST_TOPIC[];
264 constexpr char Camera::COLOR_TOPIC[];
265 constexpr char Camera::RECT_COLOR_TOPIC[];
266 constexpr char Camera::POINTCLOUD_TOPIC[];
267 constexpr char Camera::COLOR_POINTCLOUD_TOPIC[];
268 constexpr char Camera::ORGANIZED_POINTCLOUD_TOPIC[];
269 constexpr char Camera::COLOR_ORGANIZED_POINTCLOUD_TOPIC[];
270 constexpr char Camera::MONO_CAMERA_INFO_TOPIC[];
271 constexpr char Camera::RECT_CAMERA_INFO_TOPIC[];
272 constexpr char Camera::COLOR_CAMERA_INFO_TOPIC[];
273 constexpr char Camera::RECT_COLOR_CAMERA_INFO_TOPIC[];
274 constexpr char Camera::DEPTH_CAMERA_INFO_TOPIC[];
275 constexpr char Camera::DISPARITY_CAMERA_INFO_TOPIC[];
276 constexpr char Camera::COST_CAMERA_INFO_TOPIC[];
277 constexpr char Camera::GROUND_SURFACE_IMAGE_TOPIC[];
278 constexpr char Camera::GROUND_SURFACE_INFO_TOPIC[];
279 constexpr char Camera::GROUND_SURFACE_POINT_SPLINE_TOPIC[];
280 
281 Camera::Camera(Channel* driver, const std::string& tf_prefix) :
282  driver_(driver),
283  device_nh_(""),
284  left_nh_(device_nh_, LEFT),
285  right_nh_(device_nh_, RIGHT),
286  aux_nh_(device_nh_, AUX),
287  calibration_nh_(device_nh_, CALIBRATION),
288  ground_surface_nh_(device_nh_, GROUND_SURFACE),
289  left_mono_transport_(left_nh_),
290  right_mono_transport_(right_nh_),
291  left_rect_transport_(left_nh_),
292  right_rect_transport_(right_nh_),
293  left_rgb_transport_(left_nh_),
294  left_rgb_rect_transport_(left_nh_),
295  depth_transport_(device_nh_),
296  ni_depth_transport_(device_nh_),
297  disparity_left_transport_(left_nh_),
298  disparity_right_transport_(right_nh_),
299  disparity_cost_transport_(left_nh_),
300  aux_mono_transport_(aux_nh_),
301  aux_rgb_transport_(aux_nh_),
302  aux_rect_transport_(aux_nh_),
303  aux_rgb_rect_transport_(aux_nh_),
304  ground_surface_transport_(ground_surface_nh_),
305  stereo_calibration_manager_(nullptr),
306  frame_id_origin_(tf_prefix + ORIGIN_FRAME),
307  frame_id_head_(tf_prefix + HEAD_FRAME),
308  frame_id_left_(tf_prefix + LEFT_CAMERA_FRAME),
309  frame_id_right_(tf_prefix + RIGHT_CAMERA_FRAME),
310  frame_id_aux_(tf_prefix + AUX_CAMERA_FRAME),
311  frame_id_rectified_left_(tf_prefix + LEFT_RECTIFIED_FRAME),
312  frame_id_rectified_right_(tf_prefix + RIGHT_RECTIFIED_FRAME),
313  frame_id_rectified_aux_(tf_prefix + AUX_RECTIFIED_FRAME),
314  static_tf_broadcaster_(),
315  pointcloud_max_range_(15.0),
316  last_frame_id_(-1),
317  border_clip_type_(BorderClip::NONE),
318  border_clip_value_(0.0)
319 {
320  //
321  // Query device and version information from sensor
322 
323  Status status = driver_->getVersionInfo(version_info_);
324  if (Status_Ok != status) {
325  ROS_ERROR("Camera: failed to query version info: %s", Channel::statusString(status));
326  return;
327  }
328 
329  status = driver_->getDeviceInfo(device_info_);
330  if (Status_Ok != status) {
331  ROS_ERROR("Camera: failed to query device info: %s", Channel::statusString(status));
332  return;
333  }
334 
335  //
336  // Get the camera config
337 
338  image::Config image_config;
339  status = driver_->getImageConfig(image_config);
340  if (Status_Ok != status) {
341  ROS_ERROR("Camera: failed to query sensor configuration: %s", Channel::statusString(status));
342  return;
343  }
344 
345  //
346  // S27/S30 cameras have a 3rd aux color camera and no left color camera
347 
348  has_left_camera_ = system::DeviceInfo::HARDWARE_REV_MULTISENSE_SL == device_info_.hardwareRevision ||
349  system::DeviceInfo::HARDWARE_REV_MULTISENSE_S7 == device_info_.hardwareRevision ||
350  system::DeviceInfo::HARDWARE_REV_MULTISENSE_M == device_info_.hardwareRevision ||
351  system::DeviceInfo::HARDWARE_REV_MULTISENSE_S7S == device_info_.hardwareRevision ||
352  system::DeviceInfo::HARDWARE_REV_MULTISENSE_S21 == device_info_.hardwareRevision ||
353  system::DeviceInfo::HARDWARE_REV_MULTISENSE_ST21 == device_info_.hardwareRevision ||
354  system::DeviceInfo::HARDWARE_REV_MULTISENSE_C6S2_S27 == device_info_.hardwareRevision ||
355  system::DeviceInfo::HARDWARE_REV_MULTISENSE_S30 == device_info_.hardwareRevision ||
356  system::DeviceInfo::HARDWARE_REV_MULTISENSE_S7AR == device_info_.hardwareRevision ||
357  system::DeviceInfo::HARDWARE_REV_MULTISENSE_KS21 == device_info_.hardwareRevision ||
358  system::DeviceInfo::HARDWARE_REV_MULTISENSE_MONOCAM == device_info_.hardwareRevision ||
359  system::DeviceInfo::HARDWARE_REV_MULTISENSE_REMOTE_HEAD_STEREO == device_info_.hardwareRevision ||
360  system::DeviceInfo::HARDWARE_REV_MULTISENSE_REMOTE_HEAD_MONOCAM == device_info_.hardwareRevision ||
361  system::DeviceInfo::HARDWARE_REV_MONO == device_info_.hardwareRevision;
362 
363  has_right_camera_ = system::DeviceInfo::HARDWARE_REV_MULTISENSE_SL == device_info_.hardwareRevision ||
364  system::DeviceInfo::HARDWARE_REV_MULTISENSE_S7 == device_info_.hardwareRevision ||
365  system::DeviceInfo::HARDWARE_REV_MULTISENSE_S7S == device_info_.hardwareRevision ||
366  system::DeviceInfo::HARDWARE_REV_MULTISENSE_S21 == device_info_.hardwareRevision ||
367  system::DeviceInfo::HARDWARE_REV_MULTISENSE_ST21 == device_info_.hardwareRevision ||
368  system::DeviceInfo::HARDWARE_REV_MULTISENSE_C6S2_S27 == device_info_.hardwareRevision ||
369  system::DeviceInfo::HARDWARE_REV_MULTISENSE_S30 == device_info_.hardwareRevision ||
370  system::DeviceInfo::HARDWARE_REV_MULTISENSE_S7AR == device_info_.hardwareRevision ||
371  system::DeviceInfo::HARDWARE_REV_MULTISENSE_KS21 == device_info_.hardwareRevision ||
372  system::DeviceInfo::HARDWARE_REV_MULTISENSE_REMOTE_HEAD_STEREO == device_info_.hardwareRevision;
373 
374  has_aux_camera_ = system::DeviceInfo::HARDWARE_REV_MULTISENSE_C6S2_S27 == device_info_.hardwareRevision ||
375  system::DeviceInfo::HARDWARE_REV_MULTISENSE_S30 == device_info_.hardwareRevision;
376 
377  has_color_ = has_aux_camera_ ||
378  system::DeviceInfo::HARDWARE_REV_MULTISENSE_M == device_info_.hardwareRevision ||
379  system::DeviceInfo::IMAGER_TYPE_CMV2000_COLOR == device_info_.imagerType ||
380  system::DeviceInfo::IMAGER_TYPE_CMV4000_COLOR == device_info_.imagerType ||
381  system::DeviceInfo::IMAGER_TYPE_AR0239_COLOR == device_info_.imagerType;
382 
383 
384  //
385  // Topics published for all device types
386 
387  device_info_pub_ = calibration_nh_.advertise<multisense_ros::DeviceInfo>(DEVICE_INFO_TOPIC, 1, true);
388  raw_cam_cal_pub_ = calibration_nh_.advertise<multisense_ros::RawCamCal>(RAW_CAM_CAL_TOPIC, 1, true);
389  raw_cam_config_pub_ = calibration_nh_.advertise<multisense_ros::RawCamConfig>(RAW_CAM_CONFIG_TOPIC, 1, true);
390  histogram_pub_ = device_nh_.advertise<multisense_ros::Histogram>(HISTOGRAM_TOPIC, 5);
391 
392  //
393  // Create spline-based ground surface publishers for S27/S30 cameras
394 
395  const bool can_support_ground_surface_ =
396  system::DeviceInfo::HARDWARE_REV_MULTISENSE_C6S2_S27 == device_info_.hardwareRevision ||
397  system::DeviceInfo::HARDWARE_REV_MULTISENSE_S30 == device_info_.hardwareRevision ||
398  system::DeviceInfo::HARDWARE_REV_MULTISENSE_KS21 == device_info_.hardwareRevision ||
399  system::DeviceInfo::HARDWARE_REV_MULTISENSE_REMOTE_HEAD_STEREO == device_info_.hardwareRevision;
400 
401  if (can_support_ground_surface_) {
402  ground_surface_cam_pub_ = ground_surface_transport_.advertise(GROUND_SURFACE_IMAGE_TOPIC, 5,
403  std::bind(&Camera::connectStream, this, Source_Ground_Surface_Class_Image),
404  std::bind(&Camera::disconnectStream, this, Source_Ground_Surface_Class_Image));
405 
406  ground_surface_info_pub_ = ground_surface_nh_.advertise<sensor_msgs::CameraInfo>(GROUND_SURFACE_INFO_TOPIC, 1, true);
407 
408  ground_surface_spline_pub_ = ground_surface_nh_.advertise<sensor_msgs::PointCloud2>(GROUND_SURFACE_POINT_SPLINE_TOPIC, 5, true);
409  }
410 
411 
412  if (system::DeviceInfo::HARDWARE_REV_BCAM == device_info_.hardwareRevision) {
413 
414  // bcam has unique topics compared to all other S* variants
415 
416  left_mono_cam_pub_ = left_mono_transport_.advertise(MONO_TOPIC, 5,
417  std::bind(&Camera::connectStream, this, Source_Luma_Left),
418  std::bind(&Camera::disconnectStream, this, Source_Luma_Left));
419 
420  left_rgb_cam_pub_ = left_rgb_transport_.advertise(COLOR_TOPIC, 5,
421  std::bind(&Camera::connectStream, this, Source_Jpeg_Left),
422  std::bind(&Camera::disconnectStream, this, Source_Jpeg_Left));
423 
424  left_rgb_rect_cam_pub_ = left_rgb_rect_transport_.advertiseCamera(RECT_COLOR_TOPIC, 5,
425  std::bind(&Camera::connectStream, this, Source_Jpeg_Left),
426  std::bind(&Camera::disconnectStream, this, Source_Jpeg_Left));
427 
428  left_mono_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>(MONO_CAMERA_INFO_TOPIC, 1, true);
429  left_rgb_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>(COLOR_CAMERA_INFO_TOPIC, 1, true);
430  left_rgb_rect_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>(RECT_COLOR_CAMERA_INFO_TOPIC, 1, true);
431 
432  } else {
433 
434  // all other MultiSense-S* variations
435 
436  if (has_left_camera_) {
437 
438  left_mono_cam_pub_ = left_mono_transport_.advertise(MONO_TOPIC, 5,
439  std::bind(&Camera::connectStream, this, Source_Luma_Left),
440  std::bind(&Camera::disconnectStream, this, Source_Luma_Left));
441 
442  left_mono_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>(MONO_CAMERA_INFO_TOPIC, 1, true);
443 
444  left_rect_cam_pub_ = left_rect_transport_.advertiseCamera(RECT_TOPIC, 5,
445  std::bind(&Camera::connectStream, this, Source_Luma_Rectified_Left),
446  std::bind(&Camera::disconnectStream, this, Source_Luma_Rectified_Left));
447 
448  left_rect_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>(RECT_CAMERA_INFO_TOPIC, 1, true);
449  }
450 
451  if (has_right_camera_) {
452 
453  right_mono_cam_pub_ = right_mono_transport_.advertise(MONO_TOPIC, 5,
454  std::bind(&Camera::connectStream, this, Source_Luma_Right),
455  std::bind(&Camera::disconnectStream, this, Source_Luma_Right));
456 
457  right_mono_cam_info_pub_ = right_nh_.advertise<sensor_msgs::CameraInfo>(MONO_CAMERA_INFO_TOPIC, 1, true);
458 
459  right_rect_cam_pub_ = right_rect_transport_.advertiseCamera(RECT_TOPIC, 5,
460  std::bind(&Camera::connectStream, this, Source_Luma_Rectified_Right),
461  std::bind(&Camera::disconnectStream, this, Source_Luma_Rectified_Right));
462 
463  right_rect_cam_info_pub_ = right_nh_.advertise<sensor_msgs::CameraInfo>(RECT_CAMERA_INFO_TOPIC, 1, true);
464  }
465 
466  // only publish for stereo cameras
467  if (has_left_camera_ && has_right_camera_) {
468 
469  // run the stereo topics if left and right cameras both exist
470 
471  depth_cam_pub_ = depth_transport_.advertise(DEPTH_TOPIC, 5,
472  std::bind(&Camera::connectStream, this, Source_Disparity),
473  std::bind(&Camera::disconnectStream, this, Source_Disparity));
474 
475  ni_depth_cam_pub_ = ni_depth_transport_.advertise(OPENNI_DEPTH_TOPIC, 5,
476  std::bind(&Camera::connectStream, this, Source_Disparity),
477  std::bind(&Camera::disconnectStream, this, Source_Disparity));
478 
479  depth_cam_info_pub_ = device_nh_.advertise<sensor_msgs::CameraInfo>(DEPTH_CAMERA_INFO_TOPIC, 1, true);
480 
481  luma_point_cloud_pub_ = device_nh_.advertise<sensor_msgs::PointCloud2>(POINTCLOUD_TOPIC, 5,
482  std::bind(&Camera::connectStream, this, Source_Luma_Rectified_Left | Source_Disparity),
483  std::bind(&Camera::disconnectStream, this, Source_Luma_Rectified_Left | Source_Disparity));
484 
485  luma_organized_point_cloud_pub_ = device_nh_.advertise<sensor_msgs::PointCloud2>(ORGANIZED_POINTCLOUD_TOPIC, 5,
486  std::bind(&Camera::connectStream, this, Source_Luma_Rectified_Left | Source_Disparity),
487  std::bind(&Camera::disconnectStream, this, Source_Luma_Rectified_Left | Source_Disparity));
488 
489  raw_cam_data_pub_ = calibration_nh_.advertise<multisense_ros::RawCamData>(RAW_CAM_DATA_TOPIC, 5,
490  std::bind(&Camera::connectStream, this, Source_Luma_Rectified_Left | Source_Disparity),
491  std::bind(&Camera::disconnectStream, this, Source_Luma_Rectified_Left | Source_Disparity));
492 
493  left_disparity_pub_ = disparity_left_transport_.advertise(DISPARITY_TOPIC, 5,
494  std::bind(&Camera::connectStream, this, Source_Disparity),
495  std::bind(&Camera::disconnectStream, this, Source_Disparity));
496 
497  left_disp_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>(DISPARITY_CAMERA_INFO_TOPIC, 1, true);
498 
499  left_stereo_disparity_pub_ = left_nh_.advertise<stereo_msgs::DisparityImage>(DISPARITY_IMAGE_TOPIC, 5,
500  std::bind(&Camera::connectStream, this, Source_Disparity),
501  std::bind(&Camera::disconnectStream, this, Source_Disparity));
502 
503  if (version_info_.sensorFirmwareVersion >= 0x0300) {
504 
505  right_disparity_pub_ = disparity_right_transport_.advertise(DISPARITY_TOPIC, 5,
506  std::bind(&Camera::connectStream, this, Source_Disparity_Right),
507  std::bind(&Camera::disconnectStream, this, Source_Disparity_Right));
508 
509  right_disp_cam_info_pub_ = right_nh_.advertise<sensor_msgs::CameraInfo>(DISPARITY_CAMERA_INFO_TOPIC, 1, true);
510 
511  right_stereo_disparity_pub_ = right_nh_.advertise<stereo_msgs::DisparityImage>(DISPARITY_IMAGE_TOPIC, 5,
512  std::bind(&Camera::connectStream, this, Source_Disparity_Right),
513  std::bind(&Camera::disconnectStream, this, Source_Disparity_Right));
514 
515  left_disparity_cost_pub_ = disparity_cost_transport_.advertise(COST_TOPIC, 5,
516  std::bind(&Camera::connectStream, this, Source_Disparity_Cost),
517  std::bind(&Camera::disconnectStream, this, Source_Disparity_Cost));
518 
519  left_cost_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>(COST_CAMERA_INFO_TOPIC, 1, true);
520  }
521 
522  // publish colorized stereo topics
523  if (has_aux_camera_) {
524 
525  const auto point_cloud_color_topics = has_aux_camera_ ? Source_Luma_Rectified_Aux | Source_Chroma_Rectified_Aux :
526  Source_Luma_Left | Source_Chroma_Left;
527 
528  color_point_cloud_pub_ = device_nh_.advertise<sensor_msgs::PointCloud2>(COLOR_POINTCLOUD_TOPIC, 5,
529  std::bind(&Camera::connectStream, this,
530  Source_Disparity | point_cloud_color_topics),
531  std::bind(&Camera::disconnectStream, this,
532  Source_Disparity | point_cloud_color_topics));
533 
534  color_organized_point_cloud_pub_ = device_nh_.advertise<sensor_msgs::PointCloud2>(COLOR_ORGANIZED_POINTCLOUD_TOPIC, 5,
535  std::bind(&Camera::connectStream, this,
536  Source_Disparity | point_cloud_color_topics),
537  std::bind(&Camera::disconnectStream, this,
538  Source_Disparity | point_cloud_color_topics));
539 
540  }
541 
542  }
543 
544  // handle color topic publishing if color data exists
545  if (has_aux_camera_) {
546 
547  aux_mono_cam_pub_ = aux_mono_transport_.advertise(MONO_TOPIC, 5,
548  std::bind(&Camera::connectStream, this, Source_Luma_Aux),
549  std::bind(&Camera::disconnectStream, this, Source_Luma_Aux));
550 
551  aux_mono_cam_info_pub_ = aux_nh_.advertise<sensor_msgs::CameraInfo>(MONO_CAMERA_INFO_TOPIC, 1, true);
552 
553  aux_rgb_cam_pub_ = aux_rgb_transport_.advertise(COLOR_TOPIC, 5,
554  std::bind(&Camera::connectStream, this, Source_Luma_Aux | Source_Chroma_Aux),
555  std::bind(&Camera::disconnectStream, this, Source_Luma_Aux | Source_Chroma_Aux));
556 
557  aux_rgb_cam_info_pub_ = aux_nh_.advertise<sensor_msgs::CameraInfo>(COLOR_CAMERA_INFO_TOPIC, 1, true);
558 
559  aux_rect_cam_pub_ = aux_rect_transport_.advertiseCamera(RECT_TOPIC, 5,
560  std::bind(&Camera::connectStream, this, Source_Luma_Rectified_Aux),
561  std::bind(&Camera::disconnectStream, this, Source_Luma_Rectified_Aux));
562 
563  aux_rect_cam_info_pub_ = aux_nh_.advertise<sensor_msgs::CameraInfo>(RECT_CAMERA_INFO_TOPIC, 1, true);
564 
565  aux_rgb_rect_cam_pub_ = aux_rgb_rect_transport_.advertiseCamera(RECT_COLOR_TOPIC, 5,
566  std::bind(&Camera::connectStream, this, Source_Luma_Rectified_Aux | Source_Chroma_Rectified_Aux),
567  std::bind(&Camera::disconnectStream, this, Source_Luma_Rectified_Aux | Source_Chroma_Rectified_Aux));
568 
569  aux_rgb_rect_cam_info_pub_ = aux_nh_.advertise<sensor_msgs::CameraInfo>(RECT_COLOR_CAMERA_INFO_TOPIC, 1, true);
570 
571  } else if (has_color_ && !has_aux_camera_) { // !has_aux_camera_ is redundant, but leaving to prevent confusion over edge cases
572 
573  left_rgb_cam_pub_ = left_rgb_transport_.advertise(COLOR_TOPIC, 5,
574  std::bind(&Camera::connectStream, this, Source_Luma_Left | Source_Chroma_Left),
575  std::bind(&Camera::disconnectStream, this, Source_Luma_Left | Source_Chroma_Left));
576 
577  left_rgb_rect_cam_pub_ = left_rgb_rect_transport_.advertiseCamera(RECT_COLOR_TOPIC, 5,
578  std::bind(&Camera::connectStream, this, Source_Luma_Left | Source_Chroma_Left),
579  std::bind(&Camera::disconnectStream, this, Source_Luma_Left | Source_Chroma_Left));
580 
581  left_rgb_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>(COLOR_CAMERA_INFO_TOPIC, 1, true);
582  left_rgb_rect_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>(RECT_COLOR_CAMERA_INFO_TOPIC, 1, true);
583 
584  }
585 
586  }
587 
588  //
589  // All image streams off
590 
591  stop();
592 
593  //
594  // Publish device info
595 
596  multisense_ros::DeviceInfo msg;
597 
598  msg.deviceName = device_info_.name;
599  msg.buildDate = device_info_.buildDate;
600  msg.serialNumber = device_info_.serialNumber;
601  msg.deviceRevision = device_info_.hardwareRevision;
602 
603  msg.numberOfPcbs = device_info_.pcbs.size();
604  for(const auto &pcb : device_info_.pcbs) {
605  msg.pcbSerialNumbers.push_back(pcb.revision);
606  msg.pcbNames.push_back(pcb.name);
607  }
608 
609  msg.imagerName = device_info_.imagerName;
610  msg.imagerType = device_info_.imagerType;
611  msg.imagerWidth = device_info_.imagerWidth;
612  msg.imagerHeight = device_info_.imagerHeight;
613 
614  msg.lensName = device_info_.lensName;
615  msg.lensType = device_info_.lensType;
616  msg.nominalBaseline = device_info_.nominalBaseline;
617  msg.nominalFocalLength = device_info_.nominalFocalLength;
618  msg.nominalRelativeAperture = device_info_.nominalRelativeAperture;
619 
620  msg.lightingType = device_info_.lightingType;
621  msg.numberOfLights = device_info_.numberOfLights;
622 
623  msg.laserName = device_info_.laserName;
624  msg.laserType = device_info_.laserType;
625 
626  msg.motorName = device_info_.motorName;
627  msg.motorType = device_info_.motorType;
628  msg.motorGearReduction = device_info_.motorGearReduction;
629 
630  msg.apiBuildDate = version_info_.apiBuildDate;
631  msg.apiVersion = version_info_.apiVersion;
632  msg.firmwareBuildDate = version_info_.sensorFirmwareBuildDate;
633  msg.firmwareVersion = version_info_.sensorFirmwareVersion;
634  msg.bitstreamVersion = version_info_.sensorHardwareVersion;
635  msg.bitstreamMagic = version_info_.sensorHardwareMagic;
636  msg.fpgaDna = version_info_.sensorFpgaDna;
637 
638  device_info_pub_.publish(msg);
639 
640  //
641  // Publish image calibration
642 
643  if (has_left_camera_ || has_right_camera_ || has_aux_camera_)
644  {
645  image::Calibration image_calibration;
646  status = driver_->getImageCalibration(image_calibration);
647  if (Status_Ok != status) {
648  ROS_ERROR("Camera: failed to query image calibration: %s",
649  Channel::statusString(status));
650  }
651  else {
652  // currently no ROS cameras have only an aux camera
653  // HARDWARE_REV_MULTISENSE_MONOCAM uses the left cal for its calibration
654 
655  multisense_ros::RawCamCal cal;
656  const float *cP;
657 
658  cP = reinterpret_cast<const float *>(&(image_calibration.left.M[0][0]));
659  for(uint32_t i=0; i<9; i++) cal.left_M[i] = cP[i];
660  cP = reinterpret_cast<const float *>(&(image_calibration.left.D[0]));
661  for(uint32_t i=0; i<8; i++) cal.left_D[i] = cP[i];
662  cP = reinterpret_cast<const float *>(&(image_calibration.left.R[0][0]));
663  for(uint32_t i=0; i<9; i++) cal.left_R[i] = cP[i];
664  cP = reinterpret_cast<const float *>(&(image_calibration.left.P[0][0]));
665  for(uint32_t i=0; i<12; i++) cal.left_P[i] = cP[i];
666 
667  if (not has_right_camera_) {
668 
669  // publish the left calibration twice if there is no right camera
670  cP = reinterpret_cast<const float *>(&(image_calibration.left.M[0][0]));
671  for(uint32_t i=0; i<9; i++) cal.right_M[i] = cP[i];
672  cP = reinterpret_cast<const float *>(&(image_calibration.left.D[0]));
673  for(uint32_t i=0; i<8; i++) cal.right_D[i] = cP[i];
674  cP = reinterpret_cast<const float *>(&(image_calibration.left.R[0][0]));
675  for(uint32_t i=0; i<9; i++) cal.right_R[i] = cP[i];
676  cP = reinterpret_cast<const float *>(&(image_calibration.left.P[0][0]));
677  for(uint32_t i=0; i<12; i++) cal.right_P[i] = cP[i];
678 
679  } else {
680 
681  cP = reinterpret_cast<const float *>(&(image_calibration.right.M[0][0]));
682  for(uint32_t i=0; i<9; i++) cal.right_M[i] = cP[i];
683  cP = reinterpret_cast<const float *>(&(image_calibration.right.D[0]));
684  for(uint32_t i=0; i<8; i++) cal.right_D[i] = cP[i];
685  cP = reinterpret_cast<const float *>(&(image_calibration.right.R[0][0]));
686  for(uint32_t i=0; i<9; i++) cal.right_R[i] = cP[i];
687  cP = reinterpret_cast<const float *>(&(image_calibration.right.P[0][0]));
688  for(uint32_t i=0; i<12; i++) cal.right_P[i] = cP[i];
689 
690  }
691 
692  raw_cam_cal_pub_.publish(cal);
693  }
694 
695  stereo_calibration_manager_ = std::make_shared<StereoCalibrationManager>(image_config, image_calibration, device_info_);
696 
697  if (has_aux_camera_ && !stereo_calibration_manager_->validAux()) {
698  ROS_WARN("Camera: invalid aux camera calibration");
699  }
700 
701  //
702  // Publish the static transforms for our camera extrinsics for the left/right/aux frames. We will
703  // use the left camera frame as the reference coordinate frame
704 
705  const bool has_right_extrinsics = has_right_camera_ && stereo_calibration_manager_->validRight();
706  const bool has_aux_extrinsics = has_aux_camera_ && stereo_calibration_manager_->validAux();
707 
708  std::vector<geometry_msgs::TransformStamped> stamped_transforms{};
709 
710  stamped_transforms.emplace_back();
711  tf2::Transform rectified_left_T_left{toRotation(image_calibration.left.R), tf2::Vector3{0., 0., 0.}};
712  stamped_transforms.back().header.stamp = ros::Time::now();
713  stamped_transforms.back().header.frame_id = frame_id_rectified_left_;
714  stamped_transforms.back().child_frame_id = frame_id_left_;
715  stamped_transforms.back().transform = tf2::toMsg(rectified_left_T_left);
716 
717  if (has_right_extrinsics)
718  {
719  stamped_transforms.emplace_back();
720  tf2::Transform rectified_right_T_rectified_left{tf2::Matrix3x3::getIdentity(),
721  tf2::Vector3{stereo_calibration_manager_->T(), 0., 0.}};
722  stamped_transforms.back().header.stamp = ros::Time::now();
723  stamped_transforms.back().header.frame_id = frame_id_rectified_left_;
724  stamped_transforms.back().child_frame_id = frame_id_rectified_right_;
725  stamped_transforms.back().transform = tf2::toMsg(rectified_right_T_rectified_left.inverse());
726 
727  stamped_transforms.emplace_back();
728  tf2::Transform rectified_right_T_right{toRotation(image_calibration.right.R), tf2::Vector3{0., 0., 0.}};
729  stamped_transforms.back().header.stamp = ros::Time::now();
730  stamped_transforms.back().header.frame_id = frame_id_rectified_right_;
731  stamped_transforms.back().child_frame_id = frame_id_right_;
732  stamped_transforms.back().transform = tf2::toMsg(rectified_right_T_right);
733  }
734 
735  if (has_aux_extrinsics)
736  {
737  const Eigen::Vector3d aux_T = stereo_calibration_manager_->aux_T();
738 
739  stamped_transforms.emplace_back();
740  tf2::Transform rectified_aux_T_rectified_left{tf2::Matrix3x3::getIdentity(),
741  tf2::Vector3{aux_T(0), aux_T(1), aux_T(2)}};
742  stamped_transforms.back().header.stamp = ros::Time::now();
743  stamped_transforms.back().header.frame_id = frame_id_rectified_left_;
744  stamped_transforms.back().child_frame_id = frame_id_rectified_aux_;
745  stamped_transforms.back().transform = tf2::toMsg(rectified_aux_T_rectified_left.inverse());
746 
747  stamped_transforms.emplace_back();
748  tf2::Transform rectified_aux_T_aux{toRotation(image_calibration.aux.R), tf2::Vector3{0., 0., 0.}};
749  stamped_transforms.back().header.stamp = ros::Time::now();
750  stamped_transforms.back().header.frame_id = frame_id_rectified_aux_;
751  stamped_transforms.back().child_frame_id = frame_id_aux_;
752  stamped_transforms.back().transform = tf2::toMsg(rectified_aux_T_aux);
753  }
754 
755  static_tf_broadcaster_.sendTransform(stamped_transforms);
756 
757  //
758  // Update our internal image config and publish intitial camera info
759 
760  updateConfig(image_config);
761  }
762 
763  //
764  // Initialize point cloud data structures
765 
766  luma_point_cloud_ = initialize_pointcloud<float>(true, frame_id_rectified_left_, "intensity");
767  color_point_cloud_ = initialize_pointcloud<float>(true, frame_id_rectified_left_, "rgb");
768  luma_organized_point_cloud_ = initialize_pointcloud<float>(false, frame_id_rectified_left_, "intensity");
769  color_organized_point_cloud_ = initialize_pointcloud<float>(false, frame_id_rectified_left_, "rgb");
770 
771  //
772  // Add driver-level callbacks.
773  //
774  // -Driver creates individual background thread for each callback.
775  // -Images are queued (depth=5) per callback, with oldest silently dropped if not keeping up.
776  // -All images presented are backed by a referenced buffer system (no copying of image data is done.)
777 
778  if (system::DeviceInfo::HARDWARE_REV_BCAM == device_info_.hardwareRevision) {
779 
780  driver_->addIsolatedCallback(monoCB, Source_Luma_Left, this);
781  driver_->addIsolatedCallback(jpegCB, Source_Jpeg_Left, this);
782 
783  } else if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_REMOTE_HEAD_VPB == device_info_.hardwareRevision) {
784  // no incoming image data from vpb
785  } else if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_REMOTE_HEAD_MONOCAM == device_info_.hardwareRevision) {
786 
787  driver_->addIsolatedCallback(monoCB, Source_Luma_Left, this);
788  driver_->addIsolatedCallback(rectCB, Source_Luma_Rectified_Left, this);
789 
790  } else {
791 
792  if (has_left_camera_ || has_right_camera_ || has_aux_camera_) {
793  driver_->addIsolatedCallback(monoCB, Source_Luma_Left | Source_Luma_Right | Source_Luma_Aux, this);
794  driver_->addIsolatedCallback(rectCB, Source_Luma_Rectified_Left | Source_Luma_Rectified_Right | Source_Luma_Rectified_Aux, this);
795  }
796 
797  if (has_color_ && has_aux_camera_) {
798  driver_->addIsolatedCallback(colorizeCB, Source_Luma_Rectified_Aux | Source_Chroma_Rectified_Aux | Source_Luma_Aux |
799  Source_Luma_Left | Source_Luma_Rectified_Left, this);
800  driver_->addIsolatedCallback(colorCB, Source_Chroma_Rectified_Aux | Source_Chroma_Aux, this);
801  } else {
802  driver_->addIsolatedCallback(colorizeCB, Source_Luma_Left | Source_Chroma_Left | Source_Luma_Rectified_Left, this);
803  driver_->addIsolatedCallback(colorCB, Source_Chroma_Left, this);
804  }
805 
806  if (has_left_camera_ && has_right_camera_) {
807  driver_->addIsolatedCallback(depthCB, Source_Disparity, this);
808  driver_->addIsolatedCallback(pointCB, Source_Disparity, this);
809  driver_->addIsolatedCallback(rawCB, Source_Disparity | Source_Luma_Rectified_Left, this);
810  driver_->addIsolatedCallback(dispCB, Source_Disparity | Source_Disparity_Right | Source_Disparity_Cost, this);
811  }
812  }
813 
814  //
815  // A common callback to publish histograms
816 
817  driver_->addIsolatedCallback(histCB, allImageSources, this);
818 
819  //
820  // Add ground surface callbacks for S27/S30 cameras
821 
822  if (can_support_ground_surface_) {
823  driver_->addIsolatedCallback(groundSurfaceCB, Source_Ground_Surface_Class_Image, this);
824  driver_->addIsolatedCallback(groundSurfaceSplineCB, this);
825  }
826 
827  //
828  // Disable color point cloud strict frame syncing, if desired
829 
830  const char *pcColorFrameSyncEnvStringP = getenv("MULTISENSE_ROS_PC_COLOR_FRAME_SYNC_OFF");
831  if (NULL != pcColorFrameSyncEnvStringP) {
832  ROS_INFO("color point cloud frame sync is disabled");
833  }
834 
835  //
836  // Diagnostics
837  diagnostic_updater_.setHardwareID(device_info_.name + " " + std::to_string(device_info_.hardwareRevision));
838  diagnostic_updater_.add("device_info", this, &Camera::deviceInfoDiagnostic);
839  diagnostic_updater_.add("device_status", this, &Camera::deviceStatusDiagnostic);
840  diagnostic_trigger_ = device_nh_.createTimer(ros::Duration(1), &Camera::diagnosticTimerCallback, this);
841 }
842 
843 Camera::~Camera()
844 {
845  stop();
846 
847  if (system::DeviceInfo::HARDWARE_REV_BCAM == device_info_.hardwareRevision) {
848 
849  driver_->removeIsolatedCallback(monoCB);
850  driver_->removeIsolatedCallback(jpegCB);
851 
852  } else {
853 
854  if (has_left_camera_ || has_right_camera_ || has_aux_camera_) {
855  driver_->removeIsolatedCallback(monoCB);
856  driver_->removeIsolatedCallback(rectCB);
857  }
858 
859  if (has_color_) {
860  driver_->removeIsolatedCallback(colorizeCB);
861  driver_->removeIsolatedCallback(colorCB);
862  }
863 
864  if (has_left_camera_ && has_right_camera_) {
865  driver_->removeIsolatedCallback(depthCB);
866  driver_->removeIsolatedCallback(pointCB);
867  driver_->removeIsolatedCallback(rawCB);
868  driver_->removeIsolatedCallback(dispCB);
869  }
870  }
871 
872  if (can_support_ground_surface_)
873  {
874  driver_->removeIsolatedCallback(groundSurfaceCB);
875  driver_->removeIsolatedCallback(groundSurfaceSplineCB);
876  }
877 }
878 
879 void Camera::borderClipChanged(const BorderClip &borderClipType, double borderClipValue)
880 {
881  //
882  // Avoid locking since updating this while a point cloud is current being projected is a non-standard case
883 
884  border_clip_type_ = borderClipType;
885  border_clip_value_ = borderClipValue;
886 }
887 
888 void Camera::maxPointCloudRangeChanged(double range)
889 {
890  pointcloud_max_range_ = range;
891 }
892 
893 void Camera::extrinsicsChanged(crl::multisense::system::ExternalCalibration extrinsics)
894 {
895  // Generate extrinsics matrix
896  Eigen::Matrix<float, 3, 3> eigen_rot =
897  (Eigen::AngleAxis<float>(extrinsics.yaw, Eigen::Matrix<float, 3, 1>(0, 0, 1))
898  * Eigen::AngleAxis<float>(extrinsics.pitch, Eigen::Matrix<float, 3, 1>(0, 1, 0))
899  * Eigen::AngleAxis<float>(extrinsics.roll, Eigen::Matrix<float, 3, 1>(1, 0, 0))).matrix();
900 
901  tf2::Matrix3x3 tf2_rot{
902  eigen_rot(0, 0),
903  eigen_rot(0, 1),
904  eigen_rot(0, 2),
905  eigen_rot(1, 0),
906  eigen_rot(1, 1),
907  eigen_rot(1, 2),
908  eigen_rot(2, 0),
909  eigen_rot(2, 1),
910  eigen_rot(2, 2)};
911 
912  std::vector<geometry_msgs::TransformStamped> extrinsic_transforms_(1);
913 
914  tf2::Transform multisense_head_T_origin{tf2_rot, tf2::Vector3{extrinsics.x, extrinsics.y, extrinsics.z}};
915  extrinsic_transforms_[0].header.stamp = ros::Time::now();
916  extrinsic_transforms_[0].header.frame_id = frame_id_origin_;
917  extrinsic_transforms_[0].child_frame_id = frame_id_rectified_left_;
918  extrinsic_transforms_[0].transform = tf2::toMsg(multisense_head_T_origin);
919 
920  static_tf_broadcaster_.sendTransform(extrinsic_transforms_);
921 }
922 
923 void Camera::groundSurfaceSplineDrawParametersChanged(
924  const ground_surface_utilities::SplineDrawParameters &spline_draw_params)
925 {
926  spline_draw_params_ = spline_draw_params;
927 }
928 
929 void Camera::histogramCallback(const image::Header& header)
930 {
931  if (last_frame_id_ >= header.frameId)
932  return;
933 
934  last_frame_id_ = header.frameId;
935 
936  if (histogram_pub_.getNumSubscribers() > 0) {
937  multisense_ros::Histogram rh;
938  image::Histogram mh;
939 
940  Status status = driver_->getImageHistogram(header.frameId, mh);
941  if (Status_Ok == status) {
942  rh.frame_count = header.frameId;
943  rh.time_stamp = ros::Time(header.timeSeconds,
944  1000 * header.timeMicroSeconds);
945  rh.width = header.width;
946  rh.height = header.height;
947  switch(header.source) {
948  case Source_Chroma_Left:
949  case Source_Chroma_Right:
950  rh.width *= 2;
951  rh.height *= 2;
952  }
953 
954  rh.exposure_time = header.exposure;
955  rh.gain = header.gain;
956  rh.fps = header.framesPerSecond;
957  rh.channels = mh.channels;
958  rh.bins = mh.bins;
959  rh.data = mh.data;
960  histogram_pub_.publish(rh);
961  }
962  }
963 }
964 
965 void Camera::jpegImageCallback(const image::Header& header)
966 {
967  if (Source_Jpeg_Left != header.source) {
968  return;
969  }
970 
971  const ros::Time t = ros::Time(header.timeSeconds, 1000 * header.timeMicroSeconds);
972 
973  if (!stereo_calibration_manager_)
974  {
975  throw std::runtime_error("Uninitialized stereo calibration manager");
976  }
977 
978  const uint32_t height = header.height;
979  const uint32_t width = header.width;
980  const uint32_t rgbLength = height * width * 3;
981 
982  left_rgb_image_.header.frame_id = frame_id_left_;
983  left_rgb_image_.height = height;
984  left_rgb_image_.width = width;
985  left_rgb_image_.encoding = sensor_msgs::image_encodings::RGB8;
986  left_rgb_image_.is_bigendian = (htonl(1) == 1);
987  left_rgb_image_.step = 3 * width;
988  left_rgb_image_.header.stamp = t;
989 
990  left_rgb_image_.data.resize(rgbLength);
991 
992  tjhandle jpegDecompressor = tjInitDecompress();
993  tjDecompress2(jpegDecompressor,
994  reinterpret_cast<unsigned char*>(const_cast<void*>(header.imageDataP)),
995  header.imageLength,
996  &(left_rgb_image_.data[0]),
997  width, 0/*pitch*/, height, TJPF_RGB, 0);
998  tjDestroy(jpegDecompressor);
999 
1000  const auto left_camera_info = stereo_calibration_manager_->leftCameraInfo(frame_id_left_, t);
1001 
1002  left_rgb_cam_pub_.publish(left_rgb_image_);
1003  left_rgb_cam_info_pub_.publish(left_camera_info);
1004 
1005  if (left_rgb_rect_cam_pub_.getNumSubscribers() > 0) {
1006 
1007  const auto left_rectified_camera_info = stereo_calibration_manager_->leftCameraInfo(frame_id_rectified_left_, t);
1008 
1009  left_rgb_rect_image_.data.resize(rgbLength);
1010 
1011  const cv::Mat rgb_image(height, width, CV_8UC3, &(left_rgb_image_.data[0]));
1012  cv::Mat rect_rgb_image(height, width, CV_8UC3, &(left_rgb_rect_image_.data[0]));
1013 
1014  const auto left_remap = stereo_calibration_manager_->leftRemap();
1015 
1016  cv::remap(rgb_image, rect_rgb_image, left_remap->map1, left_remap->map2, cv::INTER_LINEAR);
1017 
1018  left_rgb_rect_image_.header.frame_id = frame_id_rectified_left_;
1019  left_rgb_rect_image_.header.stamp = t;
1020  left_rgb_rect_image_.height = height;
1021  left_rgb_rect_image_.width = width;
1022  left_rgb_rect_image_.encoding = sensor_msgs::image_encodings::RGB8;
1023  left_rgb_rect_image_.is_bigendian = (htonl(1) == 1);
1024  left_rgb_rect_image_.step = 3 * width;
1025  left_rgb_rect_cam_pub_.publish(left_rgb_rect_image_, left_rectified_camera_info);
1026  left_rgb_rect_cam_info_pub_.publish(left_rectified_camera_info);
1027  }
1028 }
1029 
1030 void Camera::disparityImageCallback(const image::Header& header)
1031 {
1032  if (!((Source_Disparity == header.source &&
1033  left_disparity_pub_.getNumSubscribers() > 0) ||
1034  (Source_Disparity_Right == header.source &&
1035  right_disparity_pub_.getNumSubscribers() > 0) ||
1036  (Source_Disparity_Cost == header.source &&
1037  left_disparity_cost_pub_.getNumSubscribers() > 0) ||
1038  (Source_Disparity == header.source &&
1039  left_stereo_disparity_pub_.getNumSubscribers() > 0) ||
1040  (Source_Disparity_Right == header.source &&
1041  right_stereo_disparity_pub_.getNumSubscribers() > 0) ))
1042  return;
1043 
1044  const uint32_t imageSize = (header.width * header.height * header.bitsPerPixel) / 8;
1045 
1046  const ros::Time t = ros::Time(header.timeSeconds, 1000 * header.timeMicroSeconds);
1047 
1048  if (!stereo_calibration_manager_)
1049  {
1050  throw std::runtime_error("Uninitialized stereo calibration manager");
1051  }
1052 
1053  switch(header.source) {
1054  case Source_Disparity:
1055  case Source_Disparity_Right:
1056  {
1057  sensor_msgs::Image *imageP = NULL;
1058  image_transport::Publisher *pubP = NULL;
1059  sensor_msgs::CameraInfo camInfo;
1060  ros::Publisher *camInfoPubP = NULL;
1061  ros::Publisher *stereoDisparityPubP = NULL;
1062  stereo_msgs::DisparityImage *stereoDisparityImageP = NULL;
1063 
1064 
1065  if (Source_Disparity == header.source) {
1066  pubP = &left_disparity_pub_;
1067  imageP = &left_disparity_image_;
1068  imageP->header.frame_id = frame_id_rectified_left_;
1069  camInfo = stereo_calibration_manager_->leftCameraInfo(frame_id_rectified_left_, t);
1070  camInfoPubP = &left_disp_cam_info_pub_;
1071  stereoDisparityPubP = &left_stereo_disparity_pub_;
1072  stereoDisparityImageP = &left_stereo_disparity_;
1073  stereoDisparityImageP->header.frame_id = frame_id_rectified_left_;
1074  } else {
1075  pubP = &right_disparity_pub_;
1076  imageP = &right_disparity_image_;
1077  imageP->header.frame_id = frame_id_rectified_right_;
1078  camInfo = stereo_calibration_manager_->rightCameraInfo(frame_id_rectified_right_, t);
1079  camInfoPubP = &right_disp_cam_info_pub_;
1080  stereoDisparityPubP = &right_stereo_disparity_pub_;
1081  stereoDisparityImageP = &right_stereo_disparity_;
1082  stereoDisparityImageP->header.frame_id = frame_id_rectified_right_;
1083  }
1084 
1085  if (pubP->getNumSubscribers() > 0)
1086  {
1087  imageP->data.resize(imageSize);
1088  memcpy(&imageP->data[0], header.imageDataP, imageSize);
1089 
1090  imageP->header.stamp = t;
1091  imageP->height = header.height;
1092  imageP->width = header.width;
1093  imageP->is_bigendian = (htonl(1) == 1);
1094 
1095  switch(header.bitsPerPixel) {
1096  case 8:
1097  imageP->encoding = sensor_msgs::image_encodings::MONO8;
1098  imageP->step = header.width;
1099  break;
1100  case 16:
1101  imageP->encoding = sensor_msgs::image_encodings::MONO16;
1102  imageP->step = header.width * 2;
1103  break;
1104  }
1105 
1106  pubP->publish(*imageP);
1107  }
1108 
1109  if (stereoDisparityPubP->getNumSubscribers() > 0)
1110  {
1111  if (!stereo_calibration_manager_->validRight())
1112  {
1113  throw std::runtime_error("Stereo calibration manager missing right calibration");
1114  }
1115 
1116  //
1117  // If our current image resolution is using non-square pixels, i.e.
1118  // fx != fy then warn the user. This support is lacking in
1119  // stereo_msgs::DisparityImage and stereo_image_proc
1120 
1121  if (camInfo.P[0] != camInfo.P[5])
1122  {
1123  std::stringstream warning;
1124  warning << "Current camera configuration has non-square pixels (fx != fy).";
1125  warning << "The stereo_msgs/DisparityImage does not account for";
1126  warning << " this. Be careful when reprojecting to a pointcloud.";
1127  ROS_WARN("%s", warning.str().c_str());
1128  }
1129 
1130  //
1131  // Our final floating point image will be serialized into uint8_t
1132  // meaning we need to allocate 4 bytes per pixel
1133 
1134  uint32_t floatingPointImageSize = header.width * header.height * 4;
1135  stereoDisparityImageP->image.data.resize(floatingPointImageSize);
1136 
1137  stereoDisparityImageP->header.stamp = t;
1138 
1139  stereoDisparityImageP->image.height = header.height;
1140  stereoDisparityImageP->image.width = header.width;
1141  stereoDisparityImageP->image.is_bigendian = (htonl(1) == 1);
1142  stereoDisparityImageP->image.header.stamp = t;
1143  stereoDisparityImageP->image.header.frame_id = stereoDisparityImageP->header.frame_id;
1144  stereoDisparityImageP->image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
1145  stereoDisparityImageP->image.step = 4 * header.width;
1146 
1147 
1148  //
1149  // Fx is the same for both the right and left cameras
1150 
1151  stereoDisparityImageP->f = camInfo.P[0];
1152 
1153  //
1154  // Our Tx is negative. The DisparityImage message expects Tx to be
1155  // positive
1156 
1157  stereoDisparityImageP->T = fabs(stereo_calibration_manager_->T());
1158  stereoDisparityImageP->min_disparity = 0;
1159  stereoDisparityImageP->max_disparity = stereo_calibration_manager_->config().disparities();
1160  stereoDisparityImageP->delta_d = 1./16.;
1161 
1162  //
1163  // The stereo_msgs::DisparityImage message expects the disparity
1164  // image to be floating point. We will use OpenCV to perform our
1165  // element-wise division
1166 
1167 
1168  cv::Mat_<uint16_t> tmpImage(header.height,
1169  header.width,
1170  reinterpret_cast<uint16_t*>(
1171  const_cast<void*>(header.imageDataP)));
1172 
1173  //
1174  // We will copy our data directly into our output message
1175 
1176  cv::Mat_<float> floatingPointImage(header.height,
1177  header.width,
1178  reinterpret_cast<float*>(&stereoDisparityImageP->image.data[0]));
1179 
1180  //
1181  // Convert our disparity to floating point by dividing by 16 and
1182  // copy the result to the output message
1183 
1184  floatingPointImage = tmpImage / 16.0;
1185 
1186  stereoDisparityPubP->publish(*stereoDisparityImageP);
1187  }
1188 
1189  camInfoPubP->publish(camInfo);
1190 
1191  break;
1192  }
1193  case Source_Disparity_Cost:
1194 
1195  left_disparity_cost_image_.data.resize(imageSize);
1196  memcpy(&left_disparity_cost_image_.data[0], header.imageDataP, imageSize);
1197 
1198  left_disparity_cost_image_.header.frame_id = frame_id_rectified_left_;
1199  left_disparity_cost_image_.header.stamp = t;
1200  left_disparity_cost_image_.height = header.height;
1201  left_disparity_cost_image_.width = header.width;
1202 
1203  left_disparity_cost_image_.encoding = sensor_msgs::image_encodings::MONO8;
1204  left_disparity_cost_image_.is_bigendian = (htonl(1) == 1);
1205  left_disparity_cost_image_.step = header.width;
1206 
1207  left_disparity_cost_pub_.publish(left_disparity_cost_image_);
1208 
1209  left_cost_cam_info_pub_.publish(stereo_calibration_manager_->leftCameraInfo(frame_id_rectified_left_, t));
1210 
1211  break;
1212  }
1213 }
1214 
1215 void Camera::monoCallback(const image::Header& header)
1216 {
1217  if (Source_Luma_Left != header.source &&
1218  Source_Luma_Right != header.source &&
1219  Source_Luma_Aux != header.source) {
1220 
1221  ROS_ERROR("Camera: unexpected mono image source: 0x%x", header.source);
1222  return;
1223  }
1224 
1225  ros::Time t = ros::Time(header.timeSeconds, 1000 * header.timeMicroSeconds);
1226 
1227  if (!stereo_calibration_manager_)
1228  {
1229  throw std::runtime_error("Uninitialized stereo calibration manager");
1230  }
1231 
1232  switch(header.source) {
1233  case Source_Luma_Left:
1234  {
1235 
1236  left_mono_image_.data.resize(header.imageLength);
1237  memcpy(&left_mono_image_.data[0], header.imageDataP, header.imageLength);
1238 
1239  left_mono_image_.header.frame_id = frame_id_left_;
1240  left_mono_image_.header.stamp = t;
1241  left_mono_image_.height = header.height;
1242  left_mono_image_.width = header.width;
1243 
1244  switch(header.bitsPerPixel) {
1245  case 8:
1246  left_mono_image_.encoding = sensor_msgs::image_encodings::MONO8;
1247  left_mono_image_.step = header.width;
1248  break;
1249  case 16:
1250  left_mono_image_.encoding = sensor_msgs::image_encodings::MONO16;
1251  left_mono_image_.step = header.width * 2;
1252  break;
1253  }
1254 
1255  left_mono_image_.is_bigendian = (htonl(1) == 1);
1256 
1257  left_mono_cam_pub_.publish(left_mono_image_);
1258 
1259  //
1260  // Publish a specific camera info message for the left mono image
1261  left_mono_cam_info_pub_.publish(stereo_calibration_manager_->leftCameraInfo(frame_id_left_, t));
1262 
1263  break;
1264  }
1265  case Source_Luma_Right:
1266  {
1267  right_mono_image_.data.resize(header.imageLength);
1268  memcpy(&right_mono_image_.data[0], header.imageDataP, header.imageLength);
1269 
1270  right_mono_image_.header.frame_id = frame_id_right_;
1271  right_mono_image_.header.stamp = t;
1272  right_mono_image_.height = header.height;
1273  right_mono_image_.width = header.width;
1274 
1275  switch(header.bitsPerPixel) {
1276  case 8:
1277  right_mono_image_.encoding = sensor_msgs::image_encodings::MONO8;
1278  right_mono_image_.step = header.width;
1279  break;
1280  case 16:
1281  right_mono_image_.encoding = sensor_msgs::image_encodings::MONO16;
1282  right_mono_image_.step = header.width * 2;
1283  break;
1284  }
1285  right_mono_image_.is_bigendian = (htonl(1) == 1);
1286 
1287  right_mono_cam_pub_.publish(right_mono_image_);
1288 
1289  //
1290  // Publish a specific camera info message for the right mono image
1291  right_mono_cam_info_pub_.publish(stereo_calibration_manager_->rightCameraInfo(frame_id_right_, t));
1292 
1293  break;
1294  }
1295  case Source_Luma_Aux:
1296  {
1297  aux_mono_image_.data.resize(header.imageLength);
1298  memcpy(&aux_mono_image_.data[0], header.imageDataP, header.imageLength);
1299 
1300  aux_mono_image_.header.frame_id = frame_id_aux_;
1301  aux_mono_image_.header.stamp = t;
1302  aux_mono_image_.height = header.height;
1303  aux_mono_image_.width = header.width;
1304 
1305  switch(header.bitsPerPixel) {
1306  case 8:
1307  aux_mono_image_.encoding = sensor_msgs::image_encodings::MONO8;
1308  aux_mono_image_.step = header.width;
1309  break;
1310  case 16:
1311  aux_mono_image_.encoding = sensor_msgs::image_encodings::MONO16;
1312  aux_mono_image_.step = header.width * 2;
1313  break;
1314  }
1315  aux_mono_image_.is_bigendian = (htonl(1) == 1);
1316 
1317  aux_mono_cam_pub_.publish(aux_mono_image_);
1318 
1319  //
1320  // Publish a specific camera info message for the aux mono image
1321  aux_mono_cam_info_pub_.publish(stereo_calibration_manager_->auxCameraInfo(frame_id_aux_, t, header.width, header.height));
1322  break;
1323  }
1324  }
1325 }
1326 
1327 void Camera::rectCallback(const image::Header& header)
1328 {
1329  if (Source_Luma_Rectified_Left != header.source &&
1330  Source_Luma_Rectified_Right != header.source &&
1331  Source_Luma_Rectified_Aux != header.source) {
1332 
1333  ROS_ERROR("Camera: unexpected rectified image source: 0x%x", header.source);
1334  return;
1335  }
1336 
1337  ros::Time t = ros::Time(header.timeSeconds, 1000 * header.timeMicroSeconds);
1338 
1339  if (!stereo_calibration_manager_)
1340  {
1341  throw std::runtime_error("Uninitialized stereo calibration manager");
1342  }
1343 
1344  switch(header.source) {
1345  case Source_Luma_Rectified_Left:
1346  {
1347 
1348  left_rect_image_.data.resize(header.imageLength);
1349  memcpy(&left_rect_image_.data[0], header.imageDataP, header.imageLength);
1350 
1351  left_rect_image_.header.frame_id = frame_id_rectified_left_;
1352  left_rect_image_.header.stamp = t;
1353  left_rect_image_.height = header.height;
1354  left_rect_image_.width = header.width;
1355 
1356  switch(header.bitsPerPixel) {
1357  case 8:
1358  left_rect_image_.encoding = sensor_msgs::image_encodings::MONO8;
1359  left_rect_image_.step = header.width;
1360 
1361  break;
1362  case 16:
1363  left_rect_image_.encoding = sensor_msgs::image_encodings::MONO16;
1364  left_rect_image_.step = header.width * 2;
1365 
1366  break;
1367  }
1368 
1369  left_rect_image_.is_bigendian = (htonl(1) == 1);
1370 
1371  const auto left_camera_info = stereo_calibration_manager_->leftCameraInfo(frame_id_rectified_left_, t);
1372 
1373  //
1374  // Continue to publish the rect camera info on the
1375  // <namespace>/left/camera_info topic for backward compatibility with
1376  // older versions of the driver
1377  left_rect_cam_pub_.publish(left_rect_image_, left_camera_info);
1378 
1379  left_rect_cam_info_pub_.publish(left_camera_info);
1380 
1381  break;
1382  }
1383  case Source_Luma_Rectified_Right:
1384  {
1385 
1386  right_rect_image_.data.resize(header.imageLength);
1387  memcpy(&right_rect_image_.data[0], header.imageDataP, header.imageLength);
1388 
1389  right_rect_image_.header.frame_id = frame_id_rectified_right_;
1390  right_rect_image_.header.stamp = t;
1391  right_rect_image_.height = header.height;
1392  right_rect_image_.width = header.width;
1393 
1394  switch(header.bitsPerPixel) {
1395  case 8:
1396  right_rect_image_.encoding = sensor_msgs::image_encodings::MONO8;
1397  right_rect_image_.step = header.width;
1398  break;
1399  case 16:
1400  right_rect_image_.encoding = sensor_msgs::image_encodings::MONO16;
1401  right_rect_image_.step = header.width * 2;
1402  break;
1403  }
1404 
1405  right_rect_image_.is_bigendian = (htonl(1) == 1);
1406 
1407  const auto right_camera_info = stereo_calibration_manager_->rightCameraInfo(frame_id_rectified_right_, t);
1408 
1409  //
1410  // Continue to publish the rect camera info on the
1411  // <namespace>/right/camera_info topic for backward compatibility with
1412  // older versions of the driver
1413  right_rect_cam_pub_.publish(right_rect_image_, right_camera_info);
1414 
1415  right_rect_cam_info_pub_.publish(right_camera_info);
1416 
1417  break;
1418  }
1419  case Source_Luma_Rectified_Aux:
1420  {
1421 
1422  aux_rect_image_.data.resize(header.imageLength);
1423  memcpy(&aux_rect_image_.data[0], header.imageDataP, header.imageLength);
1424 
1425  aux_rect_image_.header.frame_id = frame_id_rectified_aux_;
1426  aux_rect_image_.header.stamp = t;
1427  aux_rect_image_.height = header.height;
1428  aux_rect_image_.width = header.width;
1429 
1430  switch(header.bitsPerPixel) {
1431  case 8:
1432  aux_rect_image_.encoding = sensor_msgs::image_encodings::MONO8;
1433  aux_rect_image_.step = header.width;
1434  break;
1435  case 16:
1436  aux_rect_image_.encoding = sensor_msgs::image_encodings::MONO16;
1437  aux_rect_image_.step = header.width * 2;
1438  break;
1439  }
1440 
1441  aux_rect_image_.is_bigendian = (htonl(1) == 1);
1442 
1443  const auto aux_camera_info = stereo_calibration_manager_->auxCameraInfo(frame_id_rectified_aux_, t, header.width, header.height);
1444 
1445  //
1446  // Continue to publish the rect camera info on the
1447  // <namespace>/aux/camera_info topic for backward compatibility with
1448  // older versions of the driver
1449  aux_rect_cam_pub_.publish(aux_rect_image_, aux_camera_info);
1450 
1451  aux_rect_cam_info_pub_.publish(aux_camera_info);
1452 
1453  break;
1454  }
1455  }
1456 }
1457 
1458 void Camera::depthCallback(const image::Header& header)
1459 {
1460  if (Source_Disparity != header.source) {
1461 
1462  ROS_ERROR("Camera: unexpected depth image source: 0x%x", header.source);
1463  return;
1464  }
1465 
1466  uint32_t niDepthSubscribers = ni_depth_cam_pub_.getNumSubscribers();
1467  uint32_t depthSubscribers = depth_cam_pub_.getNumSubscribers();
1468 
1469  if (0 == niDepthSubscribers && 0 == depthSubscribers)
1470  {
1471  return;
1472  }
1473 
1474  const ros::Time t(header.timeSeconds, 1000 * header.timeMicroSeconds);
1475 
1476  if (!stereo_calibration_manager_)
1477  {
1478  throw std::runtime_error("Uninitialized stereo calibration manager");
1479  }
1480 
1481  const float bad_point = std::numeric_limits<float>::quiet_NaN();
1482  const uint32_t depthSize = header.height * header.width * sizeof(float);
1483  const uint32_t niDepthSize = header.height * header.width * sizeof(uint16_t);
1484  const uint32_t imageSize = header.width * header.height;
1485 
1486  depth_image_.header.stamp = t;
1487  depth_image_.header.frame_id = frame_id_rectified_left_;
1488  depth_image_.height = header.height;
1489  depth_image_.width = header.width;
1490  depth_image_.is_bigendian = (htonl(1) == 1);
1491 
1492  ni_depth_image_ = depth_image_;
1493 
1494  ni_depth_image_.encoding = sensor_msgs::image_encodings::MONO16;
1495  ni_depth_image_.step = header.width * 2;
1496 
1497  depth_image_.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
1498  depth_image_.step = header.width * 4;
1499 
1500  depth_image_.data.resize(depthSize);
1501  ni_depth_image_.data.resize(niDepthSize);
1502 
1503  float *depthImageP = reinterpret_cast<float*>(&depth_image_.data[0]);
1504  uint16_t *niDepthImageP = reinterpret_cast<uint16_t*>(&ni_depth_image_.data[0]);
1505 
1506  const uint16_t min_ni_depth = std::numeric_limits<uint16_t>::lowest();
1507  const uint16_t max_ni_depth = std::numeric_limits<uint16_t>::max();
1508 
1509  // disparity conversion requires valid right image
1510  if (!stereo_calibration_manager_->validRight())
1511  {
1512  throw std::runtime_error("Stereo calibration manager missing right calibration");
1513  }
1514 
1515  //
1516  // Disparity is in 32-bit floating point
1517 
1518  if (32 == header.bitsPerPixel) {
1519 
1520  //
1521  // Depth = focal_length*baseline/disparity
1522  // From the Q matrix used to reproject disparity images using non-isotropic
1523  // pixels we see that z = (fx*fy*Tx). Normalizing z so that
1524  // the scale factor on the homogeneous Cartesian coordinate is 1 results
1525  // in z = (fx*fy*Tx)/(-fy*d) or z = (fx*Tx)/(-d).
1526  // The 4th element of the right camera projection matrix is defined
1527  // as fx*Tx.
1528 
1529  const double scale = stereo_calibration_manager_->rightCameraInfo(frame_id_right_, t).P[3];
1530 
1531  const float *disparityImageP = reinterpret_cast<const float*>(header.imageDataP);
1532 
1533  for (uint32_t i = 0 ; i < imageSize ; ++i)
1534  {
1535  if (0.0 >= disparityImageP[i])
1536  {
1537  depthImageP[i] = bad_point;
1538  niDepthImageP[i] = 0;
1539  }
1540  else
1541  {
1542  depthImageP[i] = scale / disparityImageP[i];
1543  niDepthImageP[i] = static_cast<uint16_t>(std::min(static_cast<float>(max_ni_depth),
1544  std::max(static_cast<float>(min_ni_depth),
1545  depthImageP[i] * 1000)));
1546  }
1547  }
1548 
1549  //
1550  // Disparity is in 1/16th pixel, unsigned integer
1551 
1552  } else if (16 == header.bitsPerPixel) {
1553 
1554  //
1555  // Depth = focal_length*baseline/disparity
1556  // From the Q matrix used to reproject disparity images using non-isotropic
1557  // pixels we see that z = (fx*fy*Tx). Normalizing z so that
1558  // the scale factor on the homogeneous Cartesian coordinate is 1 results
1559  // in z = (fx*fy*Tx)/(-fy*d) or z = (fx*Tx)/(-d). Because our disparity
1560  // image is 16 bits we must also divide by 16 making z = (fx*Tx*16)/(-d)
1561  // The 4th element of the right camera projection matrix is defined
1562  // as fx*Tx.
1563 
1564 
1565  const float scale = stereo_calibration_manager_->rightCameraInfo(frame_id_right_, t).P[3] * -16.0f;
1566 
1567  const uint16_t *disparityImageP = reinterpret_cast<const uint16_t*>(header.imageDataP);
1568 
1569  for (uint32_t i = 0 ; i < imageSize ; ++i)
1570  {
1571  if (0 == disparityImageP[i])
1572  {
1573  depthImageP[i] = bad_point;
1574  niDepthImageP[i] = 0;
1575  }
1576  else
1577  {
1578  depthImageP[i] = scale / disparityImageP[i];
1579  niDepthImageP[i] = static_cast<uint16_t>(std::min(static_cast<float>(max_ni_depth),
1580  std::max(static_cast<float>(min_ni_depth),
1581  depthImageP[i] * 1000)));
1582  }
1583  }
1584 
1585  } else {
1586  ROS_ERROR("Camera: unsupported disparity bpp: %d", header.bitsPerPixel);
1587  return;
1588  }
1589 
1590  if (0 != niDepthSubscribers)
1591  {
1592  ni_depth_cam_pub_.publish(ni_depth_image_);
1593  }
1594 
1595  if (0 != depthSubscribers)
1596  {
1597  depth_cam_pub_.publish(depth_image_);
1598  }
1599 
1600  depth_cam_info_pub_.publish(stereo_calibration_manager_->leftCameraInfo(frame_id_rectified_left_, t));
1601 }
1602 
1603 void Camera::pointCloudCallback(const image::Header& header)
1604 {
1605  if (Source_Disparity != header.source) {
1606 
1607  ROS_ERROR("Camera: unexpected pointcloud image source: 0x%x", header.source);
1608  return;
1609  }
1610 
1611  if (!stereo_calibration_manager_)
1612  {
1613  throw std::runtime_error("Uninitialized stereo calibration manager");
1614  }
1615 
1616  //
1617  // Get the corresponding visual images so we can colorize properly
1618 
1619  std::shared_ptr<BufferWrapper<image::Header>> left_luma_rect = nullptr;
1620  std::shared_ptr<BufferWrapper<image::Header>> left_luma = nullptr;
1621  std::shared_ptr<BufferWrapper<image::Header>> left_chroma = nullptr;
1622  std::shared_ptr<BufferWrapper<image::Header>> aux_luma_rectified = nullptr;
1623  std::shared_ptr<BufferWrapper<image::Header>> aux_chroma_rectified = nullptr;
1624 
1625  const auto left_rect_image = image_buffers_.find(Source_Luma_Rectified_Left);
1626  if (left_rect_image != std::end(image_buffers_) && left_rect_image->second->data().frameId == header.frameId) {
1627  left_luma_rect = left_rect_image->second;
1628  }
1629 
1630  const auto left_luma_image = image_buffers_.find(Source_Luma_Left);
1631  if (left_luma_image != std::end(image_buffers_) && left_luma_image->second->data().frameId == header.frameId) {
1632  left_luma = left_luma_image->second;
1633  }
1634 
1635  const auto chroma_image = image_buffers_.find(Source_Chroma_Left);
1636  if (chroma_image != std::end(image_buffers_) && chroma_image->second->data().frameId == header.frameId) {
1637  left_chroma = chroma_image->second;
1638  }
1639 
1640  const auto aux_luma_rectified_image = image_buffers_.find(Source_Luma_Rectified_Aux);
1641  if (aux_luma_rectified_image != std::end(image_buffers_) && aux_luma_rectified_image->second->data().frameId == header.frameId) {
1642  aux_luma_rectified = aux_luma_rectified_image->second;
1643  }
1644 
1645  const auto aux_chroma_rectified_image = image_buffers_.find(Source_Chroma_Rectified_Aux);
1646  if (aux_chroma_rectified_image != std::end(image_buffers_) && aux_chroma_rectified_image->second->data().frameId == header.frameId) {
1647  aux_chroma_rectified = aux_chroma_rectified_image->second;
1648  }
1649 
1650  const bool color_data = (has_aux_camera_ && aux_luma_rectified && aux_chroma_rectified && stereo_calibration_manager_->validAux()) ||
1651  (!has_aux_camera_ && left_luma && left_chroma);
1652 
1653  const bool pub_pointcloud = luma_point_cloud_pub_.getNumSubscribers() > 0 && left_luma_rect;
1654  const bool pub_color_pointcloud = color_point_cloud_pub_.getNumSubscribers() > 0 && color_data;
1655  const bool pub_organized_pointcloud = luma_organized_point_cloud_pub_.getNumSubscribers() > 0 && left_luma_rect;
1656  const bool pub_color_organized_pointcloud = color_organized_point_cloud_pub_.getNumSubscribers() > 0 && color_data;
1657 
1658  if (!(pub_pointcloud || pub_color_pointcloud || pub_organized_pointcloud || pub_color_organized_pointcloud))
1659  {
1660  return;
1661  }
1662 
1663  const ros::Time t(header.timeSeconds, 1000 * header.timeMicroSeconds);
1664 
1665  //
1666  // Resize our corresponding pointclouds if we plan on publishing them
1667 
1668  if (pub_pointcloud)
1669  {
1670  luma_point_cloud_.header.stamp = t;
1671  luma_point_cloud_.data.resize(header.width * header.height * luma_point_cloud_.point_step);
1672  }
1673 
1674  if (pub_color_pointcloud)
1675  {
1676  color_point_cloud_.header.stamp = t;
1677  color_point_cloud_.data.resize(header.width * header.height * color_point_cloud_.point_step);
1678  }
1679 
1680  if (pub_organized_pointcloud)
1681  {
1682  luma_organized_point_cloud_.header.stamp = t;
1683  luma_organized_point_cloud_.data.resize(header.width * header.height * luma_organized_point_cloud_.point_step);
1684  luma_organized_point_cloud_.width = header.width;
1685  luma_organized_point_cloud_.height = header.height;
1686  luma_organized_point_cloud_.row_step = header.width * luma_organized_point_cloud_.point_step;
1687  }
1688 
1689  if (pub_color_organized_pointcloud)
1690  {
1691  color_organized_point_cloud_.header.stamp = t;
1692  color_organized_point_cloud_.data.resize(header.width * header.height * color_organized_point_cloud_.point_step);
1693  color_organized_point_cloud_.width = header.width;
1694  color_organized_point_cloud_.height = header.height;
1695  color_organized_point_cloud_.row_step = header.width * color_organized_point_cloud_.point_step;
1696  }
1697 
1698  const Eigen::Vector3f invalid_point(std::numeric_limits<float>::quiet_NaN(),
1699  std::numeric_limits<float>::quiet_NaN(),
1700  std::numeric_limits<float>::quiet_NaN());
1701 
1702  //
1703  // Create rectified color image upfront if we are planning to publish color pointclouds
1704 
1705  cv::Mat rectified_color;
1706  if (!has_aux_camera_ && (pub_color_pointcloud || pub_color_organized_pointcloud))
1707  {
1708  const auto &luma = left_luma->data();
1709 
1710  pointcloud_color_buffer_.resize(3 * luma.width * luma.height);
1711  pointcloud_rect_color_buffer_.resize(3 * luma.width * luma.height);
1712  ycbcrToBgr(luma, left_chroma->data(), &(pointcloud_color_buffer_[0]));
1713 
1714  cv::Mat rgb_image(luma.height, luma.width, CV_8UC3, &(pointcloud_color_buffer_[0]));
1715  cv::Mat rect_rgb_image(luma.height, luma.width, CV_8UC3, &(pointcloud_rect_color_buffer_[0]));
1716 
1717  const auto left_remap = stereo_calibration_manager_->leftRemap();
1718 
1719  cv::remap(rgb_image, rect_rgb_image, left_remap->map1, left_remap->map2, cv::INTER_LINEAR);
1720 
1721  rectified_color = std::move(rect_rgb_image);
1722  }
1723  else if(has_aux_camera_ && (pub_color_pointcloud || pub_color_organized_pointcloud))
1724  {
1725  const auto &luma = aux_luma_rectified->data();
1726 
1727  pointcloud_rect_color_buffer_.resize(3 * luma.width * luma.height);
1728 
1729  ycbcrToBgr(luma, aux_chroma_rectified->data(), reinterpret_cast<uint8_t*>(&(pointcloud_rect_color_buffer_[0])));
1730 
1731  cv::Mat rect_rgb_image(luma.height, luma.width, CV_8UC3, &(pointcloud_rect_color_buffer_[0]));
1732 
1733  //
1734  // If we are running in full-aux mode and 1/4 res mode resize the rectified aux image to match the
1735  // disparity resolution
1736 
1737  if (stereo_calibration_manager_->config().cameraProfile() == Full_Res_Aux_Cam &&
1738  (header.width != luma.width || header.height != luma.height))
1739  {
1740  cv::Mat resized_rect_rgb_image;
1741  cv::resize(rect_rgb_image,
1742  resized_rect_rgb_image,
1743  cv::Size{static_cast<int>(header.width), static_cast<int>(header.height)},
1744  0, 0, cv::INTER_AREA);
1745 
1746  rect_rgb_image = std::move(resized_rect_rgb_image);
1747  }
1748 
1749  rectified_color = std::move(rect_rgb_image);
1750  }
1751 
1752  // disparity conversion requires valid right image
1753  if (!stereo_calibration_manager_->validRight())
1754  {
1755  throw std::runtime_error("Stereo calibration manager missing right calibration");
1756  }
1757 
1758  const auto left_camera_info = stereo_calibration_manager_->leftCameraInfo(frame_id_left_, t);
1759  const auto right_camera_info = stereo_calibration_manager_->rightCameraInfo(frame_id_right_, t);
1760  const auto aux_camera_info = stereo_calibration_manager_->auxCameraInfo(frame_id_rectified_aux_, t,
1761  rectified_color.cols, rectified_color.rows);
1762 
1763  //
1764  // Iterate through our disparity image once populating our pointcloud structures if we plan to publish them
1765 
1766  uint32_t packed_color = 0;
1767 
1768  const float squared_max_range = pointcloud_max_range_ * pointcloud_max_range_;
1769 
1770  size_t valid_points = 0;
1771  for (size_t y = 0 ; y < header.height ; ++y)
1772  {
1773  for (size_t x = 0 ; x < header.width ; ++x)
1774  {
1775  const size_t index = y * header.width + x;
1776 
1777  float disparity = 0.0f;
1778  switch(header.bitsPerPixel)
1779  {
1780  case 16:
1781  {
1782  disparity = static_cast<float>(reinterpret_cast<const uint16_t*>(header.imageDataP)[index]) / 16.0f;
1783  break;
1784  }
1785  case 32:
1786  {
1787  disparity = static_cast<float>(reinterpret_cast<const float*>(header.imageDataP)[index]);
1788  break;
1789  }
1790  default:
1791  {
1792  ROS_ERROR("Camera: unsupported disparity detph: %d", header.bitsPerPixel);
1793  return;
1794  }
1795  }
1796 
1797  const Eigen::Vector3f point = stereo_calibration_manager_->reproject(x, y, disparity,
1798  left_camera_info, right_camera_info);
1799 
1800  //
1801  // We have a valid rectified color image meaning we plan to publish color pointcloud topics. Assemble the
1802  // color pixel here since it may be needed in our organized pointclouds
1803 
1804  if (!rectified_color.empty())
1805  {
1806  packed_color = 0;
1807 
1808  const auto color_pixel = (has_aux_camera_ && disparity != 0.0) ?
1809  interpolate_color(stereo_calibration_manager_->rectifiedAuxProject(point, aux_camera_info),
1810  rectified_color) :
1811  rectified_color.at<cv::Vec3b>(y, x);
1812 
1813 
1814  packed_color |= color_pixel[2] << 16 | color_pixel[1] << 8 | color_pixel[0];
1815  }
1816 
1817  //
1818  // If our disparity is 0 pixels our corresponding 3D point is infinite. If we plan to publish organized
1819  // pointclouds we will need to add a invalid point to our pointcloud(s)
1820 
1821  if (disparity == 0.0 || clipPoint(border_clip_type_, border_clip_value_, header.width, header.height, x, y))
1822  {
1823  if (pub_organized_pointcloud)
1824  {
1825  writePoint(luma_organized_point_cloud_, index, invalid_point, index, left_luma_rect->data());
1826  }
1827 
1828  if (pub_color_organized_pointcloud)
1829  {
1830  writePoint(color_organized_point_cloud_, index, invalid_point, packed_color);
1831  }
1832 
1833  continue;
1834  }
1835 
1836  const bool valid = isValidReprojectedPoint(point, squared_max_range);
1837 
1838  if (pub_pointcloud && valid)
1839  {
1840  writePoint(luma_point_cloud_, valid_points, point, index, left_luma_rect->data());
1841  }
1842 
1843  if(pub_color_pointcloud && valid)
1844  {
1845  writePoint(color_point_cloud_, valid_points, point, packed_color);
1846  }
1847 
1848  if (pub_organized_pointcloud)
1849  {
1850  writePoint(luma_organized_point_cloud_, index, valid ? point : invalid_point, index, left_luma_rect->data());
1851  }
1852 
1853  if (pub_color_organized_pointcloud)
1854  {
1855  writePoint(color_organized_point_cloud_, index, valid ? point : invalid_point, packed_color);
1856  }
1857 
1858  if (valid)
1859  {
1860  ++valid_points;
1861  }
1862  }
1863  }
1864 
1865  if (pub_pointcloud)
1866  {
1867  luma_point_cloud_.height = 1;
1868  luma_point_cloud_.row_step = valid_points * luma_point_cloud_.point_step;
1869  luma_point_cloud_.width = valid_points;
1870  luma_point_cloud_.data.resize(valid_points * luma_point_cloud_.point_step);
1871  luma_point_cloud_pub_.publish(luma_point_cloud_);
1872  }
1873 
1874  if(pub_color_pointcloud)
1875  {
1876  color_point_cloud_.height = 1;
1877  color_point_cloud_.row_step = valid_points * color_point_cloud_.point_step;
1878  color_point_cloud_.width = valid_points;
1879  color_point_cloud_.data.resize(valid_points * color_point_cloud_.point_step);
1880  color_point_cloud_pub_.publish(color_point_cloud_);
1881  }
1882 
1883  if (pub_organized_pointcloud)
1884  {
1885  luma_organized_point_cloud_pub_.publish(luma_organized_point_cloud_);
1886  }
1887 
1888  if (pub_color_organized_pointcloud)
1889  {
1890  color_organized_point_cloud_pub_.publish(color_organized_point_cloud_);
1891  }
1892 
1893 }
1894 
1895 void Camera::rawCamDataCallback(const image::Header& header)
1896 {
1897  if (0 == raw_cam_data_pub_.getNumSubscribers()) {
1898  return;
1899  }
1900 
1901  if(Source_Disparity == header.source)
1902  {
1903  const auto image = image_buffers_.find(Source_Luma_Rectified_Left);
1904  if (image != std::end(image_buffers_) && image->second->data().frameId == header.frameId)
1905  {
1906  const auto luma_ptr = image->second;
1907  const auto &left_luma_rect = luma_ptr->data();
1908 
1909  const uint32_t left_luma_image_size = left_luma_rect.width * left_luma_rect.height;
1910 
1911  raw_cam_data_.gray_scale_image.resize(left_luma_image_size);
1912  memcpy(&(raw_cam_data_.gray_scale_image[0]),
1913  left_luma_rect.imageDataP,
1914  left_luma_image_size * sizeof(uint8_t));
1915 
1916  raw_cam_data_.frames_per_second = left_luma_rect.framesPerSecond;
1917  raw_cam_data_.gain = left_luma_rect.gain;
1918  raw_cam_data_.exposure_time = left_luma_rect.exposure;
1919  raw_cam_data_.frame_count = left_luma_rect.frameId;
1920  raw_cam_data_.time_stamp = ros::Time(left_luma_rect.timeSeconds, 1000 * left_luma_rect.timeMicroSeconds);
1921  raw_cam_data_.width = left_luma_rect.width;
1922  raw_cam_data_.height = left_luma_rect.height;
1923 
1924  const uint32_t disparity_size = header.width * header.height;
1925 
1926  raw_cam_data_.disparity_image.resize(disparity_size);
1927  memcpy(&(raw_cam_data_.disparity_image[0]),
1928  header.imageDataP,
1929  disparity_size * header.bitsPerPixel == 16 ? sizeof(uint16_t) : sizeof(uint32_t));
1930 
1931  raw_cam_data_pub_.publish(raw_cam_data_);
1932  }
1933  }
1934 }
1935 
1936 void Camera::colorImageCallback(const image::Header& header)
1937 {
1938  //
1939  // The left-luma image is currently published before the matching chroma image so this can just trigger on that
1940 
1941  if (Source_Chroma_Left != header.source &&
1942  Source_Chroma_Rectified_Aux != header.source &&
1943  Source_Chroma_Aux != header.source)
1944  {
1945  ROS_WARN("Camera: unexpected color image source: 0x%x", header.source);
1946  return;
1947  }
1948 
1949  const ros::Time t(header.timeSeconds, 1000 * header.timeMicroSeconds);
1950 
1951  if (!stereo_calibration_manager_)
1952  {
1953  throw std::runtime_error("Uninitialized stereo calibration manager");
1954  }
1955 
1956  switch (header.source)
1957  {
1958  case Source_Chroma_Left:
1959  {
1960 
1961  const auto color_subscribers = left_rgb_cam_pub_.getNumSubscribers();
1962  const auto color_rect_subscribers = left_rgb_rect_cam_pub_.getNumSubscribers();
1963 
1964  if (color_subscribers == 0 && color_rect_subscribers == 0)
1965  {
1966  return;
1967  }
1968 
1969  const auto left_luma = image_buffers_.find(Source_Luma_Left);
1970  if (left_luma == std::end(image_buffers_)) {
1971  return;
1972  }
1973 
1974  const auto luma_ptr = left_luma->second;
1975 
1976  if (header.frameId == luma_ptr->data().frameId) {
1977 
1978  const uint32_t height = luma_ptr->data().height;
1979  const uint32_t width = luma_ptr->data().width;
1980  const uint32_t imageSize = 3 * height * width;
1981 
1982  left_rgb_image_.data.resize(imageSize);
1983 
1984  left_rgb_image_.header.frame_id = frame_id_left_;
1985  left_rgb_image_.header.stamp = t;
1986  left_rgb_image_.height = height;
1987  left_rgb_image_.width = width;
1988 
1989  left_rgb_image_.encoding = sensor_msgs::image_encodings::BGR8;
1990  left_rgb_image_.is_bigendian = (htonl(1) == 1);
1991  left_rgb_image_.step = 3 * width;
1992 
1993  //
1994  // Convert YCbCr 4:2:0 to RGB
1995 
1996  ycbcrToBgr(luma_ptr->data(), header, reinterpret_cast<uint8_t*>(&(left_rgb_image_.data[0])));
1997 
1998  const auto left_camera_info = stereo_calibration_manager_->leftCameraInfo(frame_id_left_, t);
1999 
2000  if (color_subscribers != 0) {
2001  left_rgb_cam_pub_.publish(left_rgb_image_);
2002 
2003  left_rgb_cam_info_pub_.publish(left_camera_info);
2004  }
2005 
2006  if (color_rect_subscribers > 0) {
2007  left_rgb_rect_image_.data.resize(imageSize);
2008 
2009  const auto left_rectified_camera_info = stereo_calibration_manager_->leftCameraInfo(frame_id_rectified_left_, t);
2010 
2011  const auto remaps = stereo_calibration_manager_->leftRemap();
2012 
2013  const cv::Mat rgb_image(height, width, CV_8UC3, &(left_rgb_image_.data[0]));
2014  cv::Mat rect_rgb_image(height, width, CV_8UC3, &(left_rgb_rect_image_.data[0]));
2015 
2016  cv::remap(rgb_image, rect_rgb_image, remaps->map1, remaps->map2, cv::INTER_LINEAR);
2017 
2018  left_rgb_rect_image_.header.frame_id = frame_id_rectified_left_;
2019  left_rgb_rect_image_.header.stamp = t;
2020  left_rgb_rect_image_.height = height;
2021  left_rgb_rect_image_.width = width;
2022 
2023  left_rgb_rect_image_.encoding = sensor_msgs::image_encodings::BGR8;
2024  left_rgb_rect_image_.is_bigendian = (htonl(1) == 1);
2025  left_rgb_rect_image_.step = 3 * width;
2026 
2027  left_rgb_rect_cam_pub_.publish(left_rgb_rect_image_, left_rectified_camera_info);
2028 
2029  left_rgb_rect_cam_info_pub_.publish(left_rectified_camera_info);
2030  }
2031  }
2032 
2033  break;
2034  }
2035  case Source_Chroma_Rectified_Aux:
2036  {
2037  if (aux_rgb_rect_cam_pub_.getNumSubscribers() == 0) {
2038  return;
2039  }
2040 
2041  const auto aux_luma = image_buffers_.find(Source_Luma_Rectified_Aux);
2042  if (aux_luma == std::end(image_buffers_)) {
2043  return;
2044  }
2045 
2046  const auto luma_ptr = aux_luma->second;
2047 
2048  if (header.frameId == luma_ptr->data().frameId) {
2049 
2050  const uint32_t height = luma_ptr->data().height;
2051  const uint32_t width = luma_ptr->data().width;
2052  const uint32_t imageSize = 3 * height * width;
2053 
2054  aux_rgb_rect_image_.data.resize(imageSize);
2055 
2056  aux_rgb_rect_image_.header.frame_id = frame_id_rectified_aux_;
2057  aux_rgb_rect_image_.header.stamp = t;
2058  aux_rgb_rect_image_.height = height;
2059  aux_rgb_rect_image_.width = width;
2060 
2061  aux_rgb_rect_image_.encoding = sensor_msgs::image_encodings::BGR8;
2062  aux_rgb_rect_image_.is_bigendian = (htonl(1) == 1);
2063  aux_rgb_rect_image_.step = 3 * width;
2064 
2065  //
2066  // Convert YCbCr 4:2:0 to RGB
2067 
2068  ycbcrToBgr(luma_ptr->data(), header, reinterpret_cast<uint8_t*>(&(aux_rgb_rect_image_.data[0])));
2069 
2070  const auto aux_camera_info = stereo_calibration_manager_->auxCameraInfo(frame_id_rectified_aux_, t, width, height);
2071 
2072  aux_rgb_rect_cam_pub_.publish(aux_rgb_rect_image_, aux_camera_info);
2073 
2074  aux_rgb_rect_cam_info_pub_.publish(aux_camera_info);
2075  }
2076 
2077  break;
2078  }
2079  case Source_Chroma_Aux:
2080  {
2081  if (aux_rgb_cam_pub_.getNumSubscribers() == 0) {
2082  return;
2083  }
2084 
2085  const auto aux_luma = image_buffers_.find(Source_Luma_Aux);
2086  if (aux_luma == std::end(image_buffers_)) {
2087  return;
2088  }
2089 
2090  const auto luma_ptr = aux_luma->second;
2091 
2092  if (header.frameId == luma_ptr->data().frameId) {
2093  const uint32_t height = luma_ptr->data().height;
2094  const uint32_t width = luma_ptr->data().width;
2095  const uint32_t imageSize = 3 * height * width;
2096 
2097  aux_rgb_image_.data.resize(imageSize);
2098 
2099  aux_rgb_image_.header.frame_id = frame_id_aux_;
2100  aux_rgb_image_.header.stamp = t;
2101  aux_rgb_image_.height = height;
2102  aux_rgb_image_.width = width;
2103 
2104  aux_rgb_image_.encoding = sensor_msgs::image_encodings::BGR8;
2105  aux_rgb_image_.is_bigendian = (htonl(1) == 1);
2106  aux_rgb_image_.step = 3 * width;
2107 
2108  //
2109  // Convert YCbCr 4:2:0 to RGB
2110 
2111  ycbcrToBgr(luma_ptr->data(), header, reinterpret_cast<uint8_t*>(&(aux_rgb_image_.data[0])));
2112 
2113  const auto aux_camera_info = stereo_calibration_manager_->auxCameraInfo(frame_id_aux_, t, width, height);
2114 
2115  aux_rgb_cam_pub_.publish(aux_rgb_image_);
2116 
2117  aux_rgb_cam_info_pub_.publish(aux_camera_info);
2118 
2119  break;
2120  }
2121  }
2122  }
2123 }
2124 
2125 void Camera::colorizeCallback(const image::Header& header)
2126 {
2127  if (Source_Luma_Rectified_Aux != header.source &&
2128  Source_Chroma_Rectified_Aux != header.source &&
2129  Source_Luma_Aux != header.source &&
2130  Source_Luma_Left != header.source &&
2131  Source_Chroma_Left != header.source &&
2132  Source_Luma_Rectified_Left != header.source) {
2133  ROS_WARN("Camera: unexpected colorized image source: 0x%x", header.source);
2134  return;
2135  }
2136 
2137  image_buffers_[header.source] = std::make_shared<BufferWrapper<crl::multisense::image::Header>>(driver_, header);
2138 }
2139 
2140 void Camera::groundSurfaceCallback(const image::Header& header)
2141 {
2142  if (Source_Ground_Surface_Class_Image != header.source)
2143  {
2144  ROS_WARN("Camera: unexpected image source: 0x%x", header.source);
2145  return;
2146  }
2147 
2148  const ros::Time t(header.timeSeconds, 1000 * header.timeMicroSeconds);
2149 
2150  if (!stereo_calibration_manager_)
2151  {
2152  throw std::runtime_error("Uninitialized stereo calibration manager");
2153  }
2154 
2155  switch (header.source)
2156  {
2157  case Source_Ground_Surface_Class_Image:
2158  {
2159  const auto ground_surface_subscribers = ground_surface_cam_pub_.getNumSubscribers();
2160 
2161  if (ground_surface_subscribers == 0)
2162  return;
2163 
2164  const uint32_t height = header.height;
2165  const uint32_t width = header.width;
2166  const uint32_t imageSize = 3 * height * width;
2167 
2168  ground_surface_image_.data.resize(imageSize);
2169 
2170  ground_surface_image_.header.frame_id = frame_id_rectified_left_;
2171  ground_surface_image_.header.stamp = t;
2172  ground_surface_image_.height = height;
2173  ground_surface_image_.width = width;
2174 
2175  ground_surface_image_.encoding = sensor_msgs::image_encodings::RGB8;
2176  ground_surface_image_.is_bigendian = (htonl(1) == 1);
2177  ground_surface_image_.step = 3* width;
2178 
2179  // Get pointer to output image
2180  uint8_t* output = reinterpret_cast<uint8_t*>(&(ground_surface_image_.data[0]));
2181 
2182  // Colorize image with classes
2183  const size_t rgb_stride = ground_surface_image_.width * 3;
2184 
2185  for(uint32_t y = 0; y < ground_surface_image_.height; ++y)
2186  {
2187  const size_t row_offset = y * rgb_stride;
2188 
2189  for(uint32_t x = 0; x < ground_surface_image_.width; ++x)
2190  {
2191  const uint8_t *imageP = reinterpret_cast<const uint8_t*>(header.imageDataP);
2192 
2193  const size_t image_offset = (y * ground_surface_image_.width) + x;
2194  memcpy(output + row_offset + (3 * x), ground_surface_utilities::groundSurfaceClassToPixelColor(imageP[image_offset]).data(), 3);
2195  }
2196  }
2197 
2198  ground_surface_cam_pub_.publish(ground_surface_image_);
2199 
2200  // Publish info
2201  const auto ground_surface_info = stereo_calibration_manager_->leftCameraInfo(frame_id_rectified_left_, t);
2202  ground_surface_info_pub_.publish(ground_surface_info);
2203 
2204  break;
2205  }
2206  }
2207 }
2208 
2209 void Camera::groundSurfaceSplineCallback(const ground_surface::Header& header)
2210 {
2211  if (header.controlPointsBitsPerPixel != 32)
2212  {
2213  ROS_WARN("Expecting floats for spline control points, got %u bits per pixel instead", header.controlPointsBitsPerPixel);
2214  return;
2215  }
2216 
2217  if (!header.success)
2218  {
2219  ROS_WARN("Ground surface modelling failed, consider modifying camera extrinsics and/or algorithm parameters");
2220  return;
2221  }
2222 
2223  if (!stereo_calibration_manager_)
2224  {
2225  throw std::runtime_error("Uninitialized stereo calibration manager");
2226  }
2227 
2228  // Convert row spline control point data to eigen matrix
2229  Eigen::Map<const Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> controlGrid(
2230  reinterpret_cast<const float*>(header.controlPointsImageDataP), header.controlPointsHeight, header.controlPointsWidth);
2231 
2232  // Calculate frustum azimuth angles
2233  const auto config = stereo_calibration_manager_->config();
2234 
2235  float minMaxAzimuthAngle[2];
2236  minMaxAzimuthAngle[0] = M_PI_2 - atan(config.cx() / config.fx());
2237  minMaxAzimuthAngle[1] = M_PI_2 + atan((config.width() - config.cx()) / config.fx());
2238 
2239  // Generate pointcloud for visualization
2240  auto eigen_pcl = ground_surface_utilities::convertSplineToPointcloud(
2241  controlGrid,
2242  spline_draw_params_,
2243  pointcloud_max_range_,
2244  header.xzCellOrigin,
2245  header.xzCellSize,
2246  minMaxAzimuthAngle,
2247  header.extrinsics,
2248  header.quadraticParams,
2249  config.tx());
2250 
2251  // Send pointcloud message
2252  ground_surface_spline_pub_.publish(ground_surface_utilities::eigenToPointcloud(eigen_pcl, frame_id_origin_));
2253 }
2254 
2255 void Camera::updateConfig(const image::Config& config)
2256 {
2257  if (!stereo_calibration_manager_)
2258  {
2259  throw std::runtime_error("Uninitialized stereo calibration manager");
2260  }
2261 
2262  stereo_calibration_manager_->updateConfig(config);
2263 
2264  //
2265  // Publish the "raw" config message
2266 
2267  multisense_ros::RawCamConfig cfg;
2268 
2269  cfg.width = config.width();
2270  cfg.height = config.height();
2271  cfg.frames_per_second = config.fps();
2272  cfg.gain = config.gain();
2273  cfg.exposure_time = config.exposure();
2274 
2275  cfg.fx = config.fx();
2276  cfg.fy = config.fy();
2277  cfg.cx = config.cx();
2278  cfg.cy = config.cy();
2279  cfg.tx = config.tx();
2280  cfg.ty = config.ty();
2281  cfg.tz = config.tz();
2282  cfg.roll = config.roll();
2283  cfg.pitch = config.pitch();
2284  cfg.yaw = config.yaw();
2285 
2286  raw_cam_config_pub_.publish(cfg);
2287 
2288  //
2289  // Republish our camera info topics since the resolution changed
2290 
2291  publishAllCameraInfo();
2292 }
2293 
2294 void Camera::publishAllCameraInfo()
2295 {
2296  const auto stamp = ros::Time::now();
2297 
2298  if (!stereo_calibration_manager_)
2299  {
2300  throw std::runtime_error("Uninitialized stereo calibration manager");
2301  }
2302 
2303  const auto left_camera_info = stereo_calibration_manager_->leftCameraInfo(frame_id_left_, stamp);
2304  const auto right_camera_info = stereo_calibration_manager_->rightCameraInfo(frame_id_right_, stamp);
2305 
2306  const auto left_rectified_camera_info = stereo_calibration_manager_->leftCameraInfo(frame_id_rectified_left_, stamp);
2307  const auto right_rectified_camera_info = stereo_calibration_manager_->rightCameraInfo(frame_id_rectified_right_, stamp);
2308 
2309  //
2310  // Republish camera info messages outside of image callbacks.
2311  // The camera info publishers are latching so the messages
2312  // will persist until a new message is published in one of the image
2313  // callbacks. This makes it easier when a user is trying access a camera_info
2314  // for a topic which they are not subscribed to
2315 
2316  if (system::DeviceInfo::HARDWARE_REV_BCAM == device_info_.hardwareRevision) {
2317 
2318  left_mono_cam_info_pub_.publish(left_camera_info);
2319  left_rgb_cam_info_pub_.publish(left_camera_info);
2320  left_rgb_rect_cam_info_pub_.publish(left_rectified_camera_info);
2321 
2322  } else if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_M == device_info_.hardwareRevision) {
2323 
2324  left_mono_cam_info_pub_.publish(left_camera_info);
2325  left_rect_cam_info_pub_.publish(left_rectified_camera_info);
2326  left_rgb_cam_info_pub_.publish(left_camera_info);
2327  left_rgb_rect_cam_info_pub_.publish(left_rectified_camera_info);
2328 
2329  } else { // all other MultiSense-S* variations
2330 
2331  if (has_left_camera_) {
2332  left_mono_cam_info_pub_.publish(left_camera_info);
2333  left_rect_cam_info_pub_.publish(left_rectified_camera_info);
2334  }
2335 
2336  if (has_right_camera_) {
2337  right_mono_cam_info_pub_.publish(right_camera_info);
2338  right_rect_cam_info_pub_.publish(right_rectified_camera_info);
2339  }
2340 
2341  // only publish for stereo cameras
2342  if (has_left_camera_ && has_right_camera_) {
2343 
2344  left_disp_cam_info_pub_.publish(left_rectified_camera_info);
2345  depth_cam_info_pub_.publish(left_rectified_camera_info);
2346 
2347  if (version_info_.sensorFirmwareVersion >= 0x0300) {
2348 
2349  right_disp_cam_info_pub_.publish(right_rectified_camera_info);
2350  left_cost_cam_info_pub_.publish(left_rectified_camera_info);
2351  }
2352  }
2353 
2354  // handle color topic publishing if color data exists
2355  if (has_aux_camera_) {
2356 
2357  //
2358  // The mono aux camera will operate at the native aux camera resolution. The rectified cameras will match
2359  // the main stereo pair
2360 
2361  const auto stereoResolution = stereo_calibration_manager_->operatingStereoResolution();
2362  const auto auxResolution = stereo_calibration_manager_->operatingStereoResolution();
2363 
2364  aux_mono_cam_info_pub_.publish(stereo_calibration_manager_->auxCameraInfo(frame_id_aux_, stamp, auxResolution));
2365  aux_rect_cam_info_pub_.publish(stereo_calibration_manager_->auxCameraInfo(frame_id_rectified_aux_, stamp, stereoResolution));
2366 
2367  aux_rgb_cam_info_pub_.publish(stereo_calibration_manager_->auxCameraInfo(frame_id_aux_, stamp, auxResolution));
2368  aux_rgb_rect_cam_info_pub_.publish(stereo_calibration_manager_->auxCameraInfo(frame_id_rectified_aux_, stamp, stereoResolution));
2369 
2370  } else if (has_color_ && !has_aux_camera_) { // !has_aux_camera_ is redundant, but leaving to prevent confusion over edge cases
2371 
2372  left_rgb_cam_info_pub_.publish(left_camera_info);
2373  left_rgb_rect_cam_info_pub_.publish(left_rectified_camera_info);
2374 
2375  }
2376  }
2377 }
2378 
2379 void Camera::deviceInfoDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat)
2380 {
2381  stat.add("device name", device_info_.name);
2382  stat.add("build date", device_info_.buildDate);
2383  stat.add("serial number", device_info_.serialNumber);
2384  stat.add("device revision", device_info_.hardwareRevision);
2385 
2386  for(const auto &pcb : device_info_.pcbs) {
2387  stat.add("pcb: " + pcb.name, pcb.revision);
2388  }
2389 
2390  stat.add("imager name", device_info_.imagerName);
2391  stat.add("imager type", device_info_.imagerType);
2392  stat.add("imager width", device_info_.imagerWidth);
2393  stat.add("imager height", device_info_.imagerHeight);
2394 
2395  stat.add("lens name", device_info_.lensName);
2396  stat.add("lens type", device_info_.lensType);
2397  stat.add("nominal baseline", device_info_.nominalBaseline);
2398  stat.add("nominal focal length", device_info_.nominalFocalLength);
2399  stat.add("nominal relative aperture", device_info_.nominalRelativeAperture);
2400 
2401  stat.add("lighting type", device_info_.lightingType);
2402  stat.add("number of lights", device_info_.numberOfLights);
2403 
2404  stat.add("laser name", device_info_.laserName);
2405  stat.add("laser type", device_info_.laserType);
2406 
2407  stat.add("motor name", device_info_.motorName);
2408  stat.add("motor type", device_info_.motorType);
2409  stat.add("motor gear reduction", device_info_.motorGearReduction);
2410 
2411  stat.add("api build date", version_info_.apiBuildDate);
2412  stat.add("api version", version_info_.apiVersion);
2413  stat.add("firmware build date", version_info_.sensorFirmwareBuildDate);
2414  stat.add("firmware version", version_info_.sensorFirmwareVersion);
2415  stat.add("bitstream version", version_info_.sensorHardwareVersion);
2416  stat.add("bitstream magic", version_info_.sensorHardwareMagic);
2417  stat.add("fpga dna", version_info_.sensorFpgaDna);
2418  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "MultiSense Device Info");
2419 }
2420 
2421 void Camera::deviceStatusDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat)
2422 {
2423  crl::multisense::system::StatusMessage statusMessage;
2424 
2425  if (crl::multisense::Status_Ok == driver_->getDeviceStatus(statusMessage)) {
2426  stat.add("uptime", ros::Time(statusMessage.uptime));
2427  stat.add("system", statusMessage.systemOk);
2428  stat.add("laser", statusMessage.laserOk);
2429  stat.add("laser motor", statusMessage.laserMotorOk);
2430  stat.add("cameras", statusMessage.camerasOk);
2431  stat.add("imu", statusMessage.imuOk);
2432  stat.add("external leds", statusMessage.externalLedsOk);
2433  stat.add("processing pipeline", statusMessage.processingPipelineOk);
2434  stat.add("power supply temp", statusMessage.powerSupplyTemperature);
2435  stat.add("fpga temp", statusMessage.fpgaTemperature);
2436  stat.add("left imager temp", statusMessage.leftImagerTemperature);
2437  stat.add("right imager temp", statusMessage.rightImagerTemperature);
2438  stat.add("input voltage", statusMessage.inputVoltage);
2439  stat.add("input current", statusMessage.inputCurrent);
2440  stat.add("fpga power", statusMessage.fpgaPower);
2441  stat.add("logic power", statusMessage.logicPower);
2442  stat.add("imager power", statusMessage.imagerPower);
2443  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "MultiSense Status: OK");
2444  } else {
2445  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "MultiSense Status: ERROR - Unable to retrieve status");
2446  }
2447 }
2448 
2449 void Camera::diagnosticTimerCallback(const ros::TimerEvent&)
2450 {
2451  diagnostic_updater_.update();
2452 }
2453 
2454 void Camera::stop()
2455 {
2456  std::lock_guard<std::mutex> lock(stream_lock_);
2457 
2458  stream_map_.clear();
2459 
2460  Status status = driver_->stopStreams(allImageSources);
2461  if (Status_Ok != status)
2462  ROS_ERROR("Camera: failed to stop all streams: %s",
2463  Channel::statusString(status));
2464 }
2465 
2466 void Camera::connectStream(DataSource enableMask)
2467 {
2468  std::lock_guard<std::mutex> lock(stream_lock_);
2469 
2470  DataSource notStarted = 0;
2471 
2472  for(uint32_t i=0; i<32; i++)
2473  if ((1<<i) & enableMask && 0 == stream_map_[(1<<i)]++)
2474  notStarted |= (1<<i);
2475 
2476  if (0 != notStarted) {
2477 
2478  Status status = driver_->startStreams(notStarted);
2479  if (Status_Ok != status)
2480  ROS_ERROR("Camera: failed to start streams 0x%x: %s",
2481  notStarted, Channel::statusString(status));
2482  }
2483 }
2484 
2485 void Camera::disconnectStream(DataSource disableMask)
2486 {
2487  std::lock_guard<std::mutex> lock(stream_lock_);
2488 
2489  DataSource notStopped = 0;
2490 
2491  for(uint32_t i=0; i<32; i++)
2492  if ((1<<i) & disableMask && 0 == --stream_map_[(1<<i)])
2493  notStopped |= (1<<i);
2494 
2495  if (0 != notStopped) {
2496  Status status = driver_->stopStreams(notStopped);
2497  if (Status_Ok != status)
2498  ROS_ERROR("Camera: failed to stop streams 0x%x: %s\n",
2499  notStopped, Channel::statusString(status));
2500  }
2501 }
2502 
2503 } // 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:217
msg
msg
multisense_ros::Camera::frame_id_rectified_right_
const std::string frame_id_rectified_right_
Definition: camera.h:305
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:234
multisense_ros::Camera::RECT_CAMERA_INFO_TOPIC
static constexpr char RECT_CAMERA_INFO_TOPIC[]
Definition: camera.h:134
multisense_ros::Camera::maxPointCloudRangeChanged
void maxPointCloudRangeChanged(double range)
Definition: camera.cpp:888
multisense_ros::Camera::luma_organized_point_cloud_pub_
ros::Publisher luma_organized_point_cloud_pub_
Definition: camera.h:230
crl::multisense::Source_Luma_Aux
static CRL_CONSTEXPR DataSource Source_Luma_Aux
multisense_ros::Camera::pointCloudCallback
void pointCloudCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:1603
multisense_ros::Camera::rawCamDataCallback
void rawCamDataCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:1895
multisense_ros::Camera::disparity_cost_transport_
image_transport::ImageTransport disparity_cost_transport_
Definition: camera.h:186
multisense_ros::Camera::left_mono_cam_pub_
image_transport::Publisher left_mono_cam_pub_
Definition: camera.h:196
sensor_msgs::image_encodings::BGR8
const std::string BGR8
tf2::Matrix3x3::getIdentity
static const Matrix3x3 & getIdentity()
multisense_ros::Camera::RAW_CAM_DATA_TOPIC
static constexpr char RAW_CAM_DATA_TOPIC[]
Definition: camera.h:118
multisense_ros::Camera::right_rect_image_
sensor_msgs::Image right_rect_image_
Definition: camera.h:255
multisense_ros::Camera::COLOR_CAMERA_INFO_TOPIC
static constexpr char COLOR_CAMERA_INFO_TOPIC[]
Definition: camera.h:135
multisense_ros::Camera::disparityImageCallback
void disparityImageCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:1030
multisense_ros::Camera::device_nh_
ros::NodeHandle device_nh_
Definition: camera.h:166
crl::multisense::Source_Disparity_Right
static CRL_CONSTEXPR DataSource Source_Disparity_Right
multisense_ros::Camera::static_tf_broadcaster_
tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_
Definition: camera.h:308
multisense_ros::Camera::DEPTH_TOPIC
static constexpr char DEPTH_TOPIC[]
Definition: camera.h:124
sensor_msgs::image_encodings::RGB8
const std::string RGB8
multisense_ros::Camera::right_stereo_disparity_pub_
ros::Publisher right_stereo_disparity_pub_
Definition: camera.h:238
multisense_ros::Camera::has_aux_camera_
bool has_aux_camera_
Definition: camera.h:356
multisense_ros::Camera::MONO_TOPIC
static constexpr char MONO_TOPIC[]
Definition: camera.h:120
crl::multisense::ground_surface::Header::extrinsics
float extrinsics[6]
multisense_ros::Camera::right_rect_cam_info_pub_
ros::Publisher right_rect_cam_info_pub_
Definition: camera.h:213
multisense_ros::Camera::colorImageCallback
void colorImageCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:1936
multisense_ros::Camera::DISPARITY_CAMERA_INFO_TOPIC
static constexpr char DISPARITY_CAMERA_INFO_TOPIC[]
Definition: camera.h:138
crl::multisense::Source_Jpeg_Left
static CRL_CONSTEXPR DataSource Source_Jpeg_Left
multisense_ros::Camera::COST_CAMERA_INFO_TOPIC
static constexpr char COST_CAMERA_INFO_TOPIC[]
Definition: camera.h:139
multisense_ros::Camera::left_rect_transport_
image_transport::ImageTransport left_rect_transport_
Definition: camera.h:178
multisense_ros::Camera::ni_depth_image_
sensor_msgs::Image ni_depth_image_
Definition: camera.h:257
NONE
NONE
crl::multisense::ground_surface::Header::controlPointsBitsPerPixel
uint32_t controlPointsBitsPerPixel
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:260
multisense_ros::Camera::luma_point_cloud_
sensor_msgs::PointCloud2 luma_point_cloud_
Definition: camera.h:258
multisense_ros::Camera::monoCallback
void monoCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:1215
ros::NodeHandle::createTimer
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
multisense_ros::Camera::stop
void stop()
Definition: camera.cpp:2454
crl::multisense::ground_surface::Header::controlPointsWidth
uint32_t controlPointsWidth
image_transport::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
diagnostic_updater::DiagnosticStatusWrapper::summary
void summary(unsigned char lvl, const std::string s)
multisense_ros::BorderClip
BorderClip
Definition: camera_utilities.h:62
multisense_ros::Camera::left_disparity_cost_pub_
image_transport::Publisher left_disparity_cost_pub_
Definition: camera.h:235
multisense_ros::Camera::color_point_cloud_
sensor_msgs::PointCloud2 color_point_cloud_
Definition: camera.h:259
multisense_ros::Camera::left_stereo_disparity_pub_
ros::Publisher left_stereo_disparity_pub_
Definition: camera.h:237
multisense_ros::Camera::luma_point_cloud_pub_
ros::Publisher luma_point_cloud_pub_
Definition: camera.h:226
multisense_ros::Camera::aux_nh_
ros::NodeHandle aux_nh_
Definition: camera.h:169
crl::multisense::Source_Chroma_Left
static CRL_CONSTEXPR DataSource Source_Chroma_Left
multisense_ros::Camera::GROUND_SURFACE_POINT_SPLINE_TOPIC
static constexpr char GROUND_SURFACE_POINT_SPLINE_TOPIC[]
Definition: camera.h:142
multisense_ros::Camera::depth_cam_info_pub_
ros::Publisher depth_cam_info_pub_
Definition: camera.h:219
crl::multisense::Channel::stopStreams
virtual Status stopStreams(DataSource mask)=0
multisense_ros::Camera::histogram_pub_
ros::Publisher histogram_pub_
Definition: camera.h:247
multisense_ros::Camera::left_mono_cam_info_pub_
ros::Publisher left_mono_cam_info_pub_
Definition: camera.h:210
crl::multisense::ground_surface::Header::xzCellOrigin
float xzCellOrigin[2]
diagnostic_updater::Updater::setHardwareID
void setHardwareID(const std::string &hwid)
multisense_ros::Camera::frame_id_left_
const std::string frame_id_left_
Definition: camera.h:301
multisense_ros::Camera::ORGANIZED_POINTCLOUD_TOPIC
static constexpr char ORGANIZED_POINTCLOUD_TOPIC[]
Definition: camera.h:131
multisense_ros::Camera::diagnosticTimerCallback
void diagnosticTimerCallback(const ros::TimerEvent &)
Definition: camera.cpp:2449
multisense_ros::Camera::right_disparity_image_
sensor_msgs::Image right_disparity_image_
Definition: camera.h:272
image_transport::ImageTransport::advertise
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
multisense_ros::Camera::image_buffers_
std::unordered_map< crl::multisense::DataSource, std::shared_ptr< BufferWrapper< crl::multisense::image::Header > > > image_buffers_
Definition: camera.h:341
crl::multisense::Channel::getImageHistogram
virtual Status getImageHistogram(int64_t frameId, image::Histogram &histogram)=0
multisense_ros::Camera::DEVICE_INFO_TOPIC
static constexpr char DEVICE_INFO_TOPIC[]
Definition: camera.h:115
multisense_ros::Camera::RECT_TOPIC
static constexpr char RECT_TOPIC[]
Definition: camera.h:121
multisense_ros::Camera::border_clip_value_
double border_clip_value_
Definition: camera.h:331
multisense_ros::Camera::depth_image_
sensor_msgs::Image depth_image_
Definition: camera.h:256
multisense_ros::Camera::aux_rgb_transport_
image_transport::ImageTransport aux_rgb_transport_
Definition: camera.h:188
diagnostic_updater::DiagnosticTaskVector::add
void add(const std::string &name, TaskFunction f)
multisense_ros::Camera::disconnectStream
void disconnectStream(crl::multisense::DataSource disableMask)
Definition: camera.cpp:2485
multisense_ros::Camera::aux_mono_cam_pub_
image_transport::Publisher aux_mono_cam_pub_
Definition: camera.h:205
multisense_ros::Camera::DISPARITY_IMAGE_TOPIC
static constexpr char DISPARITY_IMAGE_TOPIC[]
Definition: camera.h:123
crl::multisense::Source_Luma_Left
static CRL_CONSTEXPR DataSource Source_Luma_Left
data
data
multisense_ros::Camera::depthCallback
void depthCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:1458
multisense_ros::Camera::ground_surface_info_pub_
ros::Publisher ground_surface_info_pub_
Definition: camera.h:224
multisense_ros::Camera::connectStream
void connectStream(crl::multisense::DataSource enableMask)
Definition: camera.cpp:2466
multisense_ros::Camera::ground_surface_nh_
ros::NodeHandle ground_surface_nh_
Definition: camera.h:171
multisense_ros::Camera::raw_cam_config_pub_
ros::Publisher raw_cam_config_pub_
Definition: camera.h:244
multisense_ros::Camera::COST_TOPIC
static constexpr char COST_TOPIC[]
Definition: camera.h:126
multisense_ros::Camera::left_disparity_cost_image_
sensor_msgs::Image left_disparity_cost_image_
Definition: camera.h:271
ROS_WARN
#define ROS_WARN(...)
multisense_ros::Camera::has_right_camera_
bool has_right_camera_
Definition: camera.h:346
multisense_ros::Camera::has_left_camera_
bool has_left_camera_
Definition: camera.h:351
crl::multisense::Channel::getVersionInfo
virtual Status getVersionInfo(system::VersionInfo &v)=0
t
geometry_msgs::TransformStamped t
multisense_ros::Camera::right_mono_transport_
image_transport::ImageTransport right_mono_transport_
Definition: camera.h:177
multisense_ros::Camera::left_rgb_transport_
image_transport::ImageTransport left_rgb_transport_
Definition: camera.h:180
multisense_ros::Camera::colorizeCallback
void colorizeCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:2125
multisense_ros::Camera::ground_surface_image_
sensor_msgs::Image ground_surface_image_
Definition: camera.h:277
multisense_ros::Camera::right_mono_cam_info_pub_
ros::Publisher right_mono_cam_info_pub_
Definition: camera.h:211
multisense_ros::Camera::frame_id_right_
const std::string frame_id_right_
Definition: camera.h:302
multisense_ros::Camera::left_stereo_disparity_
stereo_msgs::DisparityImage left_stereo_disparity_
Definition: camera.h:274
multisense_ros::Camera::can_support_ground_surface_
bool can_support_ground_surface_
Definition: camera.h:366
multisense_ros::Camera::aux_mono_transport_
image_transport::ImageTransport aux_mono_transport_
Definition: camera.h:187
multisense_ros::Camera::aux_rgb_cam_info_pub_
ros::Publisher aux_rgb_cam_info_pub_
Definition: camera.h:221
crl::multisense::image::Histogram::data
std::vector< uint32_t > data
crl::multisense::Source_Luma_Rectified_Aux
static CRL_CONSTEXPR DataSource Source_Luma_Rectified_Aux
multisense_ros::Camera::diagnostic_trigger_
ros::Timer diagnostic_trigger_
Definition: camera.h:375
multisense_ros::Camera::right_mono_cam_pub_
image_transport::Publisher right_mono_cam_pub_
Definition: camera.h:197
multisense_ros::Camera::borderClipChanged
void borderClipChanged(const BorderClip &borderClipType, double borderClipValue)
Definition: camera.cpp:879
multisense_ros::Camera::stream_map_
StreamMapType stream_map_
Definition: camera.h:315
multisense_ros::Camera::device_info_
crl::multisense::system::DeviceInfo device_info_
Definition: camera.h:288
image_transport::ImageTransport::advertiseCamera
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
multisense_ros::Camera::aux_rgb_rect_cam_pub_
image_transport::CameraPublisher aux_rgb_rect_cam_pub_
Definition: camera.h:207
multisense_ros::Camera::ground_surface_cam_pub_
image_transport::Publisher ground_surface_cam_pub_
Definition: camera.h:208
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
crl::multisense::Source_Luma_Right
static CRL_CONSTEXPR DataSource Source_Luma_Right
multisense_ros::Camera::aux_rect_cam_info_pub_
ros::Publisher aux_rect_cam_info_pub_
Definition: camera.h:222
multisense_ros::Camera::left_mono_image_
sensor_msgs::Image left_mono_image_
Definition: camera.h:252
multisense_ros::Camera::ni_depth_cam_pub_
image_transport::Publisher ni_depth_cam_pub_
Definition: camera.h:201
multisense_ros::Camera::rectCallback
void rectCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:1327
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::aux_rgb_rect_transport_
image_transport::ImageTransport aux_rgb_rect_transport_
Definition: camera.h:190
multisense_ros::Camera::frame_id_rectified_left_
const std::string frame_id_rectified_left_
Definition: camera.h:304
multisense_ros::Camera::updateConfig
void updateConfig(const crl::multisense::image::Config &config)
Definition: camera.cpp:2255
multisense_ros::Camera::left_disparity_pub_
image_transport::Publisher left_disparity_pub_
Definition: camera.h:233
multisense_ros::Camera::raw_cam_data_pub_
ros::Publisher raw_cam_data_pub_
Definition: camera.h:243
multisense_ros::Camera::left_rgb_rect_cam_pub_
image_transport::CameraPublisher left_rgb_rect_cam_pub_
Definition: camera.h:203
atan
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
multisense_ros::Camera::groundSurfaceCallback
void groundSurfaceCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:2140
multisense_ros::Camera::left_disp_cam_info_pub_
ros::Publisher left_disp_cam_info_pub_
Definition: camera.h:214
multisense_ros::Camera::depth_cam_pub_
image_transport::Publisher depth_cam_pub_
Definition: camera.h:200
image_transport::CameraPublisher::getNumSubscribers
uint32_t getNumSubscribers() const
multisense_ros::Camera::border_clip_type_
BorderClip border_clip_type_
Definition: camera.h:330
multisense_ros::Camera::ground_surface_transport_
image_transport::ImageTransport ground_surface_transport_
Definition: camera.h:191
multisense_ros::ycbcrToBgr
constexpr Eigen::Matrix< T, 3, 1 > ycbcrToBgr(const crl::multisense::image::Header &luma, const crl::multisense::image::Header &chroma, size_t u, size_t v)
Definition: camera_utilities.h:104
multisense_ros::Camera::groundSurfaceSplineDrawParametersChanged
void groundSurfaceSplineDrawParametersChanged(const ground_surface_utilities::SplineDrawParameters &spline_draw_params)
Definition: camera.cpp:923
multisense_ros::Camera::GROUND_SURFACE_INFO_TOPIC
static constexpr char GROUND_SURFACE_INFO_TOPIC[]
Definition: camera.h:141
ROS_INFO
#define ROS_INFO(...)
multisense_ros::Camera::aux_rect_transport_
image_transport::ImageTransport aux_rect_transport_
Definition: camera.h:189
multisense_ros::Camera::deviceStatusDiagnostic
void deviceStatusDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
Definition: camera.cpp:2421
crl::multisense::Channel::getImageCalibration
virtual Status getImageCalibration(image::Calibration &c)=0
ground_surface_utilities::SplineDrawParameters
Struct containing parameters for drawing a pointcloud representation of a B-Spline model...
Definition: ground_surface_utilities.h:68
sensor_msgs::image_encodings::TYPE_32FC1
const std::string TYPE_32FC1
crl::multisense::ground_surface::Header
multisense_ros::Camera::right_nh_
ros::NodeHandle right_nh_
Definition: camera.h:168
diagnostic_updater::Updater::update
void update()
multisense_ros::Camera::right_rect_cam_pub_
image_transport::CameraPublisher right_rect_cam_pub_
Definition: camera.h:199
crl::multisense::ground_surface::Header::controlPointsImageDataP
const void * controlPointsImageDataP
multisense_ros::Camera::left_mono_transport_
image_transport::ImageTransport left_mono_transport_
Definition: camera.h:176
multisense_ros::Camera::GROUND_SURFACE_IMAGE_TOPIC
static constexpr char GROUND_SURFACE_IMAGE_TOPIC[]
Definition: camera.h:140
sensor_msgs::image_encodings::MONO8
const std::string MONO8
multisense_ros::Camera::aux_rect_cam_pub_
image_transport::CameraPublisher aux_rect_cam_pub_
Definition: camera.h:206
multisense_ros::Camera::raw_cam_cal_pub_
ros::Publisher raw_cam_cal_pub_
Definition: camera.h:245
crl::multisense::Status_Ok
static CRL_CONSTEXPR Status Status_Ok
multisense_ros::Camera::left_rect_cam_pub_
image_transport::CameraPublisher left_rect_cam_pub_
Definition: camera.h:198
crl::multisense::Source_Chroma_Aux
static CRL_CONSTEXPR DataSource Source_Chroma_Aux
multisense_ros::Camera::left_rect_image_
sensor_msgs::Image left_rect_image_
Definition: camera.h:254
multisense_ros::Camera::diagnostic_updater_
diagnostic_updater::Updater diagnostic_updater_
Definition: camera.h:370
multisense_ros::Camera::right_disp_cam_info_pub_
ros::Publisher right_disp_cam_info_pub_
Definition: camera.h:215
distortion_models.h
multisense_ros::Camera::driver_
crl::multisense::Channel * driver_
Definition: camera.h:161
multisense_ros::Camera::deviceInfoDiagnostic
void deviceInfoDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
Definition: camera.cpp:2379
multisense_ros::Camera::groundSurfaceSplineCallback
void groundSurfaceSplineCallback(const crl::multisense::ground_surface::Header &header)
Definition: camera.cpp:2209
multisense_ros::Camera::device_info_pub_
ros::Publisher device_info_pub_
Definition: camera.h:246
tf2_geometry_msgs.h
multisense_ros::Camera::RAW_CAM_CAL_TOPIC
static constexpr char RAW_CAM_CAL_TOPIC[]
Definition: camera.h:116
multisense_ros::Camera::left_rgb_cam_pub_
image_transport::Publisher left_rgb_cam_pub_
Definition: camera.h:202
multisense_ros::Camera::frame_id_rectified_aux_
const std::string frame_id_rectified_aux_
Definition: camera.h:306
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
multisense_ros::Camera::extrinsicsChanged
void extrinsicsChanged(crl::multisense::system::ExternalCalibration extrinsics)
Definition: camera.cpp:893
sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
multisense_ros::Status
Definition: status.h:43
sensor_msgs::image_encodings::MONO16
const std::string MONO16
multisense_ros::Camera::RECT_COLOR_CAMERA_INFO_TOPIC
static constexpr char RECT_COLOR_CAMERA_INFO_TOPIC[]
Definition: camera.h:136
multisense_ros::Camera::COLOR_POINTCLOUD_TOPIC
static constexpr char COLOR_POINTCLOUD_TOPIC[]
Definition: camera.h:130
multisense_ros::Camera::OPENNI_DEPTH_TOPIC
static constexpr char OPENNI_DEPTH_TOPIC[]
Definition: camera.h:125
multisense_ros::Camera::COLOR_TOPIC
static constexpr char COLOR_TOPIC[]
Definition: camera.h:127
multisense_ros::Camera::left_rgb_rect_transport_
image_transport::ImageTransport left_rgb_rect_transport_
Definition: camera.h:181
multisense_ros::Camera::stream_lock_
std::mutex stream_lock_
Definition: camera.h:314
multisense_ros::Camera::stereo_calibration_manager_
std::shared_ptr< StereoCalibrationManager > stereo_calibration_manager_
Definition: camera.h:294
multisense_ros::Camera::calibration_nh_
ros::NodeHandle calibration_nh_
Definition: camera.h:170
multisense_ros::Camera::aux_rgb_rect_cam_info_pub_
ros::Publisher aux_rgb_rect_cam_info_pub_
Definition: camera.h:223
image_transport::Publisher::publish
void publish(const sensor_msgs::Image &message) const
multisense_ros::Camera::aux_rgb_image_
sensor_msgs::Image aux_rgb_image_
Definition: camera.h:265
multisense_ros::Camera::left_rgb_image_
sensor_msgs::Image left_rgb_image_
Definition: camera.h:264
multisense_ros::Camera::right_mono_image_
sensor_msgs::Image right_mono_image_
Definition: camera.h:253
multisense_ros::Camera::depth_transport_
image_transport::ImageTransport depth_transport_
Definition: camera.h:182
multisense_ros::Camera::left_rgb_rect_cam_info_pub_
ros::Publisher left_rgb_rect_cam_info_pub_
Definition: camera.h:218
multisense_ros::Camera::histogramCallback
void histogramCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:929
multisense_ros::Camera::spline_draw_params_
ground_surface_utilities::SplineDrawParameters spline_draw_params_
Definition: camera.h:336
multisense_ros::Camera::right_stereo_disparity_
stereo_msgs::DisparityImage right_stereo_disparity_
Definition: camera.h:275
tf2::toMsg
B toMsg(const A &a)
multisense_ros::Camera::RECT_COLOR_TOPIC
static constexpr char RECT_COLOR_TOPIC[]
Definition: camera.h:128
crl::multisense::ground_surface::Header::success
uint8_t success
DataSource
uint32_t DataSource
multisense_ros::Camera::left_nh_
ros::NodeHandle left_nh_
Definition: camera.h:167
multisense_ros::Camera::pointcloud_max_range_
double pointcloud_max_range_
Definition: camera.h:320
multisense_ros::Camera::right_rect_transport_
image_transport::ImageTransport right_rect_transport_
Definition: camera.h:179
multisense_ros::Camera::ground_surface_spline_pub_
ros::Publisher ground_surface_spline_pub_
Definition: camera.h:228
crl::multisense::Source_Chroma_Rectified_Aux
static CRL_CONSTEXPR DataSource Source_Chroma_Rectified_Aux
multisense_ros::Camera::left_cost_cam_info_pub_
ros::Publisher left_cost_cam_info_pub_
Definition: camera.h:216
crl::multisense::Channel::getDeviceStatus
virtual Status getDeviceStatus(system::StatusMessage &status)=0
multisense_ros::Camera::DEPTH_CAMERA_INFO_TOPIC
static constexpr char DEPTH_CAMERA_INFO_TOPIC[]
Definition: camera.h:137
multisense_ros::Camera::HISTOGRAM_TOPIC
static constexpr char HISTOGRAM_TOPIC[]
Definition: camera.h:119
multisense_ros::Camera::has_color_
bool has_color_
Definition: camera.h:361
multisense_ros::Camera::aux_rgb_cam_pub_
image_transport::Publisher aux_rgb_cam_pub_
Definition: camera.h:204
image_transport::CameraPublisher::publish
void publish(const sensor_msgs::Image &image, const sensor_msgs::CameraInfo &info) const
multisense_ros::Camera::ni_depth_transport_
image_transport::ImageTransport ni_depth_transport_
Definition: camera.h:183
point_cloud_utilities.h
multisense_ros::Camera::disparity_right_transport_
image_transport::ImageTransport disparity_right_transport_
Definition: camera.h:185
multisense_ros::Camera::POINTCLOUD_TOPIC
static constexpr char POINTCLOUD_TOPIC[]
Definition: camera.h:129
multisense_ros::Camera::aux_mono_image_
sensor_msgs::Image aux_mono_image_
Definition: camera.h:263
crl::multisense::ground_surface::Header::controlPointsHeight
uint32_t controlPointsHeight
crl::multisense::ground_surface::Header::xzCellSize
float xzCellSize[2]
multisense_ros::Camera::left_rect_cam_info_pub_
ros::Publisher left_rect_cam_info_pub_
Definition: camera.h:212
multisense_ros::Camera::pointcloud_rect_color_buffer_
std::vector< uint8_t > pointcloud_rect_color_buffer_
Definition: camera.h:282
ros::Time::now
static Time now()
crl::multisense::Channel::getDeviceInfo
virtual Status getDeviceInfo(system::DeviceInfo &info)=0
ground_surface_utilities::eigenToPointcloud
sensor_msgs::PointCloud2 eigenToPointcloud(const std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f >> &input, const std::string &frame_id)
Convert an eigen representation of a pointcloud to a ROS sensor_msgs::PointCloud2 format...
Definition: ground_surface_utilities.cpp:180
ground_surface_utilities::groundSurfaceClassToPixelColor
Eigen::Matrix< uint8_t, 3, 1 > groundSurfaceClassToPixelColor(const uint8_t value)
Look up table to convert a ground surface class value into into a color for visualization (out-of-bou...
Definition: ground_surface_utilities.cpp:167
multisense_ros::Camera::jpegImageCallback
void jpegImageCallback(const crl::multisense::image::Header &header)
Definition: camera.cpp:965
multisense_ros::Camera::left_disparity_image_
sensor_msgs::Image left_disparity_image_
Definition: camera.h:270
image_transport::Publisher
multisense_ros::Camera::color_organized_point_cloud_pub_
ros::Publisher color_organized_point_cloud_pub_
Definition: camera.h:231
crl::multisense::ground_surface::Header::quadraticParams
float quadraticParams[6]
multisense_ros::Camera::aux_rgb_rect_image_
sensor_msgs::Image aux_rgb_rect_image_
Definition: camera.h:268
multisense_ros::Camera::MONO_CAMERA_INFO_TOPIC
static constexpr char MONO_CAMERA_INFO_TOPIC[]
Definition: camera.h:133
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:184
diagnostic_updater::DiagnosticStatusWrapper::add
void add(const std::string &key, const T &val)
multisense_ros::Camera::frame_id_origin_
const std::string frame_id_origin_
Definition: camera.h:299
image_encodings.h
multisense_ros::Camera::color_organized_point_cloud_
sensor_msgs::PointCloud2 color_organized_point_cloud_
Definition: camera.h:261
multisense_ros::Camera::version_info_
crl::multisense::system::VersionInfo version_info_
Definition: camera.h:287
multisense_ros::Camera::COLOR_ORGANIZED_POINTCLOUD_TOPIC
static constexpr char COLOR_ORGANIZED_POINTCLOUD_TOPIC[]
Definition: camera.h:132
tf2_ros::StaticTransformBroadcaster::sendTransform
void sendTransform(const geometry_msgs::TransformStamped &transform)
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
multisense_ros::Camera::left_rgb_rect_image_
sensor_msgs::Image left_rgb_rect_image_
Definition: camera.h:266
multisense_ros::Camera::frame_id_aux_
const std::string frame_id_aux_
Definition: camera.h:303
multisense_ros::Camera::publishAllCameraInfo
void publishAllCameraInfo()
Definition: camera.cpp:2294
multisense_ros::Camera::aux_mono_cam_info_pub_
ros::Publisher aux_mono_cam_info_pub_
Definition: camera.h:220
ROS_ERROR
#define ROS_ERROR(...)
multisense_ros::Camera::raw_cam_data_
multisense_ros::RawCamData raw_cam_data_
Definition: camera.h:279
multisense_ros
Definition: camera.h:58
multisense_ros::Camera::color_point_cloud_pub_
ros::Publisher color_point_cloud_pub_
Definition: camera.h:227
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:325
multisense_ros::Camera::aux_rect_image_
sensor_msgs::Image aux_rect_image_
Definition: camera.h:267
multisense_ros::Camera::~Camera
~Camera()
Definition: camera.cpp:843
multisense_ros::Camera::pointcloud_color_buffer_
std::vector< uint8_t > pointcloud_color_buffer_
Definition: camera.h:281
diagnostic_updater::DiagnosticStatusWrapper
ground_surface_utilities::convertSplineToPointcloud
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > convertSplineToPointcloud(const Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > &controlGrid, const SplineDrawParameters &splineDrawParams, const double pointcloudMaxRange, const float *xzCellOrigin, const float *xzCellSize, const float *minMaxAzimuthAngle, const float *extrinsics, const float *quadraticParams, const float baseline)
Generate a pointcloud representation of a b-spline ground surface model for visualization.
Definition: ground_surface_utilities.cpp:206
multisense_ros::Camera::DISPARITY_TOPIC
static constexpr char DISPARITY_TOPIC[]
Definition: camera.h:122
multisense_ros::Camera::RAW_CAM_CONFIG_TOPIC
static constexpr char RAW_CAM_CONFIG_TOPIC[]
Definition: camera.h:117
crl::multisense::Source_Luma_Rectified_Left
static CRL_CONSTEXPR DataSource Source_Luma_Rectified_Left

multisense_ros
Author(s):
autogenerated on Sat Jun 24 2023 03:01:30