Intel® RealSense™ Cross Platform API
Intel Realsense Cross-platform API
ros_file_format.h
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2017 Intel Corporation. All Rights Reserved.
3 
4 #pragma once
5 #include <string>
6 #include <chrono>
7 #include "librealsense2/rs.h"
8 #include "sensor_msgs/image_encodings.h"
9 #include "sensor_msgs/Imu.h"
10 #include "sensor_msgs/Image.h"
11 #include "diagnostic_msgs/KeyValue.h"
12 #include "std_msgs/UInt32.h"
13 #include "std_msgs/Float32.h"
14 #include "std_msgs/String.h"
15 #include "realsense_msgs/StreamInfo.h"
16 #include "realsense_msgs/ImuIntrinsic.h"
17 #include "realsense_msgs/Notification.h"
18 #include "realsense_legacy_msgs/legacy_headers.h"
19 #include "sensor_msgs/CameraInfo.h"
20 #include "geometry_msgs/Transform.h"
21 #include "geometry_msgs/Twist.h"
22 #include "geometry_msgs/Accel.h"
23 #include "metadata-parser.h"
24 #include "option.h"
25 #include "rosbag/structures.h"
26 #include <regex>
27 #include "stream.h"
28 #include "types.h"
29 #include <vector>
30 
31 namespace librealsense
32 {
33  inline void convert(rs2_format source, std::string& target)
34  {
35  switch (source)
36  {
37  case RS2_FORMAT_Z16: target = sensor_msgs::image_encodings::MONO16; return;
38  case RS2_FORMAT_RGB8: target = sensor_msgs::image_encodings::RGB8; return;
39  case RS2_FORMAT_BGR8: target = sensor_msgs::image_encodings::BGR8; return;
40  case RS2_FORMAT_RGBA8: target = sensor_msgs::image_encodings::RGBA8; return;
41  case RS2_FORMAT_BGRA8: target = sensor_msgs::image_encodings::BGRA8; return;
42  case RS2_FORMAT_Y8: target = sensor_msgs::image_encodings::TYPE_8UC1; return;
43  case RS2_FORMAT_Y16: target = sensor_msgs::image_encodings::TYPE_16UC1; return;
44  case RS2_FORMAT_RAW8: target = sensor_msgs::image_encodings::MONO8; return;
45  case RS2_FORMAT_UYVY: target = sensor_msgs::image_encodings::YUV422; return;
46  default: target = rs2_format_to_string(source);
47  }
48  }
49 
50  inline void convert(const std::string& source, rs2_format& target)
51  {
52  if (source == sensor_msgs::image_encodings::MONO16) { target = RS2_FORMAT_Z16; return; }
53  if (source == sensor_msgs::image_encodings::RGB8) { target = RS2_FORMAT_RGB8; return; }
54  if (source == sensor_msgs::image_encodings::BGR8) { target = RS2_FORMAT_BGR8; return; }
55  if (source == sensor_msgs::image_encodings::RGBA8) { target = RS2_FORMAT_RGBA8; return; }
56  if (source == sensor_msgs::image_encodings::BGRA8) { target = RS2_FORMAT_BGRA8; return; }
57  if (source == sensor_msgs::image_encodings::TYPE_8UC1) { target = RS2_FORMAT_Y8; return; }
58  if (source == sensor_msgs::image_encodings::TYPE_16UC1) { target = RS2_FORMAT_Y16; return; }
59  if (source == sensor_msgs::image_encodings::MONO8) { target = RS2_FORMAT_RAW8; return; }
60  if (source == sensor_msgs::image_encodings::YUV422) { target = RS2_FORMAT_UYVY; return; }
61  if (!try_parse(source, target))
62  {
63  throw std::runtime_error(to_string() << "Failed to convert source: \"" << "\" to matching rs2_format");
64  }
65  }
66 
67  inline void convert(const std::string& source, rs2_stream& target)
68  {
69  if(!try_parse(source, target))
70  {
71  throw std::runtime_error(to_string() << "Failed to convert source: \"" << "\" to matching rs2_stream");
72  }
73  }
74 
75  inline void convert(const std::string& source, rs2_distortion& target)
76  {
77  if (!try_parse(source, target))
78  {
79  throw std::runtime_error(to_string() << "Failed to convert source: \"" << "\" to matching rs2_distortion");
80  }
81  }
82 
83  inline void convert(const std::string& source, rs2_option& target)
84  {
85  if (!try_parse(source, target))
86  {
87  throw std::runtime_error(to_string() << "Failed to convert source: \"" << "\" to matching rs2_optin");
88  }
89  }
90 
91  inline void convert(const std::string& source, rs2_frame_metadata_value& target)
92  {
93  if (!try_parse(source, target))
94  {
95  throw std::runtime_error(to_string() << "Failed to convert source: \"" << "\" to matching rs2_frame_metadata");
96  }
97  }
98 
99  inline void convert(const std::string& source, rs2_camera_info& target)
100  {
101  if (!try_parse(source, target))
102  {
103  throw std::runtime_error(to_string() << "Failed to convert source: \"" << "\" to matching rs2_camera_info");
104  }
105  }
106 
107  inline void convert(const std::string& source, rs2_timestamp_domain& target)
108  {
109  if (!try_parse(source, target))
110  {
111  throw std::runtime_error(to_string() << "Failed to convert source: \"" << "\" to matching rs2_timestamp_domain");
112  }
113  }
114 
115  inline void convert(const std::string& source, rs2_notification_category& target)
116  {
117  if (!try_parse(source, target))
118  {
119  throw std::runtime_error(to_string() << "Failed to convert source: \"" << "\" to matching rs2_notification_category");
120  }
121  }
122 
123  inline void convert(const std::string& source, rs2_log_severity& target)
124  {
125  if (!try_parse(source, target))
126  {
127  throw std::runtime_error(to_string() << "Failed to convert source: \"" << "\" to matching rs2_log_severity");
128  }
129  }
130 
131  inline void convert(const std::string& source, double& target)
132  {
133  target = std::stod(source);
134  }
135 
136  inline void convert(const std::string& source, long long& target)
137  {
138  target = std::stoll(source);
139  }
140  /*
141  quat2rot(), rot2quat()
142  ------------------
143 
144  rotation matrix is column major
145  m[3][3] <==> r[9]
146 
147  [00, 01, 02] <==> [ r[0], r[3], r[6] ]
148  m = [10, 11, 12] <==> [ r[1], r[4], r[7] ]
149  [20, 21, 22] <==> [ r[2], r[5], r[8] ]
150  */
151 
152  inline void quat2rot(const geometry_msgs::Transform::_rotation_type& q, float(&r)[9])
153  {
154  r[0] = 1 - 2 * q.y*q.y - 2 * q.z*q.z; // m00 = 1 - 2 * qy2 - 2 * qz2
155  r[3] = 2 * q.x*q.y - 2 * q.z*q.w; // m01 = 2 * qx*qy - 2 * qz*qw
156  r[6] = 2 * q.x*q.z + 2 * q.y*q.w; // m02 = 2 * qx*qz + 2 * qy*qw
157  r[1] = 2 * q.x*q.y + 2 * q.z*q.w; // m10 = 2 * qx*qy + 2 * qz*qw
158  r[4] = 1 - 2 * q.x*q.x - 2 * q.z*q.z; // m11 = 1 - 2 * qx2 - 2 * qz2
159  r[7] = 2 * q.y*q.z - 2 * q.x*q.w; // m12 = 2 * qy*qz - 2 * qx*qw
160  r[2] = 2 * q.x*q.z - 2 * q.y*q.w; // m20 = 2 * qx*qz - 2 * qy*qw
161  r[5] = 2 * q.y*q.z + 2 * q.x*q.w; // m21 = 2 * qy*qz + 2 * qx*qw
162  r[8] = 1 - 2 * q.x*q.x - 2 * q.y*q.y; // m22 = 1 - 2 * qx2 - 2 * qy2
163  }
164 
165  inline void rot2quat(const float(&r)[9], geometry_msgs::Transform::_rotation_type& q)
166  {
167  q.w = sqrtf(1 + r[0] + r[4] + r[8]) / 2; // qw= sqrt(1 + m00 + m11 + m22) /2
168  q.x = (r[5] - r[7]) / (4 * q.w); // qx = (m21 - m12)/( 4 *qw)
169  q.y = (r[6] - r[2]) / (4 * q.w); // qy = (m02 - m20)/( 4 *qw)
170  q.z = (r[1] - r[3]) / (4 * q.w); // qz = (m10 - m01)/( 4 *qw)
171  }
172 
173  inline void convert(const geometry_msgs::Transform& source, rs2_extrinsics& target)
174  {
175  target.translation[0] = source.translation.x;
176  target.translation[1] = source.translation.y;
177  target.translation[2] = source.translation.z;
178  quat2rot(source.rotation, target.rotation);
179  }
180 
181  inline void convert(const rs2_extrinsics& source, geometry_msgs::Transform& target)
182  {
183  target.translation.x = source.translation[0];
184  target.translation.y = source.translation[1];
185  target.translation.z = source.translation[2];
186  rot2quat(source.rotation, target.rotation);
187  }
188 
189  constexpr const char* TIMESTAMP_DOMAIN_MD_STR = "timestamp_domain";
190  constexpr const char* SYSTEM_TIME_MD_STR = "system_time";
191  constexpr const char* MAPPER_CONFIDENCE_MD_STR = "Mapper Confidence";
192  constexpr const char* FRAME_TIMESTAMP_MD_STR = "frame_timestamp";
193  constexpr const char* TRACKER_CONFIDENCE_MD_STR = "Tracker Confidence";
194 
196  {
197  public:
199  rs2_metadata_type get(const frame& frm) const override
200  {
202  if (try_get(frm, v) == false)
203  {
204  throw invalid_value_exception("Frame does not support this type of metadata");
205  }
206  return v;
207  }
208  bool supports(const frame& frm) const override
209  {
211  return try_get(frm, v);
212  }
213  private:
214  bool try_get(const frame& frm, rs2_metadata_type& result) const
215  {
216  auto pair_size = (sizeof(rs2_frame_metadata_value) + sizeof(rs2_metadata_type));
217  const uint8_t* pos = frm.additional_data.metadata_blob.data();
218  while (pos <= frm.additional_data.metadata_blob.data() + frm.additional_data.metadata_blob.size())
219  {
220  const rs2_frame_metadata_value* type = reinterpret_cast<const rs2_frame_metadata_value*>(pos);
221  pos += sizeof(rs2_frame_metadata_value);
222  if (_type == *type)
223  {
224  const rs2_metadata_type* value = reinterpret_cast<const rs2_metadata_type*>(pos);
225  result = *value;
226  return true;
227  }
228  pos += sizeof(rs2_metadata_type);
229  }
230  return false;
231  }
233  };
234 
235  class ros_topic
236  {
237  public:
238  static constexpr const char* elements_separator() { return "/"; }
239  static constexpr const char* ros_image_type_str() { return "image"; }
240  static constexpr const char* ros_imu_type_str() { return "imu"; }
241  static constexpr const char* ros_pose_type_str() { return "pose"; }
242 
243  static uint32_t get_device_index(const std::string& topic)
244  {
245  return get_id("device_", get<1>(topic));
246  }
247 
248  static uint32_t get_sensor_index(const std::string& topic)
249  {
250  return get_id("sensor_", get<2>(topic));
251  }
252 
253  static rs2_stream get_stream_type(const std::string& topic)
254  {
255  auto stream_with_id = get<3>(topic);
256  rs2_stream type;
257  convert(stream_with_id.substr(0, stream_with_id.find_first_of("_")), type);
258  return type;
259  }
260 
261  static uint32_t get_stream_index(const std::string& topic)
262  {
263  auto stream_with_id = get<3>(topic);
264  return get_id(stream_with_id.substr(0, stream_with_id.find_first_of("_") + 1), get<3>(topic));
265  }
266 
268  {
269  return device_serializer::sensor_identifier{ get_device_index(topic), get_sensor_index(topic) };
270  }
271 
273  {
274  return device_serializer::stream_identifier{ get_device_index(topic), get_sensor_index(topic), get_stream_type(topic), get_stream_index(topic) };
275  }
276 
277  static uint32_t get_extrinsic_group_index(const std::string& topic)
278  {
279  return std::stoul(get<5>(topic));
280  }
281 
282  static std::string get_option_name(const std::string& topic)
283  {
284  return get<4>(topic);
285  }
286  static std::string file_version_topic()
287  {
288  return create_from({ "file_version" });
289  }
290  static std::string device_info_topic(uint32_t device_id)
291  {
292  return create_from({ device_prefix(device_id), "info" });
293  }
294  static std::string sensor_info_topic(const device_serializer::sensor_identifier& sensor_id)
295  {
296  return create_from({ device_prefix(sensor_id.device_index), sensor_prefix(sensor_id.sensor_index), "info" });
297  }
299  {
300  return create_from({ stream_full_prefix(stream_id), "info" });
301  }
303  {
304  return create_from({ stream_full_prefix(stream_id), "info", "camera_info" });
305  }
307  {
308  return create_from({ stream_full_prefix(stream_id), "imu_intrinsic" });
309  }
310 
311  /*version 2 and down*/
312  static std::string property_topic(const device_serializer::sensor_identifier& sensor_id)
313  {
314  return create_from({ device_prefix(sensor_id.device_index), sensor_prefix(sensor_id.sensor_index), "property" });
315  }
316 
317  /*version 3 and up*/
318  static std::string option_value_topic(const device_serializer::sensor_identifier& sensor_id, rs2_option option_type)
319  {
320  return create_from({ device_prefix(sensor_id.device_index), sensor_prefix(sensor_id.sensor_index), "option", rs2_option_to_string(option_type), "value" });
321  }
322 
323  /*version 3 and up*/
324  static std::string option_description_topic(const device_serializer::sensor_identifier& sensor_id, rs2_option option_type)
325  {
326  return create_from({ device_prefix(sensor_id.device_index), sensor_prefix(sensor_id.sensor_index), "option", rs2_option_to_string(option_type), "description" });
327  }
328 
330  {
331  assert(stream_id.stream_type == RS2_STREAM_POSE);
332  return create_from({ stream_full_prefix(stream_id), stream_to_ros_type(stream_id.stream_type), "transform", "data" });
333  }
334 
336  {
337  assert(stream_id.stream_type == RS2_STREAM_POSE);
338  return create_from({ stream_full_prefix(stream_id), stream_to_ros_type(stream_id.stream_type), "accel", "data" });
339  }
341  {
342  assert(stream_id.stream_type == RS2_STREAM_POSE);
343  return create_from({ stream_full_prefix(stream_id), stream_to_ros_type(stream_id.stream_type),"twist", "data" });
344  }
345 
347  {
348  return create_from({ stream_full_prefix(stream_id), stream_to_ros_type(stream_id.stream_type), "data" });
349  }
350 
352  {
353  return create_from({ stream_full_prefix(stream_id), stream_to_ros_type(stream_id.stream_type), "metadata" });
354  }
355 
356  static std::string stream_extrinsic_topic(const device_serializer::stream_identifier& stream_id, uint32_t ref_id)
357  {
358  return create_from({ stream_full_prefix(stream_id), "tf", std::to_string(ref_id) });
359  }
360 
361  static std::string additional_info_topic()
362  {
363  return create_from({ "additional_info" });
364  }
365 
367  {
368  return create_from({ device_prefix(stream_id.device_index), sensor_prefix(stream_id.sensor_index), stream_prefix(stream_id.stream_type, stream_id.stream_index) }).substr(1); //substr(1) to remove the first "/"
369  }
370 
372  {
373  return create_from({ device_prefix(sensor_id.device_index), sensor_prefix(sensor_id.sensor_index), "notification", rs2_notification_category_to_string(nc)});
374  }
375 
376  template<uint32_t index>
377  static std::string get(const std::string& value)
378  {
379  size_t current_pos = 0;
380  std::string value_copy = value;
381  uint32_t elements_iterator = 0;
382  const auto seperator_length = std::string(elements_separator()).length();
383  while ((current_pos = value_copy.find(elements_separator())) != std::string::npos)
384  {
385  auto token = value_copy.substr(0, current_pos);
386  if (elements_iterator == index)
387  {
388  return token;
389  }
390  value_copy.erase(0, current_pos + seperator_length);
391  ++elements_iterator;
392  }
393 
394  if (elements_iterator == index)
395  return value_copy;
396 
397  throw std::out_of_range(to_string() << "Requeted index \"" << index << "\" is out of bound of topic: \"" << value << "\"");
398  }
399  private:
400  static std::string stream_to_ros_type(rs2_stream type)
401  {
402  switch (type)
403  {
405  case RS2_STREAM_DEPTH:
406  case RS2_STREAM_COLOR:
407  case RS2_STREAM_INFRARED:
408  case RS2_STREAM_FISHEYE:
409  return ros_image_type_str();
410 
411  case RS2_STREAM_GYRO:
412  case RS2_STREAM_ACCEL:
413  return ros_imu_type_str();
414 
415  case RS2_STREAM_POSE:
416  return ros_pose_type_str();
417  }
418  throw io_exception(to_string() << "Unknown stream type when resolving ros type: " << type);
419  }
420  static std::string create_from(const std::vector<std::string>& parts)
421  {
422  std::ostringstream oss;
423  oss << elements_separator();
424  if (parts.empty() == false)
425  {
426  std::copy(parts.begin(), parts.end() - 1, std::ostream_iterator<std::string>(oss, elements_separator()));
427  oss << parts.back();
428  }
429  return oss.str();
430  }
431 
432 
433  static uint32_t get_id(const std::string& prefix, const std::string& str)
434  {
435  if (str.compare(0, prefix.size(), prefix) != 0)
436  {
437  throw std::runtime_error("Failed to get id after prefix \"" + prefix + "\"from string \"" + str + "\"");
438  }
439 
440  std::string id_str = str.substr(prefix.size());
441  return static_cast<uint32_t>(std::stoll(id_str));
442  }
443 
444  static std::string device_prefix(uint32_t device_id)
445  {
446  return "device_" + std::to_string(device_id);
447  }
448  static std::string sensor_prefix(uint32_t sensor_id)
449  {
450  return "sensor_" + std::to_string(sensor_id);
451  }
452  static std::string stream_prefix(rs2_stream type, uint32_t stream_id)
453  {
454  return std::string(rs2_stream_to_string(type)) + "_" + std::to_string(stream_id);
455  }
456  };
457 
458  class FalseQuery {
459  public:
460  bool operator()(rosbag::ConnectionInfo const* info) const { return false; }
461  };
462 
464  {
465  public:
466  MultipleRegexTopicQuery(const std::vector<std::string>& regexps)
467  {
468  for (auto&& regexp : regexps)
469  {
470  LOG_DEBUG("RegexTopicQuery with expression: " << regexp);
471  _exps.emplace_back(regexp);
472  }
473  }
474 
475  bool operator()(rosbag::ConnectionInfo const* info) const
476  {
477  return std::any_of(std::begin(_exps), std::end(_exps), [info](const std::regex& exp) { return std::regex_search(info->topic, exp); });
478  }
479 
480  private:
481  std::vector<std::regex> _exps;
482  };
483 
485  {
486  public:
487  RegexTopicQuery(std::string const& regexp) : MultipleRegexTopicQuery({ regexp })
488  {
489  }
490 
491  static std::string data_msg_types()
492  { //Either "image" or "imu" or "pose/transform"
493  return to_string() << ros_topic::ros_image_type_str() << "|" << ros_topic::ros_imu_type_str() << "|" << ros_topic::ros_pose_type_str() << "/transform";
494  }
495 
497  {
498  return to_string() << "/device_" << stream_id.device_index
499  << "/sensor_" << stream_id.sensor_index
500  << "/" << get_string(stream_id.stream_type) << "_" << stream_id.stream_index;
501  }
502 
503  private:
504  std::regex _exp;
505  };
506 
508  {
509  public:
510  SensorInfoQuery(uint32_t device_index) : RegexTopicQuery(to_string() << "/device_" << device_index << R"RRR(/sensor_(\d)+/info)RRR") {}
511  };
512 
514  {
515  public:
516  //TODO: Improve readability and robustness of expressions
517  FrameQuery() : RegexTopicQuery(to_string() << R"RRR(/device_\d+/sensor_\d+/.*_\d+)RRR" << "/(" << data_msg_types() << ")/data") {}
518  };
519 
521  {
522  public:
524  RegexTopicQuery(to_string() << stream_prefix(stream_id)
525  << "/(" << data_msg_types() << ")/data")
526  {
527  }
528  };
529 
531  {
532  public:
534  RegexTopicQuery(to_string() << stream_prefix(stream_id) << "/tf")
535  {
536  }
537  };
538 
540  {
541  public:
543  RegexTopicQuery(to_string() << R"RRR(/device_\d+/sensor_\d+/option/.*/value)RRR") {}
544  };
545 
547  {
548  public:
550  RegexTopicQuery(to_string() << R"RRR(/device_\d+/sensor_\d+/notification/.*)RRR") {}
551  };
556  constexpr uint32_t get_file_version()
557  {
558  return 3u;
559  }
560 
562  {
563  return 2u;
564  }
565 
566  constexpr uint32_t get_device_index()
567  {
568  return 0; //TODO: change once SDK file supports multiple devices
569  }
570 
572  {
573  return device_serializer::nanoseconds::min();
574  }
575 
577  {
578  if (t == ros::TIME_MIN)
580 
581  return device_serializer::nanoseconds(t.toNSec());
582  }
583 
584  inline ros::Time to_rostime(const device_serializer::nanoseconds& t)
585  {
587  return ros::TIME_MIN;
588 
589  auto secs = std::chrono::duration_cast<std::chrono::duration<double>>(t);
590  return ros::Time(secs.count());
591  }
592 
593  namespace legacy_file_format
594  {
595  constexpr const char* USB_DESCRIPTOR = "{ 0x94b5fb99, 0x79f2, 0x4d66,{ 0x85, 0x06, 0xb1, 0x5e, 0x8b, 0x8c, 0x9d, 0xa1 } }";
596  constexpr const char* DEVICE_INTERFACE_VERSION = "{ 0xafcd9c11, 0x52e3, 0x4258,{ 0x8d, 0x23, 0xbe, 0x86, 0xfa, 0x97, 0xa0, 0xab } }";
597  constexpr const char* FW_VERSION = "{ 0x7b79605b, 0x5e36, 0x4abe,{ 0xb1, 0x01, 0x94, 0x86, 0xb8, 0x9a, 0xfe, 0xbe } }";
598  constexpr const char* CENTRAL_VERSION = "{ 0x5652ffdb, 0xacac, 0x47ea,{ 0xbf, 0x65, 0x73, 0x3e, 0xf3, 0xd9, 0xe2, 0x70 } }";
599  constexpr const char* CENTRAL_PROTOCOL_VERSION = "{ 0x50376dea, 0x89f4, 0x4d70,{ 0xb1, 0xb0, 0x05, 0xf6, 0x07, 0xb6, 0xae, 0x8a } }";
600  constexpr const char* EEPROM_VERSION = "{ 0x4617d177, 0xb546, 0x4747,{ 0x9d, 0xbf, 0x4f, 0xf8, 0x99, 0x0c, 0x45, 0x6b } }";
601  constexpr const char* ROM_VERSION = "{ 0x16a64010, 0xfee4, 0x4c67,{ 0xae, 0xc5, 0xa0, 0x4d, 0xff, 0x06, 0xeb, 0x0b } }";
602  constexpr const char* TM_DEVICE_TYPE = "{ 0x1212e1d5, 0xfa3e, 0x4273,{ 0x9e, 0xbf, 0xe4, 0x43, 0x87, 0xbe, 0xe5, 0xe8 } }";
603  constexpr const char* HW_VERSION = "{ 0x4439fcca, 0x8673, 0x4851,{ 0x9b, 0xb6, 0x1a, 0xab, 0xbd, 0x74, 0xbd, 0xdc } }";
604  constexpr const char* STATUS = "{ 0x5d6c6637, 0x28c7, 0x4a90,{ 0xab, 0x35, 0x90, 0xb2, 0x1f, 0x1a, 0xe6, 0xb8 } }";
605  constexpr const char* STATUS_CODE = "{ 0xe22a94a6, 0xed64, 0x46ea,{ 0x81, 0x52, 0x6a, 0xb3, 0x0b, 0x0e, 0x2a, 0x18 } }";
606  constexpr const char* EXTENDED_STATUS = "{ 0xff6e50db, 0x3c5f, 0x43e7,{ 0xb4, 0x82, 0xb8, 0xc3, 0xa6, 0x8e, 0x78, 0xcd } }";
607  constexpr const char* SERIAL_NUMBER = "{ 0x7d3e44e7, 0x8970, 0x4a32,{ 0x8e, 0xee, 0xe8, 0xd1, 0xd1, 0x32, 0xa3, 0x22 } }";
608  constexpr const char* TIMESTAMP_SORT_TYPE = "{ 0xb409b217, 0xe5cd, 0x4a04,{ 0x9e, 0x85, 0x1a, 0x7d, 0x59, 0xd7, 0xe5, 0x61 } }";
609 
610  constexpr const char* DEPTH = "DEPTH";
611  constexpr const char* COLOR = "COLOR";
612  constexpr const char* INFRARED = "INFRARED";
613  constexpr const char* FISHEYE = "FISHEYE";
614  constexpr const char* ACCEL = "ACCLEROMETER"; //Yes, there is a typo, that's how it is saved.
615  constexpr const char* GYRO = "GYROMETER";
616  constexpr const char* POSE = "rs_6DoF";
617 
618  constexpr uint32_t actual_exposure = 0; // float RS2_FRAME_METADATA_ACTUAL_EXPOSURE
619  constexpr uint32_t actual_fps = 1; // float
620  constexpr uint32_t frame_counter = 2; // float RS2_FRAME_METADATA_FRAME_COUNTER
621  constexpr uint32_t frame_timestamp = 3; // float RS2_FRAME_METADATA_FRAME_TIMESTAMP
622  constexpr uint32_t sensor_timestamp = 4; // float RS2_FRAME_METADATA_SENSOR_TIMESTAMP
623  constexpr uint32_t gain_level = 5; // float RS2_FRAME_METADATA_GAIN_LEVEL
624  constexpr uint32_t auto_exposure = 6; // float RS2_FRAME_METADATA_AUTO_EXPOSURE
625  constexpr uint32_t white_balance = 7; // float RS2_FRAME_METADATA_WHITE_BALANCE
626  constexpr uint32_t time_of_arrival = 8; // float RS2_FRAME_METADATA_TIME_OF_ARRIVAL
627  constexpr uint32_t SYSTEM_TIMESTAMP = 65536; // int64
628  constexpr uint32_t TEMPRATURE = 65537; // float
629  constexpr uint32_t EXPOSURE_TIME = 65538; // uint32_t
630  constexpr uint32_t FRAME_LENGTH = 65539; // uint32_t
631  constexpr uint32_t ARRIVAL_TIMESTAMP = 65540; // int64
632  constexpr uint32_t CONFIDENCE = 65541; // uint32
633 
634  inline bool convert_metadata_type(uint64_t type, rs2_frame_metadata_value& res)
635  {
636  switch (type)
637  {
638  case actual_exposure: res = RS2_FRAME_METADATA_ACTUAL_EXPOSURE;
639  //Not supported case actual_fps: ;
640  case frame_counter: res = RS2_FRAME_METADATA_FRAME_COUNTER;
641  case frame_timestamp: res = RS2_FRAME_METADATA_FRAME_TIMESTAMP;
642  case sensor_timestamp: res = RS2_FRAME_METADATA_SENSOR_TIMESTAMP;
643  case gain_level: res = RS2_FRAME_METADATA_GAIN_LEVEL;
644  case auto_exposure: res = RS2_FRAME_METADATA_AUTO_EXPOSURE;
645  case white_balance: res = RS2_FRAME_METADATA_WHITE_BALANCE;
646  case time_of_arrival: res = RS2_FRAME_METADATA_TIME_OF_ARRIVAL;
647  //Not supported here case SYSTEM_TIMESTAMP:
648  //Not supported case TEMPRATURE: res = RS2_FRAME_METADATA_;
649  case EXPOSURE_TIME: res = RS2_FRAME_METADATA_SENSOR_TIMESTAMP;
650  //Not supported case FRAME_LENGTH: res = RS2_FRAME_METADATA_;
651  case ARRIVAL_TIMESTAMP: res = RS2_FRAME_METADATA_TIME_OF_ARRIVAL;
652  //Not supported case CONFIDENCE: res = RS2_FRAME_METADATA_;
653  default:
654  return false;
655  }
656  return true;
657  }
658  inline rs2_timestamp_domain convert(uint64_t source)
659  {
660  switch (source)
661  {
662  case 0 /*camera*/: //[[fallthrough]]
663  case 1 /*microcontroller*/ :
665  case 2: return RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME;
666  }
667  throw std::runtime_error(to_string() << "Unknown timestamp domain: " << source);
668  }
669 
670  inline bool info_from_string(const std::string& str, rs2_camera_info& out)
671  {
672  const size_t number_of_hexadecimal_values_in_a_guid = 11;
673  const std::string left_group = R"RE(\s*(0x[0-9a-fA-F]{1,8})\s*,\s*(0x[0-9a-fA-F]{1,4})\s*,\s*(0x[0-9a-fA-F]{1,4})\s*,\s*)RE";
674  const std::string right_group = R"RE(\s*(0x[0-9a-fA-F]{1,2})\s*,\s*(0x[0-9a-fA-F]{1,2})\s*,\s*(0x[0-9a-fA-F]{1,2})\s*,\s*(0x[0-9a-fA-F]{1,2})\s*,\s*(0x[0-9a-fA-F]{1,2})\s*,\s*(0x[0-9a-fA-F]{1,2})\s*,\s*(0x[0-9a-fA-F]{1,2})\s*,\s*(0x[0-9a-fA-F]{1,2})\s*)RE";
675  const std::string guid_regex_pattern = R"RE(\{)RE" + left_group + R"RE(\{)RE" + right_group + R"RE(\}\s*\})RE";
676  //The GUID pattern looks like this: "{ 0x________, 0x____, 0x____, { 0x__, 0x__, 0x__, ... , 0x__ } }"
677 
678  std::regex reg(guid_regex_pattern, std::regex_constants::icase);
679  const std::map<rs2_camera_info, const char*> legacy_infos{
680  { RS2_CAMERA_INFO_SERIAL_NUMBER , SERIAL_NUMBER },
681  { RS2_CAMERA_INFO_FIRMWARE_VERSION , FW_VERSION },
682  { RS2_CAMERA_INFO_PHYSICAL_PORT , USB_DESCRIPTOR },
683  };
684  for (auto&& s : legacy_infos)
685  {
686  if (std::regex_match(s.second, reg))
687  {
688  out = s.first;
689  return true;
690  }
691  }
692  return false;
693  }
694 
695  constexpr uint32_t file_version()
696  {
697  return 1u;
698  }
699 
701  {
702  return file_version();
703  }
704 
705  inline std::string stream_type_to_string(const stream_descriptor& source)
706  {
707  //Other than 6DOF, streams are in the form of "<stream_type><stream_index>" where "stream_index" is empty for index 0/1 and the actual number for 2 and above
708  //6DOF is in the form "rs_6DoF<stream_index>" where "stream_index" is a zero based index
709  std::string name;
710  switch (source.type)
711  {
712  case RS2_STREAM_DEPTH: name = DEPTH; break;
713  case RS2_STREAM_COLOR: name = COLOR; break;
714  case RS2_STREAM_INFRARED: name = INFRARED; break;
715  case RS2_STREAM_FISHEYE: name = FISHEYE; break;
716  case RS2_STREAM_GYRO: name = GYRO; break;
717  case RS2_STREAM_ACCEL: name = ACCEL; break;
718  case RS2_STREAM_POSE: name = POSE; break;
719  default:
720  throw io_exception(to_string() << "Unknown stream type : " << source.type);
721  }
722  if (source.type == RS2_STREAM_POSE)
723  {
724  return name + std::to_string(source.index);
725  }
726  else
727  {
728  if (source.index == 1)
729  {
730  throw io_exception(to_string() << "Unknown index for type : " << source.type << ", index = " << source.index);
731  }
732  return name + (source.index == 0 ? "" : std::to_string(source.index));
733  }
734  }
735 
736  inline stream_descriptor parse_stream_type(const std::string& source)
737  {
738  stream_descriptor retval{};
739  auto starts_with = [source](const std::string& s) {return source.find(s) == 0; };
740  std::string type_str;
741  if (starts_with(DEPTH))
742  {
743  retval.type = RS2_STREAM_DEPTH;
744  type_str = DEPTH;
745  }
746  else if (starts_with(COLOR))
747  {
748  retval.type = RS2_STREAM_COLOR;
749  type_str = COLOR;
750  }
751  else if (starts_with(INFRARED))
752  {
753  retval.type = RS2_STREAM_INFRARED;
754  type_str = INFRARED;
755  }
756  else if (starts_with(FISHEYE))
757  {
758  retval.type = RS2_STREAM_FISHEYE;
759  type_str = FISHEYE;
760  }
761  else if (starts_with(ACCEL))
762  {
763  retval.type = RS2_STREAM_ACCEL;
764  type_str = ACCEL;
765  }
766  else if (starts_with(GYRO))
767  {
768  retval.type = RS2_STREAM_GYRO;
769  type_str = GYRO;
770  }
771  else if (starts_with(POSE))
772  {
773  retval.type = RS2_STREAM_POSE;
774  auto index_str = source.substr(std::string(POSE).length());
775  retval.index = std::stoi(index_str);
776  return retval;
777  }
778  else
779  throw io_exception(to_string() << "Unknown stream type : " << source);
780 
781  auto index_str = source.substr(type_str.length());
782  if (index_str.empty())
783  {
784 
785  retval.index = 0;
786  }
787  else
788  {
789  int index = std::stoi(index_str);
790  assert(index != 1);
791  retval.index = index;
792  }
793  return retval;
794  }
795 
797  {
798  public:
799  //Possible patterns:
800  // /camera/<CAMERA_STREAM_TYPE><id>/image_raw/0
801  // /camera/rs_6DoF<id>/0
802  // /imu/ACCELEROMETER/imu_raw/0
803  // /imu/GYROMETER/imu_raw/0
805  to_string() << R"RRR(/(camera|imu)/.*/(image|imu)_raw/\d+)RRR" ,
806  to_string() << R"RRR(/camera/rs_6DoF\d+/\d+)RRR" }) {}
807  };
808 
809  inline bool is_camera(rs2_stream s)
810  {
811  return
812  s == RS2_STREAM_DEPTH ||
813  s == RS2_STREAM_COLOR ||
814  s == RS2_STREAM_INFRARED ||
815  s == RS2_STREAM_FISHEYE ||
816  s == RS2_STREAM_POSE;
817  }
818 
820  {
821  public:
824  << (is_camera(stream_id.stream_type) ? "/camera/" : "/imu/")
825  << stream_type_to_string({ stream_id.stream_type, (int)stream_id.stream_index})
826  << ((stream_id.stream_type == RS2_STREAM_POSE) ? "/" : (is_camera(stream_id.stream_type)) ? "/image_raw/" : "/imu_raw/")
827  << stream_id.sensor_index)
828  {
829  }
830  };
831 
833  {
834  public:
837  << (is_camera(stream_id.stream_type) ? "/camera/" : "/imu/")
838  << stream_type_to_string({ stream_id.stream_type, (int)stream_id.stream_index })
839  << "/rs_frame_info_ext/" << stream_id.sensor_index)
840  {
841  }
842  };
844  {
845  auto stream = parse_stream_type(ros_topic::get<2>(topic));
846  uint32_t sensor_index;
847  if(stream.type == RS2_STREAM_POSE)
848  sensor_index = static_cast<uint32_t>(std::stoll(ros_topic::get<3>(topic)));
849  else
850  sensor_index = static_cast<uint32_t>(std::stoll(ros_topic::get<4>(topic)));
851 
852  return device_serializer::stream_identifier{ 0u, static_cast<uint32_t>(sensor_index), stream.type, static_cast<uint32_t>(stream.index) };
853  }
854 
855  inline std::string file_version_topic()
856  {
857  return "/FILE_VERSION";
858  }
859  }
860 }
constexpr uint32_t actual_exposure
Definition: ros_file_format.h:618
rs2_camera_info
Read-only strings that can be queried from the device. Not all information attributes are available o...
Definition: rs_sensor.h:22
constexpr uint32_t get_file_version()
Definition: ros_file_format.h:556
static std::string frame_data_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:346
const char * rs2_format_to_string(rs2_format format)
Definition: rs_sensor.h:63
constexpr uint32_t ARRIVAL_TIMESTAMP
Definition: ros_file_format.h:631
Definition: rs_frame.h:33
constexpr uint32_t file_version()
Definition: ros_file_format.h:695
void rot2quat(const float(&r)[9], geometry_msgs::Transform::_rotation_type &q)
Definition: ros_file_format.h:165
Definition: rs_frame.h:32
constexpr const char * DEPTH
Definition: ros_file_format.h:610
Definition: ros_file_format.h:195
Definition: rs_sensor.h:27
constexpr const char * GYRO
Definition: ros_file_format.h:615
Base class that establishes the interface for retrieving metadata attributes.
Definition: metadata-parser.h:28
Definition: backend.h:376
static std::string sensor_info_topic(const device_serializer::sensor_identifier &sensor_id)
Definition: ros_file_format.h:294
Definition: backend.h:380
OptionsQuery()
Definition: ros_file_format.h:542
Definition: ros_file_format.h:507
rs2_option
Defines general configuration controls. These can generally be mapped to camera UVC controls...
Definition: rs_option.h:22
static std::string get_option_name(const std::string &topic)
Definition: ros_file_format.h:282
constexpr uint32_t frame_timestamp
Definition: ros_file_format.h:621
Definition: rs_frame.h:21
constexpr uint32_t white_balance
Definition: ros_file_format.h:625
rs2_distortion
Distortion model: defines how pixel coordinates should be mapped to sensor coordinates.
Definition: rs_types.h:44
Definition: rs_sensor.h:24
constexpr const char * TM_DEVICE_TYPE
Definition: ros_file_format.h:602
float translation[3]
Definition: rs_sensor.h:85
constexpr uint32_t gain_level
Definition: ros_file_format.h:623
const char * rs2_option_to_string(rs2_option option)
constexpr const char * FRAME_TIMESTAMP_MD_STR
Definition: ros_file_format.h:192
constexpr const char * TIMESTAMP_SORT_TYPE
Definition: ros_file_format.h:608
uint32_t sensor_index
Definition: serialization.h:20
bool operator()(rosbag::ConnectionInfo const *info) const
Definition: ros_file_format.h:475
static std::string stream_extrinsic_topic(const device_serializer::stream_identifier &stream_id, uint32_t ref_id)
Definition: ros_file_format.h:356
static uint32_t get_device_index(const std::string &topic)
Definition: ros_file_format.h:243
FrameQuery()
Definition: ros_file_format.h:517
bool operator()(rosbag::ConnectionInfo const *info) const
Definition: ros_file_format.h:460
Definition: rs_sensor.h:44
Definition: rs_sensor.h:58
static uint32_t get_sensor_index(const std::string &topic)
Definition: ros_file_format.h:248
std::string file_version_topic()
Definition: ros_file_format.h:855
Definition: ros_file_format.h:546
sql::statement::iterator end(sql::statement &stmt)
Definition: types.h:489
Definition: rs_sensor.h:48
static std::string video_stream_info_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:302
constexpr const char * ACCEL
Definition: ros_file_format.h:614
constexpr uint32_t actual_fps
Definition: ros_file_format.h:619
void convert(rs2_format source, std::string &target)
Definition: ros_file_format.h:33
constexpr uint32_t FRAME_LENGTH
Definition: ros_file_format.h:630
static std::string additional_info_topic()
Definition: ros_file_format.h:361
static std::string device_info_topic(uint32_t device_id)
Definition: ros_file_format.h:290
RegexTopicQuery(std::string const &regexp)
Definition: ros_file_format.h:487
constexpr const char * TRACKER_CONFIDENCE_MD_STR
Definition: ros_file_format.h:193
Definition: rs_sensor.h:45
Definition: ros_file_format.h:235
Exposes librealsense functionality for C compilers.
bool convert_metadata_type(uint64_t type, rs2_frame_metadata_value &res)
Definition: ros_file_format.h:634
float rotation[9]
Definition: rs_sensor.h:84
Definition: rs_sensor.h:42
frame_additional_data additional_data
Definition: archive.h:67
Definition: types.h:182
static device_serializer::stream_identifier get_stream_identifier(const std::string &topic)
Definition: ros_file_format.h:272
Definition: rs_sensor.h:62
Definition: ros_file_format.h:463
Definition: archive.h:63
Definition: ros_file_format.h:484
Definition: algo.h:16
Definition: rs_frame.h:36
std::string stream_type_to_string(const stream_descriptor &source)
Definition: ros_file_format.h:705
uint32_t device_index
Definition: serialization.h:19
#define LOG_DEBUG(...)
Definition: types.h:108
Definition: rs_frame.h:30
static device_serializer::sensor_identifier get_sensor_identifier(const std::string &topic)
Definition: ros_file_format.h:267
rs2_stream stream_type
Definition: serialization.h:26
constexpr const char * HW_VERSION
Definition: ros_file_format.h:603
sql::statement::iterator begin(sql::statement &stmt)
static constexpr const char * ros_pose_type_str()
Definition: ros_file_format.h:241
constexpr uint32_t get_maximum_supported_legacy_file_version()
Definition: ros_file_format.h:700
constexpr uint32_t frame_counter
Definition: ros_file_format.h:620
static std::string pose_accel_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:335
uint32_t device_index
Definition: serialization.h:24
Definition: rs_sensor.h:49
constexpr uint32_t time_of_arrival
Definition: ros_file_format.h:626
constexpr const char * DEVICE_INTERFACE_VERSION
Definition: ros_file_format.h:596
constexpr const char * EEPROM_VERSION
Definition: ros_file_format.h:600
constexpr const char * SERIAL_NUMBER
Definition: ros_file_format.h:607
static std::string notification_topic(const device_serializer::sensor_identifier &sensor_id, rs2_notification_category nc)
Definition: ros_file_format.h:371
bool is_camera(rs2_stream s)
Definition: ros_file_format.h:809
constexpr const char * FW_VERSION
Definition: ros_file_format.h:597
Definition: rs_frame.h:37
NotificationsQuery()
Definition: ros_file_format.h:549
bool info_from_string(const std::string &str, rs2_camera_info &out)
Definition: ros_file_format.h:670
rs2_format
Format identifies how binary data is encoded within a frame.
Definition: rs_sensor.h:55
static std::string stream_prefix(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:496
static std::string file_version_topic()
Definition: ros_file_format.h:286
Definition: rs_sensor.h:25
uint32_t sensor_index
Definition: serialization.h:25
void quat2rot(const geometry_msgs::Transform::_rotation_type &q, float(&r)[9])
Definition: ros_file_format.h:152
constexpr const char * INFRARED
Definition: ros_file_format.h:612
static constexpr const char * ros_imu_type_str()
Definition: ros_file_format.h:240
static std::string property_topic(const device_serializer::sensor_identifier &sensor_id)
Definition: ros_file_format.h:312
Definition: rs_sensor.h:71
constexpr const char * SYSTEM_TIME_MD_STR
Definition: ros_file_format.h:190
Definition: rs_frame.h:34
static std::string imu_intrinsic_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:306
constexpr const char * STATUS
Definition: ros_file_format.h:604
constexpr const char * MAPPER_CONFIDENCE_MD_STR
Definition: ros_file_format.h:191
constexpr const char * COLOR
Definition: ros_file_format.h:611
Definition: ros_file_format.h:520
Definition: rs_sensor.h:41
device_serializer::nanoseconds to_nanoseconds(const ros::Time &t)
Definition: ros_file_format.h:576
rs2_stream
Streams are different types of data provided by RealSense devices.
Definition: rs_sensor.h:38
constexpr const char * TIMESTAMP_DOMAIN_MD_STR
Definition: ros_file_format.h:189
std::chrono::duration< uint64_t, std::nano > nanoseconds
Definition: serialization.h:46
constexpr const char * EXTENDED_STATUS
Definition: ros_file_format.h:606
ExtrinsicsQuery(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:533
FrameQuery()
Definition: ros_file_format.h:804
constexpr const char * FISHEYE
Definition: ros_file_format.h:613
static std::string pose_twist_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:340
constexpr uint32_t EXPOSURE_TIME
Definition: ros_file_format.h:629
Definition: ros_file_format.h:513
static std::string frame_metadata_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:351
constexpr uint32_t CONFIDENCE
Definition: ros_file_format.h:632
Definition: rs_frame.h:31
Definition: rs_sensor.h:43
device_serializer::stream_identifier get_stream_identifier(const std::string &topic)
Definition: ros_file_format.h:843
int index
Definition: types.h:495
Definition: rs_sensor.h:46
Definition: types.h:55
Cross-stream extrinsics: encode the topology describing how the different devices are connected...
Definition: rs_sensor.h:82
static constexpr const char * ros_image_type_str()
Definition: ros_file_format.h:239
std::string get_string(perc::Status value)
Definition: controller_event_serializer.h:26
stream_descriptor parse_stream_type(const std::string &source)
Definition: ros_file_format.h:736
Definition: ros_file_format.h:530
Definition: ros_file_format.h:458
constexpr device_serializer::nanoseconds get_static_file_info_timestamp()
Definition: ros_file_format.h:571
constexpr const char * USB_DESCRIPTOR
Definition: ros_file_format.h:595
static uint32_t get_stream_index(const std::string &topic)
Definition: ros_file_format.h:261
static std::string option_value_topic(const device_serializer::sensor_identifier &sensor_id, rs2_option option_type)
Definition: ros_file_format.h:318
constexpr uint32_t TEMPRATURE
Definition: ros_file_format.h:628
const char * rs2_stream_to_string(rs2_stream stream)
rs2_notification_category
Category of the librealsense notifications.
Definition: rs_types.h:17
int stream_id
Definition: sync.h:17
bool supports(const frame &frm) const override
Definition: ros_file_format.h:208
long long rs2_metadata_type
Definition: rs_types.h:181
Definition: stream.h:14
constexpr uint32_t get_minimum_supported_file_version()
Definition: ros_file_format.h:561
constexpr const char * CENTRAL_PROTOCOL_VERSION
Definition: ros_file_format.h:599
static uint32_t get_extrinsic_group_index(const std::string &topic)
Definition: ros_file_format.h:277
static rs2_stream get_stream_type(const std::string &topic)
Definition: ros_file_format.h:253
constexpr uint32_t sensor_timestamp
Definition: ros_file_format.h:622
static std::string option_description_topic(const device_serializer::sensor_identifier &sensor_id, rs2_option option_type)
Definition: ros_file_format.h:324
Definition: ros_file_format.h:539
constexpr const char * ROM_VERSION
Definition: ros_file_format.h:601
md_constant_parser(rs2_frame_metadata_value type)
Definition: ros_file_format.h:198
Definition: rs_sensor.h:65
MultipleRegexTopicQuery(const std::vector< std::string > &regexps)
Definition: ros_file_format.h:466
constexpr const char * STATUS_CODE
Definition: ros_file_format.h:605
StreamQuery(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:523
FrameInfoExt(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:835
StreamQuery(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:822
std::array< uint8_t, MAX_META_DATA_SIZE > metadata_blob
Definition: archive.h:29
static constexpr const char * elements_separator()
Definition: ros_file_format.h:238
static std::string pose_transform_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:329
const char * rs2_notification_category_to_string(rs2_notification_category category)
constexpr const char * CENTRAL_VERSION
Definition: ros_file_format.h:598
static std::string data_msg_types()
Definition: ros_file_format.h:491
rs2_log_severity
Severity of the librealsense logger.
Definition: rs_types.h:82
Definition: rs_sensor.h:67
ros::Time to_rostime(const device_serializer::nanoseconds &t)
Definition: ros_file_format.h:584
SensorInfoQuery(uint32_t device_index)
Definition: ros_file_format.h:510
Definition: rs_sensor.h:70
constexpr uint32_t auto_exposure
Definition: ros_file_format.h:624
Definition: rs_frame.h:38
Definition: ros_file_format.h:796
Definition: ros_file_format.h:832
Definition: rs_frame.h:22
rs2_frame_metadata_value
Per-Frame-Metadata are set of read-only properties that might be exposed for each individual frame...
Definition: rs_frame.h:28
uint32_t stream_index
Definition: serialization.h:27
rs2_stream type
Definition: types.h:494
Definition: rs_sensor.h:64
Definition: rs_sensor.h:66
Definition: ros_file_format.h:819
void copy(void *dst, void const *src, size_t size)
constexpr const char * POSE
Definition: ros_file_format.h:616
constexpr uint32_t SYSTEM_TIMESTAMP
Definition: ros_file_format.h:627
constexpr uint32_t get_device_index()
Definition: ros_file_format.h:566
static std::string stream_info_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:298
static std::string stream_full_prefix(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:366
rs2_timestamp_domain
Specifies the clock in relation to which the frame timestamp was measured.
Definition: rs_frame.h:19