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

multisense_ros
Author(s):
autogenerated on Thu Feb 20 2025 03:51:03