Intel® RealSense™ Cross Platform API
Intel Realsense Cross-platform API
ros_writer.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 
6 #include <string>
7 #include <memory>
8 #include <iomanip>
9 #include <ios> //For std::hexfloat
10 #include "core/debug.h"
11 #include "core/serialization.h"
12 #include "archive.h"
13 #include "types.h"
14 #include "stream.h"
15 #include "rosbag/bag.h"
16 #include "ros_file_format.h"
17 
18 namespace librealsense
19 {
20  using namespace device_serializer;
21 
22  class ros_writer: public writer
23  {
24  public:
25  explicit ros_writer(const std::string& file) : m_file_path(file)
26  {
27  m_bag.open(file, rosbag::BagMode::Write);
28  m_bag.setCompression(rosbag::CompressionType::LZ4);
29  write_file_version();
30  }
31 
32  void write_device_description(const librealsense::device_snapshot& device_description) override
33  {
34  for (auto&& device_extension_snapshot : device_description.get_device_extensions_snapshots().get_snapshots())
35  {
36  write_extension_snapshot(get_device_index(), get_static_file_info_timestamp(), device_extension_snapshot.first, device_extension_snapshot.second);
37  }
38 
39  for (auto&& sensors_snapshot : device_description.get_sensors_snapshots())
40  {
41  for (auto&& sensor_extension_snapshot : sensors_snapshot.get_sensor_extensions_snapshots().get_snapshots())
42  {
43  write_extension_snapshot(get_device_index(), sensors_snapshot.get_sensor_index(), get_static_file_info_timestamp(), sensor_extension_snapshot.first, sensor_extension_snapshot.second);
44  }
45  }
46  }
47 
49  {
50  if (Is<video_frame>(frame.frame))
51  {
52  write_video_frame(stream_id, timestamp, std::move(frame));
53  return;
54  }
55 
56  if (Is<motion_frame>(frame.frame))
57  {
58  write_motion_frame(stream_id, timestamp, std::move(frame));
59  return;
60  }
61 
62  if (Is<pose_frame>(frame.frame))
63  {
64  write_pose_frame(stream_id, timestamp, std::move(frame));
65  return;
66  }
67  }
68 
69  void write_snapshot(uint32_t device_index, const nanoseconds& timestamp, rs2_extension type, const std::shared_ptr<extension_snapshot>& snapshot) override
70  {
71  write_extension_snapshot(device_index, -1, timestamp, type, snapshot);
72  }
73 
74  void write_snapshot(const sensor_identifier& sensor_id, const nanoseconds& timestamp, rs2_extension type, const std::shared_ptr<extension_snapshot>& snapshot) override
75  {
76  write_extension_snapshot(sensor_id.device_index, sensor_id.sensor_index, timestamp, type, snapshot);
77  }
78 
79  const std::string& get_file_name() const override
80  {
81  return m_file_path;
82  }
83 
84  private:
85  void write_file_version()
86  {
87  std_msgs::UInt32 msg;
88  msg.data = get_file_version();
90  }
91 
92 
93  void write_frame_metadata(const stream_identifier& stream_id, const nanoseconds& timestamp, frame_interface* frame)
94  {
95  auto metadata_topic = ros_topic::frame_metadata_topic(stream_id);
96  diagnostic_msgs::KeyValue system_time;
97  system_time.key = SYSTEM_TIME_MD_STR;
98  system_time.value = std::to_string(frame->get_frame_system_time());
99  write_message(metadata_topic, timestamp, system_time);
100 
101  diagnostic_msgs::KeyValue timestamp_domain;
102  timestamp_domain.key = TIMESTAMP_DOMAIN_MD_STR;
103  timestamp_domain.value = librealsense::get_string(frame->get_frame_timestamp_domain());
104  write_message(metadata_topic, timestamp, timestamp_domain);
105 
106  for (int i = 0; i < static_cast<rs2_frame_metadata_value>(rs2_frame_metadata_value::RS2_FRAME_METADATA_COUNT); i++)
107  {
108  rs2_frame_metadata_value type = static_cast<rs2_frame_metadata_value>(i);
109  if (frame->supports_frame_metadata(type))
110  {
111  auto md = frame->get_frame_metadata(type);
112  diagnostic_msgs::KeyValue md_msg;
113  md_msg.key = librealsense::get_string(type);
114  md_msg.value = std::to_string(md);
115  write_message(metadata_topic, timestamp, md_msg);
116  }
117  }
118  }
119 
120  void write_extrinsics(const stream_identifier& stream_id, frame_interface* frame)
121  {
122  if (m_extrinsics_msgs.find(stream_id) != m_extrinsics_msgs.end())
123  {
124  return; //already wrote it
125  }
126  auto& dev = frame->get_sensor()->get_device();
127  uint32_t reference_id = 0;
128  rs2_extrinsics ext;
129  std::tie(reference_id, ext) = dev.get_extrinsics(*frame->get_stream());
130  geometry_msgs::Transform tf_msg;
131  convert(ext, tf_msg);
132  write_message(ros_topic::stream_extrinsic_topic(stream_id, reference_id), get_static_file_info_timestamp(), tf_msg);
133  m_extrinsics_msgs[stream_id] = tf_msg;
134  }
135 
136  realsense_msgs::Notification to_notification_msg(const notification& n)
137  {
138  realsense_msgs::Notification noti_msg;
139  noti_msg.category = get_string(n.category);
140  noti_msg.severity = get_string(n.severity);
141  noti_msg.description = n.description;
142  auto secs = std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::duration<double, std::nano>(n.timestamp));
143  noti_msg.timestamp = ros::Time(secs.count());
144  noti_msg.serialized_data = n.serialized_data;
145  return noti_msg;
146  }
147 
148  void write_notification(const sensor_identifier& sensor_id, const nanoseconds& timestamp, const notification& n)
149  {
150  realsense_msgs::Notification noti_msg = to_notification_msg(n);
151  write_message(ros_topic::notification_topic({ sensor_id.device_index, sensor_id.sensor_index}, n.category), timestamp, noti_msg);
152  }
153 
154  void write_additional_frame_messages(const stream_identifier& stream_id, const nanoseconds& timestamp, frame_interface* frame)
155  {
156  try
157  {
158  write_frame_metadata(stream_id, timestamp, frame);
159  }
160  catch (std::exception const& e)
161  {
162  LOG_WARNING("Failed to write frame metadata for " << stream_id << ". Exception: " << e.what());
163  }
164 
165  try
166  {
167  write_extrinsics(stream_id, frame);
168  }
169  catch (std::exception const& e)
170  {
171  LOG_WARNING("Failed to write stream extrinsics for " << stream_id << ". Exception: " << e.what());
172  }
173  }
174 
175  void write_video_frame(const stream_identifier& stream_id, const nanoseconds& timestamp, frame_holder&& frame)
176  {
177  sensor_msgs::Image image;
178  auto vid_frame = dynamic_cast<librealsense::video_frame*>(frame.frame);
179  assert(vid_frame != nullptr);
180 
181  image.width = static_cast<uint32_t>(vid_frame->get_width());
182  image.height = static_cast<uint32_t>(vid_frame->get_height());
183  image.step = static_cast<uint32_t>(vid_frame->get_stride());
184  convert(vid_frame->get_stream()->get_format(), image.encoding);
185  image.is_bigendian = is_big_endian();
186  auto size = vid_frame->get_stride() * vid_frame->get_height();
187  auto p_data = vid_frame->get_frame_data();
188  image.data.assign(p_data, p_data + size);
189  image.header.seq = static_cast<uint32_t>(vid_frame->get_frame_number());
190  std::chrono::duration<double, std::milli> timestamp_ms(vid_frame->get_frame_timestamp());
191  image.header.stamp = ros::Time(std::chrono::duration<double>(timestamp_ms).count());
192  std::string TODO_CORRECT_ME = "0";
193  image.header.frame_id = TODO_CORRECT_ME;
194  auto image_topic = ros_topic::frame_data_topic(stream_id);
195  write_message(image_topic, timestamp, image);
196  write_additional_frame_messages(stream_id, timestamp, frame);
197  }
198 
199  void write_motion_frame(const stream_identifier& stream_id, const nanoseconds& timestamp, frame_holder&& frame)
200  {
201  sensor_msgs::Imu imu_msg;
202  if (!frame)
203  {
204  throw io_exception("Null frame passed to write_motion_frame");
205  }
206 
207  imu_msg.header.seq = static_cast<uint32_t>(frame.frame->get_frame_number());
208  std::chrono::duration<double, std::milli> timestamp_ms(frame.frame->get_frame_timestamp());
209  imu_msg.header.stamp = ros::Time(std::chrono::duration<double>(timestamp_ms).count());
210  std::string TODO_CORRECT_ME = "0";
211  imu_msg.header.frame_id = TODO_CORRECT_ME;
212  auto data_ptr = reinterpret_cast<const float*>(frame.frame->get_frame_data());
213  if (stream_id.stream_type == RS2_STREAM_ACCEL)
214  {
215  imu_msg.linear_acceleration.x = data_ptr[0];
216  imu_msg.linear_acceleration.y = data_ptr[1];
217  imu_msg.linear_acceleration.z = data_ptr[2];
218  }
219 
220  else if (stream_id.stream_type == RS2_STREAM_GYRO)
221  {
222  imu_msg.angular_velocity.x = data_ptr[0];
223  imu_msg.angular_velocity.y = data_ptr[1];
224  imu_msg.angular_velocity.z = data_ptr[2];
225  }
226  else
227  {
228  throw io_exception("Unsupported stream type for a motion frame");
229  }
230 
232  write_message(topic, timestamp, imu_msg);
233  write_additional_frame_messages(stream_id, timestamp, frame);
234  }
235 
236  inline geometry_msgs::Vector3 to_vector3(const float3& f)
237  {
238  geometry_msgs::Vector3 v;
239  v.x = f.x;
240  v.y = f.y;
241  v.z = f.z;
242  return v;
243  }
244 
245  inline geometry_msgs::Quaternion to_quaternion(const float4& f)
246  {
247  geometry_msgs::Quaternion q;
248  q.x = f.x;
249  q.y = f.y;
250  q.z = f.z;
251  q.w = f.w;
252  return q;
253  }
254 
255  void write_pose_frame(const stream_identifier& stream_id, const nanoseconds& timestamp, frame_holder&& frame)
256  {
257  auto pose = As<librealsense::pose_frame>(frame.frame);
258  if (!frame)
259  {
260  throw io_exception("Null frame passed to write_motion_frame");
261  }
262  auto rotation = pose->get_rotation();
263 
264  geometry_msgs::Transform transform;
265  geometry_msgs::Accel accel;
266  geometry_msgs::Twist twist;
267 
268  transform.rotation = to_quaternion(pose->get_rotation());
269  transform.translation = to_vector3(pose->get_translation());
270  accel.angular = to_vector3(pose->get_angular_acceleration());
271  accel.linear = to_vector3(pose->get_acceleration());
272  twist.angular = to_vector3(pose->get_angular_velocity());
273  twist.linear = to_vector3(pose->get_velocity());
274 
275  std::string transform_topic = ros_topic::pose_transform_topic(stream_id);
276  std::string accel_topic = ros_topic::pose_accel_topic(stream_id);
277  std::string twist_topic = ros_topic::pose_twist_topic(stream_id);
278 
279  //Write the the pose frame as 3 seperate messages (each with different topic)
280  write_message(transform_topic, timestamp, transform);
281  write_message(accel_topic, timestamp, accel);
282  write_message(twist_topic, timestamp, twist);
283 
284  // Write the pose confidence as metadata for the pose frame
285  std::string md_topic = ros_topic::frame_metadata_topic(stream_id);
286 
287  diagnostic_msgs::KeyValue tracker_confidence_msg;
288  tracker_confidence_msg.key = TRACKER_CONFIDENCE_MD_STR;
289  tracker_confidence_msg.value = std::to_string(pose->get_tracker_confidence());
290  write_message(md_topic, timestamp, tracker_confidence_msg);
291 
292  diagnostic_msgs::KeyValue mapper_confidence_msg;
293  mapper_confidence_msg.key = MAPPER_CONFIDENCE_MD_STR;
294  mapper_confidence_msg.value = std::to_string(pose->get_mapper_confidence());
295  write_message(md_topic, timestamp, mapper_confidence_msg);
296 
297  //Write frame's timestamp as metadata
298  diagnostic_msgs::KeyValue frame_timestamp_msg;
299  frame_timestamp_msg.key = FRAME_TIMESTAMP_MD_STR;
300  frame_timestamp_msg.value = to_string() << std::hexfloat << pose->get_frame_timestamp();
301  write_message(md_topic, timestamp, frame_timestamp_msg);
302 
303  // Write the rest of the frame metadata and stream extrinsics
304  write_additional_frame_messages(stream_id, timestamp, frame);
305  }
306 
307  void write_stream_info(nanoseconds timestamp, const sensor_identifier& sensor_id, std::shared_ptr<stream_profile_interface> profile)
308  {
309  realsense_msgs::StreamInfo stream_info_msg;
310  stream_info_msg.is_recommended = profile->is_default();
311  convert(profile->get_format(), stream_info_msg.encoding);
312  stream_info_msg.fps = profile->get_framerate();
313  write_message(ros_topic::stream_info_topic({ sensor_id.device_index, sensor_id.sensor_index, profile->get_stream_type(), static_cast<uint32_t>(profile->get_stream_index()) }), timestamp, stream_info_msg);
314  }
315 
316  void write_streaming_info(nanoseconds timestamp, const sensor_identifier& sensor_id, std::shared_ptr<video_stream_profile_interface> profile)
317  {
318  write_stream_info(timestamp, sensor_id, profile);
319 
320  sensor_msgs::CameraInfo camera_info_msg;
321  camera_info_msg.width = profile->get_width();
322  camera_info_msg.height = profile->get_height();
323  rs2_intrinsics intrinsics{};
324  try {
325  intrinsics = profile->get_intrinsics();
326  }
327  catch (...)
328  {
329  LOG_ERROR("Error trying to get intrinsc data for stream " << profile->get_stream_type() << ", " << profile->get_stream_index());
330  }
331  camera_info_msg.K[0] = intrinsics.fx;
332  camera_info_msg.K[2] = intrinsics.ppx;
333  camera_info_msg.K[4] = intrinsics.fy;
334  camera_info_msg.K[5] = intrinsics.ppy;
335  camera_info_msg.K[8] = 1;
336  camera_info_msg.D.assign(std::begin(intrinsics.coeffs), std::end(intrinsics.coeffs));
337  camera_info_msg.distortion_model = rs2_distortion_to_string(intrinsics.model);
338  write_message(ros_topic::video_stream_info_topic({ sensor_id.device_index, sensor_id.sensor_index, profile->get_stream_type(), static_cast<uint32_t>(profile->get_stream_index()) }), timestamp, camera_info_msg);
339  }
340 
341  void write_streaming_info(nanoseconds timestamp, const sensor_identifier& sensor_id, std::shared_ptr<motion_stream_profile_interface> profile)
342  {
343  write_stream_info(timestamp, sensor_id, profile);
344 
345  realsense_msgs::ImuIntrinsic motion_info_msg;
346 
347  rs2_motion_device_intrinsic intrinsics{};
348  try {
349  intrinsics = profile->get_intrinsics();
350  }
351  catch (...)
352  {
353  LOG_ERROR("Error trying to get intrinsc data for stream " << profile->get_stream_type() << ", " << profile->get_stream_index());
354  }
355  //Writing empty in case of failure
356  std::copy(&intrinsics.data[0][0], &intrinsics.data[0][0] + motion_info_msg.data.size(), std::begin(motion_info_msg.data));
357  std::copy(std::begin(intrinsics.bias_variances) , std::end(intrinsics.bias_variances), std::begin(motion_info_msg.bias_variances));
358  std::copy(std::begin(intrinsics.noise_variances), std::end(intrinsics.noise_variances), std::begin(motion_info_msg.noise_variances));
359 
360  std::string topic = ros_topic::imu_intrinsic_topic({ sensor_id.device_index, sensor_id.sensor_index, profile->get_stream_type(), static_cast<uint32_t>(profile->get_stream_index()) });
361  write_message(topic, timestamp, motion_info_msg);
362  }
363  void write_streaming_info(nanoseconds timestamp, const sensor_identifier& sensor_id, std::shared_ptr<pose_stream_profile_interface> profile)
364  {
365  write_stream_info(timestamp, sensor_id, profile);
366  }
367  void write_extension_snapshot(uint32_t device_id, const nanoseconds& timestamp, rs2_extension type, std::shared_ptr<librealsense::extension_snapshot> snapshot)
368  {
369  const auto ignored = 0u;
370  write_extension_snapshot(device_id, ignored, timestamp, type, snapshot, true);
371  }
372 
373  void write_extension_snapshot(uint32_t device_id, uint32_t sensor_id, const nanoseconds& timestamp, rs2_extension type, std::shared_ptr<librealsense::extension_snapshot> snapshot)
374  {
375  write_extension_snapshot(device_id, sensor_id, timestamp, type, snapshot, false);
376  }
377 
378  template <rs2_extension E>
379  std::shared_ptr<typename ExtensionToType<E>::type> SnapshotAs(std::shared_ptr<librealsense::extension_snapshot> snapshot)
380  {
381  auto as_type = As<typename ExtensionToType<E>::type>(snapshot);
382  if (as_type == nullptr)
383  {
384  throw invalid_value_exception(to_string() << "Failed to cast snapshot to \"" << E << "\" (as \"" << ExtensionToType<E>::to_string() << "\")");
385  }
386  return as_type;
387  }
388  void write_extension_snapshot(uint32_t device_id, uint32_t sensor_id, const nanoseconds& timestamp, rs2_extension type, std::shared_ptr<librealsense::extension_snapshot> snapshot, bool is_device)
389  {
390  switch (type)
391  {
392  case RS2_EXTENSION_INFO:
393  {
394  auto info = SnapshotAs<RS2_EXTENSION_INFO>(snapshot);
395  if (is_device)
396  {
397  write_vendor_info(ros_topic::device_info_topic(device_id), timestamp, info);
398  }
399  else
400  {
401  write_vendor_info(ros_topic::sensor_info_topic({ device_id, sensor_id }), timestamp, info);
402  }
403  break;
404  }
405  case RS2_EXTENSION_DEBUG:
406  break;
407  //case RS2_EXTENSION_OPTION:
408  //{
409  // auto option = SnapshotAs<RS2_EXTENSION_OPTION>(snapshot);
410  // write_sensor_option({ device_id, sensor_id }, timestamp, *option);
411  // break;
412  //}
414  {
415  auto options = SnapshotAs<RS2_EXTENSION_OPTIONS>(snapshot);
416  write_sensor_options({ device_id, sensor_id }, timestamp, options);
417  break;
418  }
419  case RS2_EXTENSION_VIDEO:
420  case RS2_EXTENSION_ROI:
423  break;
425  {
426  auto profile = SnapshotAs<RS2_EXTENSION_VIDEO_PROFILE>(snapshot);
427  write_streaming_info(timestamp, { device_id, sensor_id }, profile);
428  break;
429  }
431  {
432  auto profile = SnapshotAs<RS2_EXTENSION_MOTION_PROFILE>(snapshot);
433  write_streaming_info(timestamp, { device_id, sensor_id }, profile);
434  break;
435  }
437  {
438  auto profile = SnapshotAs<RS2_EXTENSION_POSE_PROFILE>(snapshot);
439  write_streaming_info(timestamp, { device_id, sensor_id }, profile);
440  break;
441  }
442  default:
443  throw invalid_value_exception(to_string() << "Failed to Write Extension Snapshot: Unsupported extension \"" << librealsense::get_string(type) << "\"");
444  }
445  }
446 
447  void write_vendor_info(const std::string& topic, nanoseconds timestamp, std::shared_ptr<info_interface> info_snapshot)
448  {
449  for (uint32_t i = 0; i < static_cast<uint32_t>(RS2_CAMERA_INFO_COUNT); i++)
450  {
451  auto camera_info = static_cast<rs2_camera_info>(i);
452  if (info_snapshot->supports_info(camera_info))
453  {
454  diagnostic_msgs::KeyValue msg;
455  msg.key = rs2_camera_info_to_string(camera_info);
456  msg.value = info_snapshot->get_info(camera_info);
457  write_message(topic, timestamp, msg);
458  }
459  }
460  }
461 
462  void write_sensor_option(device_serializer::sensor_identifier sensor_id, const nanoseconds& timestamp, rs2_option type, const librealsense::option& option)
463  {
464  float value = option.query();
465  const char* str = option.get_description();
466  std::string description = str ? std::string(str) : (to_string() << "Read only option of " << librealsense::get_string(type));
467 
468  //One message for value
469  std_msgs::Float32 option_msg;
470  option_msg.data = value;
471  write_message(ros_topic::option_value_topic(sensor_id, type), timestamp, option_msg);
472 
473  //Another message for description, should be written once per topic
474 
475  if (m_written_options_descriptions[sensor_id.sensor_index].find(type) == m_written_options_descriptions[sensor_id.sensor_index].end())
476  {
477  std_msgs::String option_msg_desc;
478  option_msg_desc.data = description;
479  write_message(ros_topic::option_description_topic(sensor_id, type), get_static_file_info_timestamp(), option_msg_desc);
480  m_written_options_descriptions[sensor_id.sensor_index].insert(type);
481  }
482  }
483 
484  void write_sensor_options(device_serializer::sensor_identifier sensor_id, const nanoseconds& timestamp, std::shared_ptr<options_interface> options)
485  {
486  for (int i = 0; i < static_cast<int>(RS2_OPTION_COUNT); i++)
487  {
488  auto option_id = static_cast<rs2_option>(i);
489  try
490  {
491  if (options->supports_option(option_id))
492  {
493  write_sensor_option(sensor_id, timestamp, option_id, options->get_option(option_id));
494  }
495  }
496  catch (std::exception& e)
497  {
498  LOG_WARNING("Failed to get or write option " << option_id << " for sensor " << sensor_id.sensor_index << ". Exception: " << e.what());
499  }
500  }
501  }
502  template <typename T>
503  void write_message(std::string const& topic, nanoseconds const& time, T const& msg)
504  {
505  try
506  {
507  m_bag.write(topic, to_rostime(time), msg);
508  LOG_DEBUG("Recorded: \"" << topic << "\" . TS: " << time.count());
509  }
510  catch (rosbag::BagIOException& e)
511  {
512  throw io_exception(to_string() << "Ros Writer failed to write topic: \"" << topic << "\" to file. (Exception message: " << e.what() << ")");
513  }
514  }
515 
516  static uint8_t is_big_endian()
517  {
518  int num = 1;
519  return (*reinterpret_cast<char*>(&num) == 1) ? 0 : 1; //Little Endian: (char)0x0001 => 0x01, Big Endian: (char)0x0001 => 0x00,
520  }
521 
522  std::map<stream_identifier, geometry_msgs::Transform> m_extrinsics_msgs;
523  std::string m_file_path;
524  rosbag::Bag m_bag;
525  std::map<uint32_t, std::set<rs2_option>> m_written_options_descriptions;
526  };
527 }
std::shared_ptr< stream_profile_interface > get_stream() const override
Definition: archive.h:107
Definition: rs_option.h:65
bool supports_frame_metadata(const rs2_frame_metadata_value &frame_metadata) const override
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
virtual const char * get_description() const =0
static std::string sensor_info_topic(const device_serializer::sensor_identifier &sensor_id)
Definition: ros_file_format.h:294
float x
Definition: types.h:414
Definition: backend.h:351
Definition: types.h:417
rs2_option
Defines general configuration controls. These can generally be mapped to camera UVC controls...
Definition: rs_option.h:22
Definition: streaming.h:63
rs2_notification_category category
Definition: types.h:855
Definition: serialization.h:324
float w
Definition: types.h:415
Definition: options.h:20
#define LOG_WARNING(...)
Definition: types.h:109
Definition: extension.h:111
constexpr const char * FRAME_TIMESTAMP_MD_STR
Definition: ros_file_format.h:192
uint32_t sensor_index
Definition: serialization.h:20
float y
Definition: types.h:414
static std::string stream_extrinsic_topic(const device_serializer::stream_identifier &stream_id, uint32_t ref_id)
Definition: ros_file_format.h:356
const char * rs2_distortion_to_string(rs2_distortion distortion)
frame()
Definition: archive.h:69
sql::statement::iterator end(sql::statement &stmt)
static std::string video_stream_info_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:302
void convert(rs2_format source, std::string &target)
Definition: ros_file_format.h:33
static std::string device_info_topic(uint32_t device_id)
Definition: ros_file_format.h:290
Definition: rs_types.h:102
constexpr const char * TRACKER_CONFIDENCE_MD_STR
Definition: ros_file_format.h:193
Definition: rs_sensor.h:43
virtual float query() const =0
std::map< rs2_extension, std::shared_ptr< extension_snapshot > > get_snapshots() const
Definition: serialization.h:208
Definition: types.h:181
Definition: rs_types.h:114
std::shared_ptr< sensor_interface > get_sensor() const override
Definition: archive.h:63
Definition: algo.h:16
ros_writer(const std::string &file)
Definition: ros_writer.h:25
snapshot_collection get_device_extensions_snapshots() const
Definition: serialization.h:285
uint32_t device_index
Definition: serialization.h:19
#define LOG_DEBUG(...)
Definition: types.h:107
Definition: rs_types.h:101
float z
Definition: types.h:414
Definition: rs_types.h:112
Definition: types.h:414
Definition: rs_frame.h:42
sql::statement::iterator begin(sql::statement &stmt)
static std::string pose_accel_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:335
Definition: rs_types.h:99
rs2_timestamp_domain get_frame_timestamp_domain() const override
static std::string notification_topic(const device_serializer::sensor_identifier &sensor_id, rs2_notification_category nc)
Definition: ros_file_format.h:371
Definition: ros_writer.h:22
void write_snapshot(const sensor_identifier &sensor_id, const nanoseconds &timestamp, rs2_extension type, const std::shared_ptr< extension_snapshot > &snapshot) override
Definition: ros_writer.h:74
static std::string file_version_topic()
Definition: ros_file_format.h:286
const std::string & get_file_name() const override
Definition: ros_writer.h:79
constexpr const char * SYSTEM_TIME_MD_STR
Definition: ros_file_format.h:190
Definition: rs_sensor.h:31
static std::string imu_intrinsic_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:306
constexpr const char * MAPPER_CONFIDENCE_MD_STR
Definition: ros_file_format.h:191
Definition: rs_types.h:100
constexpr const char * TIMESTAMP_DOMAIN_MD_STR
Definition: ros_file_format.h:189
Definition: archive.h:227
std::chrono::duration< uint64_t, std::nano > nanoseconds
Definition: serialization.h:46
static std::string pose_twist_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:340
static std::string frame_metadata_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:351
Definition: rs_sensor.h:44
Definition: types.h:54
float x
Definition: types.h:415
Cross-stream extrinsics: encode the topology describing how the different devices are connected...
Definition: rs_sensor.h:79
Definition: types.h:415
void write_snapshot(uint32_t device_index, const nanoseconds &timestamp, rs2_extension type, const std::shared_ptr< extension_snapshot > &snapshot) override
Definition: ros_writer.h:69
std::vector< sensor_snapshot > get_sensors_snapshots() const
Definition: serialization.h:277
void write_device_description(const librealsense::device_snapshot &device_description) override
Definition: ros_writer.h:32
const char * rs2_camera_info_to_string(rs2_camera_info info)
std::string get_string(perc::Status value)
Definition: controller_event_serializer.h:26
float y
Definition: types.h:415
constexpr device_serializer::nanoseconds get_static_file_info_timestamp()
Definition: ros_file_format.h:570
static std::string option_value_topic(const device_serializer::sensor_identifier &sensor_id, rs2_option option_type)
Definition: ros_file_format.h:318
Definition: rs_types.h:96
rs2_extension
Specifies advanced interfaces (capabilities) objects may implement.
Definition: rs_types.h:93
int stream_id
Definition: sync.h:17
double timestamp
Definition: types.h:859
Definition: backend.h:348
Definition: types.h:587
Definition: rs_types.h:110
Definition: rs_types.h:97
Video stream intrinsics.
Definition: rs_types.h:55
static std::string option_description_topic(const device_serializer::sensor_identifier &sensor_id, rs2_option option_type)
Definition: ros_file_format.h:324
float z
Definition: types.h:415
Definition: types.h:846
std::string description
Definition: types.h:858
Motion device intrinsics: scale, bias, and variances.
Definition: rs_types.h:68
void write_frame(const stream_identifier &stream_id, const nanoseconds &timestamp, frame_holder &&frame)
Definition: ros_writer.h:48
static std::string pose_transform_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:329
Definition: rs_types.h:116
rs2_time_t get_frame_system_time() const override
ros::Time to_rostime(const device_serializer::nanoseconds &t)
Definition: ros_file_format.h:583
rs2_log_severity severity
Definition: types.h:857
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_metadata_type get_frame_metadata(const rs2_frame_metadata_value &frame_metadata) const override
#define LOG_ERROR(...)
Definition: types.h:110
void copy(void *dst, void const *src, size_t size)
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