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  {
270  }
271 
273  {
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  {
404  case RS2_STREAM_DEPTH:
405  case RS2_STREAM_COLOR:
406  case RS2_STREAM_INFRARED:
407  case RS2_STREAM_FISHEYE:
408  return ros_image_type_str();
409 
410  case RS2_STREAM_GYRO:
411  case RS2_STREAM_ACCEL:
412  return ros_imu_type_str();
413 
414  case RS2_STREAM_POSE:
415  return ros_pose_type_str();
416  }
417  throw io_exception(to_string() << "Unknown stream type when resolving ros type: " << type);
418  }
419  static std::string create_from(const std::vector<std::string>& parts)
420  {
421  std::ostringstream oss;
422  oss << elements_separator();
423  if (parts.empty() == false)
424  {
425  std::copy(parts.begin(), parts.end() - 1, std::ostream_iterator<std::string>(oss, elements_separator()));
426  oss << parts.back();
427  }
428  return oss.str();
429  }
430 
431 
432  static uint32_t get_id(const std::string& prefix, const std::string& str)
433  {
434  if (str.compare(0, prefix.size(), prefix) != 0)
435  {
436  throw std::runtime_error("Failed to get id after prefix \"" + prefix + "\"from string \"" + str + "\"");
437  }
438 
439  std::string id_str = str.substr(prefix.size());
440  return static_cast<uint32_t>(std::stoll(id_str));
441  }
442 
443  static std::string device_prefix(uint32_t device_id)
444  {
445  return "device_" + std::to_string(device_id);
446  }
447  static std::string sensor_prefix(uint32_t sensor_id)
448  {
449  return "sensor_" + std::to_string(sensor_id);
450  }
451  static std::string stream_prefix(rs2_stream type, uint32_t stream_id)
452  {
453  return std::string(rs2_stream_to_string(type)) + "_" + std::to_string(stream_id);
454  }
455  };
456 
457  class FalseQuery {
458  public:
459  bool operator()(rosbag::ConnectionInfo const* info) const { return false; }
460  };
461 
463  {
464  public:
465  MultipleRegexTopicQuery(const std::vector<std::string>& regexps)
466  {
467  for (auto&& regexp : regexps)
468  {
469  LOG_DEBUG("RegexTopicQuery with expression: " << regexp);
470  _exps.emplace_back(regexp);
471  }
472  }
473 
474  bool operator()(rosbag::ConnectionInfo const* info) const
475  {
476  return std::any_of(std::begin(_exps), std::end(_exps), [info](const std::regex& exp) { return std::regex_search(info->topic, exp); });
477  }
478 
479  private:
480  std::vector<std::regex> _exps;
481  };
482 
484  {
485  public:
486  RegexTopicQuery(std::string const& regexp) : MultipleRegexTopicQuery({ regexp })
487  {
488  }
489 
490  static std::string data_msg_types()
491  { //Either "image" or "imu" or "pose/transform"
492  return to_string() << ros_topic::ros_image_type_str() << "|" << ros_topic::ros_imu_type_str() << "|" << ros_topic::ros_pose_type_str() << "/transform";
493  }
494 
496  {
497  return to_string() << "/device_" << stream_id.device_index
498  << "/sensor_" << stream_id.sensor_index
499  << "/" << get_string(stream_id.stream_type) << "_" << stream_id.stream_index;
500  }
501 
502  private:
503  std::regex _exp;
504  };
505 
507  {
508  public:
509  SensorInfoQuery(uint32_t device_index) : RegexTopicQuery(to_string() << "/device_" << device_index << R"RRR(/sensor_(\d)+/info)RRR") {}
510  };
511 
513  {
514  public:
515  //TODO: Improve readability and robustness of expressions
516  FrameQuery() : RegexTopicQuery(to_string() << R"RRR(/device_\d+/sensor_\d+/.*_\d+)RRR" << "/(" << data_msg_types() << ")/data") {}
517  };
518 
520  {
521  public:
524  << "/(" << data_msg_types() << ")/data")
525  {
526  }
527  };
528 
530  {
531  public:
534  {
535  }
536  };
537 
539  {
540  public:
542  RegexTopicQuery(to_string() << R"RRR(/device_\d+/sensor_\d+/option/.*/value)RRR") {}
543  };
544 
546  {
547  public:
549  RegexTopicQuery(to_string() << R"RRR(/device_\d+/sensor_\d+/notification/.*)RRR") {}
550  };
555  constexpr uint32_t get_file_version()
556  {
557  return 3u;
558  }
559 
561  {
562  return 2u;
563  }
564 
565  constexpr uint32_t get_device_index()
566  {
567  return 0; //TODO: change once SDK file supports multiple devices
568  }
569 
571  {
572  return device_serializer::nanoseconds::min();
573  }
574 
576  {
577  if (t == ros::TIME_MIN)
579 
580  return device_serializer::nanoseconds(t.toNSec());
581  }
582 
583  inline ros::Time to_rostime(const device_serializer::nanoseconds& t)
584  {
586  return ros::TIME_MIN;
587 
588  auto secs = std::chrono::duration_cast<std::chrono::duration<double>>(t);
589  return ros::Time(secs.count());
590  }
591 
592  namespace legacy_file_format
593  {
594  constexpr const char* USB_DESCRIPTOR = "{ 0x94b5fb99, 0x79f2, 0x4d66,{ 0x85, 0x06, 0xb1, 0x5e, 0x8b, 0x8c, 0x9d, 0xa1 } }";
595  constexpr const char* DEVICE_INTERFACE_VERSION = "{ 0xafcd9c11, 0x52e3, 0x4258,{ 0x8d, 0x23, 0xbe, 0x86, 0xfa, 0x97, 0xa0, 0xab } }";
596  constexpr const char* FW_VERSION = "{ 0x7b79605b, 0x5e36, 0x4abe,{ 0xb1, 0x01, 0x94, 0x86, 0xb8, 0x9a, 0xfe, 0xbe } }";
597  constexpr const char* CENTRAL_VERSION = "{ 0x5652ffdb, 0xacac, 0x47ea,{ 0xbf, 0x65, 0x73, 0x3e, 0xf3, 0xd9, 0xe2, 0x70 } }";
598  constexpr const char* CENTRAL_PROTOCOL_VERSION = "{ 0x50376dea, 0x89f4, 0x4d70,{ 0xb1, 0xb0, 0x05, 0xf6, 0x07, 0xb6, 0xae, 0x8a } }";
599  constexpr const char* EEPROM_VERSION = "{ 0x4617d177, 0xb546, 0x4747,{ 0x9d, 0xbf, 0x4f, 0xf8, 0x99, 0x0c, 0x45, 0x6b } }";
600  constexpr const char* ROM_VERSION = "{ 0x16a64010, 0xfee4, 0x4c67,{ 0xae, 0xc5, 0xa0, 0x4d, 0xff, 0x06, 0xeb, 0x0b } }";
601  constexpr const char* TM_DEVICE_TYPE = "{ 0x1212e1d5, 0xfa3e, 0x4273,{ 0x9e, 0xbf, 0xe4, 0x43, 0x87, 0xbe, 0xe5, 0xe8 } }";
602  constexpr const char* HW_VERSION = "{ 0x4439fcca, 0x8673, 0x4851,{ 0x9b, 0xb6, 0x1a, 0xab, 0xbd, 0x74, 0xbd, 0xdc } }";
603  constexpr const char* STATUS = "{ 0x5d6c6637, 0x28c7, 0x4a90,{ 0xab, 0x35, 0x90, 0xb2, 0x1f, 0x1a, 0xe6, 0xb8 } }";
604  constexpr const char* STATUS_CODE = "{ 0xe22a94a6, 0xed64, 0x46ea,{ 0x81, 0x52, 0x6a, 0xb3, 0x0b, 0x0e, 0x2a, 0x18 } }";
605  constexpr const char* EXTENDED_STATUS = "{ 0xff6e50db, 0x3c5f, 0x43e7,{ 0xb4, 0x82, 0xb8, 0xc3, 0xa6, 0x8e, 0x78, 0xcd } }";
606  constexpr const char* SERIAL_NUMBER = "{ 0x7d3e44e7, 0x8970, 0x4a32,{ 0x8e, 0xee, 0xe8, 0xd1, 0xd1, 0x32, 0xa3, 0x22 } }";
607  constexpr const char* TIMESTAMP_SORT_TYPE = "{ 0xb409b217, 0xe5cd, 0x4a04,{ 0x9e, 0x85, 0x1a, 0x7d, 0x59, 0xd7, 0xe5, 0x61 } }";
608 
609  constexpr const char* DEPTH = "DEPTH";
610  constexpr const char* COLOR = "COLOR";
611  constexpr const char* INFRARED = "INFRARED";
612  constexpr const char* FISHEYE = "FISHEYE";
613  constexpr const char* ACCEL = "ACCLEROMETER"; //Yes, there is a typo, that's how it is saved.
614  constexpr const char* GYRO = "GYROMETER";
615  constexpr const char* POSE = "rs_6DoF";
616 
617  constexpr uint32_t actual_exposure = 0; // float RS2_FRAME_METADATA_ACTUAL_EXPOSURE
618  constexpr uint32_t actual_fps = 1; // float
619  constexpr uint32_t frame_counter = 2; // float RS2_FRAME_METADATA_FRAME_COUNTER
620  constexpr uint32_t frame_timestamp = 3; // float RS2_FRAME_METADATA_FRAME_TIMESTAMP
621  constexpr uint32_t sensor_timestamp = 4; // float RS2_FRAME_METADATA_SENSOR_TIMESTAMP
622  constexpr uint32_t gain_level = 5; // float RS2_FRAME_METADATA_GAIN_LEVEL
623  constexpr uint32_t auto_exposure = 6; // float RS2_FRAME_METADATA_AUTO_EXPOSURE
624  constexpr uint32_t white_balance = 7; // float RS2_FRAME_METADATA_WHITE_BALANCE
625  constexpr uint32_t time_of_arrival = 8; // float RS2_FRAME_METADATA_TIME_OF_ARRIVAL
626  constexpr uint32_t SYSTEM_TIMESTAMP = 65536; // int64
627  constexpr uint32_t TEMPRATURE = 65537; // float
628  constexpr uint32_t EXPOSURE_TIME = 65538; // uint32_t
629  constexpr uint32_t FRAME_LENGTH = 65539; // uint32_t
630  constexpr uint32_t ARRIVAL_TIMESTAMP = 65540; // int64
631  constexpr uint32_t CONFIDENCE = 65541; // uint32
632 
633  inline bool convert_metadata_type(uint64_t type, rs2_frame_metadata_value& res)
634  {
635  switch (type)
636  {
638  //Not supported case actual_fps: ;
646  //Not supported here case SYSTEM_TIMESTAMP:
647  //Not supported case TEMPRATURE: res = RS2_FRAME_METADATA_;
649  //Not supported case FRAME_LENGTH: res = RS2_FRAME_METADATA_;
651  //Not supported case CONFIDENCE: res = RS2_FRAME_METADATA_;
652  default:
653  return false;
654  }
655  return true;
656  }
657  inline rs2_timestamp_domain convert(uint64_t source)
658  {
659  switch (source)
660  {
661  case 0 /*camera*/: //[[fallthrough]]
662  case 1 /*microcontroller*/ :
664  case 2: return RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME;
665  }
666  throw std::runtime_error(to_string() << "Unknown timestamp domain: " << source);
667  }
668 
669  inline bool info_from_string(const std::string& str, rs2_camera_info& out)
670  {
671  const size_t number_of_hexadecimal_values_in_a_guid = 11;
672  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";
673  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";
674  const std::string guid_regex_pattern = R"RE(\{)RE" + left_group + R"RE(\{)RE" + right_group + R"RE(\}\s*\})RE";
675  //The GUID pattern looks like this: "{ 0x________, 0x____, 0x____, { 0x__, 0x__, 0x__, ... , 0x__ } }"
676 
677  std::regex reg(guid_regex_pattern, std::regex_constants::icase);
678  const std::map<rs2_camera_info, const char*> legacy_infos{
682  };
683  for (auto&& s : legacy_infos)
684  {
685  if (std::regex_match(s.second, reg))
686  {
687  out = s.first;
688  return true;
689  }
690  }
691  return false;
692  }
693 
694  constexpr uint32_t file_version()
695  {
696  return 1u;
697  }
698 
700  {
701  return file_version();
702  }
703 
704  inline std::string stream_type_to_string(const stream_descriptor& source)
705  {
706  //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
707  //6DOF is in the form "rs_6DoF<stream_index>" where "stream_index" is a zero based index
708  std::string name;
709  switch (source.type)
710  {
711  case RS2_STREAM_DEPTH: name = DEPTH; break;
712  case RS2_STREAM_COLOR: name = COLOR; break;
713  case RS2_STREAM_INFRARED: name = INFRARED; break;
714  case RS2_STREAM_FISHEYE: name = FISHEYE; break;
715  case RS2_STREAM_GYRO: name = GYRO; break;
716  case RS2_STREAM_ACCEL: name = ACCEL; break;
717  case RS2_STREAM_POSE: name = POSE; break;
718  default:
719  throw io_exception(to_string() << "Unknown stream type : " << source.type);
720  }
721  if (source.type == RS2_STREAM_POSE)
722  {
723  return name + std::to_string(source.index);
724  }
725  else
726  {
727  if (source.index == 1)
728  {
729  throw io_exception(to_string() << "Unknown index for type : " << source.type << ", index = " << source.index);
730  }
731  return name + (source.index == 0 ? "" : std::to_string(source.index));
732  }
733  }
734 
735  inline stream_descriptor parse_stream_type(const std::string& source)
736  {
737  stream_descriptor retval{};
738  auto starts_with = [source](const std::string& s) {return source.find(s) == 0; };
739  std::string type_str;
740  if (starts_with(DEPTH))
741  {
742  retval.type = RS2_STREAM_DEPTH;
743  type_str = DEPTH;
744  }
745  else if (starts_with(COLOR))
746  {
747  retval.type = RS2_STREAM_COLOR;
748  type_str = COLOR;
749  }
750  else if (starts_with(INFRARED))
751  {
752  retval.type = RS2_STREAM_INFRARED;
753  type_str = INFRARED;
754  }
755  else if (starts_with(FISHEYE))
756  {
757  retval.type = RS2_STREAM_FISHEYE;
758  type_str = FISHEYE;
759  }
760  else if (starts_with(ACCEL))
761  {
762  retval.type = RS2_STREAM_ACCEL;
763  type_str = ACCEL;
764  }
765  else if (starts_with(GYRO))
766  {
767  retval.type = RS2_STREAM_GYRO;
768  type_str = GYRO;
769  }
770  else if (starts_with(POSE))
771  {
772  retval.type = RS2_STREAM_POSE;
773  auto index_str = source.substr(std::string(POSE).length());
774  retval.index = std::stoi(index_str);
775  return retval;
776  }
777  else
778  throw io_exception(to_string() << "Unknown stream type : " << source);
779 
780  auto index_str = source.substr(type_str.length());
781  if (index_str.empty())
782  {
783 
784  retval.index = 0;
785  }
786  else
787  {
788  int index = std::stoi(index_str);
789  assert(index != 1);
790  retval.index = index;
791  }
792  return retval;
793  }
794 
796  {
797  public:
798  //Possible patterns:
799  // /camera/<CAMERA_STREAM_TYPE><id>/image_raw/0
800  // /camera/rs_6DoF<id>/0
801  // /imu/ACCELEROMETER/imu_raw/0
802  // /imu/GYROMETER/imu_raw/0
804  to_string() << R"RRR(/(camera|imu)/.*/(image|imu)_raw/\d+)RRR" ,
805  to_string() << R"RRR(/camera/rs_6DoF\d+/\d+)RRR" }) {}
806  };
807 
808  inline bool is_camera(rs2_stream s)
809  {
810  return
811  s == RS2_STREAM_DEPTH ||
812  s == RS2_STREAM_COLOR ||
813  s == RS2_STREAM_INFRARED ||
814  s == RS2_STREAM_FISHEYE ||
815  s == RS2_STREAM_POSE;
816  }
817 
819  {
820  public:
823  << (is_camera(stream_id.stream_type) ? "/camera/" : "/imu/")
824  << stream_type_to_string({ stream_id.stream_type, (int)stream_id.stream_index})
825  << ((stream_id.stream_type == RS2_STREAM_POSE) ? "/" : (is_camera(stream_id.stream_type)) ? "/image_raw/" : "/imu_raw/")
826  << stream_id.sensor_index)
827  {
828  }
829  };
830 
832  {
833  public:
836  << (is_camera(stream_id.stream_type) ? "/camera/" : "/imu/")
837  << stream_type_to_string({ stream_id.stream_type, (int)stream_id.stream_index })
838  << "/rs_frame_info_ext/" << stream_id.sensor_index)
839  {
840  }
841  };
843  {
844  auto stream = parse_stream_type(ros_topic::get<2>(topic));
845  uint32_t sensor_index;
846  if(stream.type == RS2_STREAM_POSE)
847  sensor_index = static_cast<uint32_t>(std::stoll(ros_topic::get<3>(topic)));
848  else
849  sensor_index = static_cast<uint32_t>(std::stoll(ros_topic::get<4>(topic)));
850 
851  return device_serializer::stream_identifier{ 0u, static_cast<uint32_t>(sensor_index), stream.type, static_cast<uint32_t>(stream.index) };
852  }
853 
854  inline std::string file_version_topic()
855  {
856  return "/FILE_VERSION";
857  }
858  }
859 }
constexpr uint32_t actual_exposure
Definition: ros_file_format.h:617
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:555
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:60
constexpr uint32_t ARRIVAL_TIMESTAMP
Definition: ros_file_format.h:630
Definition: rs_frame.h:33
constexpr uint32_t file_version()
Definition: ros_file_format.h:694
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:609
Definition: ros_file_format.h:195
Definition: rs_sensor.h:26
constexpr const char * GYRO
Definition: ros_file_format.h:614
Base class that establishes the interface for retrieving metadata attributes.
Definition: metadata-parser.h:28
Definition: backend.h:347
static std::string sensor_info_topic(const device_serializer::sensor_identifier &sensor_id)
Definition: ros_file_format.h:294
Definition: backend.h:351
OptionsQuery()
Definition: ros_file_format.h:541
Definition: ros_file_format.h:506
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:620
Definition: rs_frame.h:21
constexpr uint32_t white_balance
Definition: ros_file_format.h:624
rs2_distortion
Distortion model: defines how pixel coordinates should be mapped to sensor coordinates.
Definition: rs_types.h:43
Definition: rs_sensor.h:24
constexpr const char * TM_DEVICE_TYPE
Definition: ros_file_format.h:601
float translation[3]
Definition: rs_sensor.h:82
constexpr uint32_t gain_level
Definition: ros_file_format.h:622
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:607
uint32_t sensor_index
Definition: serialization.h:20
bool operator()(rosbag::ConnectionInfo const *info) const
Definition: ros_file_format.h:474
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:516
bool operator()(rosbag::ConnectionInfo const *info) const
Definition: ros_file_format.h:459
Definition: rs_sensor.h:42
Definition: rs_sensor.h:55
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:854
Definition: ros_file_format.h:545
sql::statement::iterator end(sql::statement &stmt)
Definition: types.h:488
Definition: rs_sensor.h:46
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:613
constexpr uint32_t actual_fps
Definition: ros_file_format.h:618
void convert(rs2_format source, std::string &target)
Definition: ros_file_format.h:33
constexpr uint32_t FRAME_LENGTH
Definition: ros_file_format.h:629
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:486
constexpr const char * TRACKER_CONFIDENCE_MD_STR
Definition: ros_file_format.h:193
Definition: rs_sensor.h:43
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:633
float rotation[9]
Definition: rs_sensor.h:81
Definition: rs_sensor.h:40
frame_additional_data additional_data
Definition: archive.h:67
Definition: types.h:181
static device_serializer::stream_identifier get_stream_identifier(const std::string &topic)
Definition: ros_file_format.h:272
Definition: rs_sensor.h:59
Definition: ros_file_format.h:462
Definition: archive.h:63
Definition: ros_file_format.h:483
Definition: algo.h:16
Definition: rs_frame.h:36
std::string stream_type_to_string(const stream_descriptor &source)
Definition: ros_file_format.h:704
uint32_t device_index
Definition: serialization.h:19
#define LOG_DEBUG(...)
Definition: types.h:107
Definition: rs_frame.h:30
static device_serializer::sensor_identifier get_sensor_identifier(const std::string &topic)
Definition: ros_file_format.h:267
constexpr const char * HW_VERSION
Definition: ros_file_format.h:602
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:699
constexpr uint32_t frame_counter
Definition: ros_file_format.h:619
static std::string pose_accel_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:335
constexpr uint32_t time_of_arrival
Definition: ros_file_format.h:625
constexpr const char * DEVICE_INTERFACE_VERSION
Definition: ros_file_format.h:595
constexpr const char * EEPROM_VERSION
Definition: ros_file_format.h:599
constexpr const char * SERIAL_NUMBER
Definition: ros_file_format.h:606
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:808
constexpr const char * FW_VERSION
Definition: ros_file_format.h:596
Definition: rs_frame.h:37
NotificationsQuery()
Definition: ros_file_format.h:548
bool info_from_string(const std::string &str, rs2_camera_info &out)
Definition: ros_file_format.h:669
rs2_format
Format identifies how binary data is encoded within a frame.
Definition: rs_sensor.h:52
static std::string stream_prefix(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:495
static std::string file_version_topic()
Definition: ros_file_format.h:286
Definition: rs_sensor.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:611
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:68
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:603
constexpr const char * MAPPER_CONFIDENCE_MD_STR
Definition: ros_file_format.h:191
constexpr const char * COLOR
Definition: ros_file_format.h:610
Definition: ros_file_format.h:519
Definition: rs_sensor.h:39
device_serializer::nanoseconds to_nanoseconds(const ros::Time &t)
Definition: ros_file_format.h:575
rs2_stream
Streams are different types of data provided by RealSense devices.
Definition: rs_sensor.h:36
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:605
ExtrinsicsQuery(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:532
FrameQuery()
Definition: ros_file_format.h:803
constexpr const char * FISHEYE
Definition: ros_file_format.h:612
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:628
Definition: ros_file_format.h:512
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:631
Definition: rs_frame.h:31
Definition: rs_sensor.h:41
device_serializer::stream_identifier get_stream_identifier(const std::string &topic)
Definition: ros_file_format.h:842
int index
Definition: types.h:494
Definition: rs_sensor.h:44
Definition: types.h:54
Cross-stream extrinsics: encode the topology describing how the different devices are connected...
Definition: rs_sensor.h:79
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:735
Definition: ros_file_format.h:529
Definition: ros_file_format.h:457
constexpr device_serializer::nanoseconds get_static_file_info_timestamp()
Definition: ros_file_format.h:570
constexpr const char * USB_DESCRIPTOR
Definition: ros_file_format.h:594
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:627
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:180
Definition: stream.h:14
constexpr uint32_t get_minimum_supported_file_version()
Definition: ros_file_format.h:560
rs2_timestamp_domain convert(uint64_t source)
Definition: ros_file_format.h:657
constexpr const char * CENTRAL_PROTOCOL_VERSION
Definition: ros_file_format.h:598
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:621
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:538
constexpr const char * ROM_VERSION
Definition: ros_file_format.h:600
md_constant_parser(rs2_frame_metadata_value type)
Definition: ros_file_format.h:198
Definition: rs_sensor.h:62
MultipleRegexTopicQuery(const std::vector< std::string > &regexps)
Definition: ros_file_format.h:465
constexpr const char * STATUS_CODE
Definition: ros_file_format.h:604
StreamQuery(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:522
FrameInfoExt(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:834
StreamQuery(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:821
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:597
static std::string data_msg_types()
Definition: ros_file_format.h:490
rs2_log_severity
Severity of the librealsense logger.
Definition: rs_types.h:81
Definition: rs_sensor.h:64
ros::Time to_rostime(const device_serializer::nanoseconds &t)
Definition: ros_file_format.h:583
SensorInfoQuery(uint32_t device_index)
Definition: ros_file_format.h:509
Definition: rs_sensor.h:67
constexpr uint32_t auto_exposure
Definition: ros_file_format.h:623
Definition: rs_frame.h:38
Definition: ros_file_format.h:795
Definition: ros_file_format.h:831
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
rs2_stream type
Definition: types.h:493
Definition: rs_sensor.h:61
Definition: rs_sensor.h:63
Definition: ros_file_format.h:818
void copy(void *dst, void const *src, size_t size)
constexpr const char * POSE
Definition: ros_file_format.h:615
constexpr uint32_t SYSTEM_TIMESTAMP
Definition: ros_file_format.h:626
constexpr uint32_t get_device_index()
Definition: ros_file_format.h:565
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