Intel® RealSense™ Cross Platform API
Intel Realsense Cross-platform API
ros_reader.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 <chrono>
6 #include <mutex>
7 #include <regex>
8 #include <ios> //For std::hexfloat
9 #include <core/serialization.h>
10 #include "rosbag/view.h"
11 #include "ros_file_format.h"
12 
13 namespace librealsense
14 {
15  using namespace device_serializer;
16 
18  {
19  public:
20  ros_reader(const std::string& file, const std::shared_ptr<context>& ctx) :
21  m_total_duration(0),
22  m_file_path(file),
23  m_context(ctx),
24  m_version(0),
25  m_metadata_parser_map(create_metadata_parser_map())
26  {
27  try
28  {
29  reset(); //Note: calling a virtual function inside c'tor, safe while base function is pure virtual
30  m_total_duration = get_file_duration(m_file, m_version);
31  }
32  catch (const std::exception& e)
33  {
34  //Rethrowing with better clearer message
35  throw io_exception(to_string() << "Failed to create ros reader: " << e.what());
36  }
37  }
38 
40  {
41  return read_device_description(time);
42  }
43 
44  std::shared_ptr<serialized_data> read_next_data() override
45  {
46  if (m_samples_view == nullptr || m_samples_itrator == m_samples_view->end())
47  {
48  LOG_DEBUG("End of file reached");
49  return std::make_shared<serialized_end_of_file>();
50  }
51 
52  rosbag::MessageInstance next_msg = *m_samples_itrator;
53  ++m_samples_itrator;
54 
55  if (next_msg.isType<sensor_msgs::Image>()
56  || next_msg.isType<sensor_msgs::Imu>()
57  || next_msg.isType<realsense_legacy_msgs::pose>()
58  || next_msg.isType<geometry_msgs::Transform>())
59  {
60  LOG_DEBUG("Next message is a frame");
61  return create_frame(next_msg);
62  }
63 
64  if (m_version >= 3)
65  {
66  if (next_msg.isType<std_msgs::Float32>())
67  {
68  LOG_DEBUG("Next message is an option");
69  auto timestamp = to_nanoseconds(next_msg.getTime());
70  auto sensor_id = ros_topic::get_sensor_identifier(next_msg.getTopic());
71  auto option = create_option(m_file, next_msg);
72  return std::make_shared<serialized_option>(timestamp, sensor_id, option.first, option.second);
73  }
74 
75  if (next_msg.isType<realsense_msgs::Notification>())
76  {
77  LOG_DEBUG("Next message is a notification");
78  auto timestamp = to_nanoseconds(next_msg.getTime());
79  auto sensor_id = ros_topic::get_sensor_identifier(next_msg.getTopic());
80  auto notification = create_notification(m_file, next_msg);
81  return std::make_shared<serialized_notification>(timestamp, sensor_id, notification);
82  }
83  }
84 
85  std::string err_msg = to_string() << "Unknown message type: " << next_msg.getDataType() << "(Topic: " << next_msg.getTopic() << ")";
86  LOG_ERROR(err_msg);
87  throw invalid_value_exception(err_msg);
88  }
89 
90  void seek_to_time(const nanoseconds& seek_time) override
91  {
92  if (seek_time > m_total_duration)
93  {
94  throw invalid_value_exception(to_string() << "Requested time is out of playback length. (Requested = " << seek_time.count() << ", Duration = " << m_total_duration.count() << ")");
95  }
96  auto seek_time_as_secs = std::chrono::duration_cast<std::chrono::duration<double>>(seek_time);
97  auto seek_time_as_rostime = ros::Time(seek_time_as_secs.count());
98 
99  m_samples_view.reset(new rosbag::View(m_file, FalseQuery()));
100 
101  //Using cached topics here and not querying them (before reseting) since a previous call to seek
102  // could have changed the view and some streams that should be streaming were dropped.
103  //E.g: Recording Depth+Color, stopping Depth, starting IR, stopping IR and Color. Play IR+Depth: will play only depth, then only IR, then we seek to a point only IR was streaming, and then to 0.
104  for (auto topic : m_enabled_streams_topics)
105  {
106  m_samples_view->addQuery(m_file, rosbag::TopicQuery(topic), seek_time_as_rostime);
107  }
108  m_samples_itrator = m_samples_view->begin();
109  }
110 
111  std::vector<std::shared_ptr<serialized_data>> fetch_last_frames(const nanoseconds& seek_time) override
112  {
113  std::vector<std::shared_ptr<serialized_data>> result;
114  rosbag::View view(m_file, FalseQuery());
115  auto as_rostime = to_rostime(seek_time);
116  auto start_time = to_rostime(get_static_file_info_timestamp());
117 
118  for (auto topic : m_enabled_streams_topics)
119  {
120  view.addQuery(m_file, rosbag::TopicQuery(topic), start_time, as_rostime);
121  }
122  std::map<device_serializer::stream_identifier, ros::Time> last_frames;
123  for (auto&& m : view)
124  {
125  if (m.isType<sensor_msgs::Image>() || m.isType<sensor_msgs::Imu>())
126  {
127  auto id = ros_topic::get_stream_identifier(m.getTopic());
128  last_frames[id] = m.getTime();
129  }
130  }
131  for (auto&& kvp : last_frames)
132  {
133  auto topic = ros_topic::frame_data_topic(kvp.first);
134  rosbag::View view(m_file, rosbag::TopicQuery(topic), kvp.second, kvp.second);
135  auto msg = view.begin();
136  auto new_frame = create_frame(*msg);
137  result.push_back(new_frame);
138  }
139  return result;
140  }
141  nanoseconds query_duration() const override
142  {
143  return m_total_duration;
144  }
145 
146  void reset() override
147  {
148  m_file.close();
149  m_file.open(m_file_path, rosbag::BagMode::Read);
150  m_version = read_file_version(m_file);
151  m_samples_view = nullptr;
152  m_frame_source = std::make_shared<frame_source>(m_version == 1 ? 128 : 32);
153  m_frame_source->init(m_metadata_parser_map);
154  m_initial_device_description = read_device_description(get_static_file_info_timestamp(), true);
155  }
156 
157  virtual void enable_stream(const std::vector<device_serializer::stream_identifier>& stream_ids) override
158  {
159  ros::Time start_time = ros::TIME_MIN + ros::Duration{ 0, 1 }; //first non 0 timestamp and afterward
160  if (m_samples_view == nullptr) //Starting to stream
161  {
162  m_samples_view = std::unique_ptr<rosbag::View>(new rosbag::View(m_file, FalseQuery()));
163  m_samples_view->addQuery(m_file, OptionsQuery(), start_time);
164  m_samples_view->addQuery(m_file, NotificationsQuery(), start_time);
165  m_samples_itrator = m_samples_view->begin();
166  }
167  else //Already streaming
168  {
169  if (m_samples_itrator != m_samples_view->end())
170  {
171  rosbag::MessageInstance sample_msg = *m_samples_itrator;
172  start_time = sample_msg.getTime();
173  }
174  }
175  auto currently_streaming = get_topics(m_samples_view);
176  //empty the view
177  m_samples_view = std::unique_ptr<rosbag::View>(new rosbag::View(m_file, FalseQuery()));
178 
179  for (auto&& stream_id : stream_ids)
180  {
181  //add new stream to view
182  if (m_version == legacy_file_format::file_version())
183  {
184  m_samples_view->addQuery(m_file, legacy_file_format::StreamQuery(stream_id), start_time);
185  }
186  else
187  {
188  m_samples_view->addQuery(m_file, StreamQuery(stream_id), start_time);
189  }
190  }
191 
192  //add already existing streams
193  for (auto topic : currently_streaming)
194  {
195  m_samples_view->addQuery(m_file, rosbag::TopicQuery(topic), start_time);
196  }
197  m_samples_itrator = m_samples_view->begin();
198  m_enabled_streams_topics = get_topics(m_samples_view);
199  }
200 
201  virtual void disable_stream(const std::vector<device_serializer::stream_identifier>& stream_ids) override
202  {
203  if (m_samples_view == nullptr)
204  {
205  return;
206  }
207  ros::Time curr_time;
208  if (m_samples_itrator == m_samples_view->end())
209  {
210  curr_time = m_samples_view->getEndTime();
211  }
212  else
213  {
214  rosbag::MessageInstance sample_msg = *m_samples_itrator;
215  curr_time = sample_msg.getTime();
216  }
217  auto currently_streaming = get_topics(m_samples_view);
218  m_samples_view = std::unique_ptr<rosbag::View>(new rosbag::View(m_file, FalseQuery()));
219  for (auto topic : currently_streaming)
220  {
221  //Find if this topic is one of the streams that should be disabled
222  auto it = std::find_if(stream_ids.begin(), stream_ids.end(), [&topic](const device_serializer::stream_identifier& s) {
223  //return topic.starts_with(s);
224  return topic.find(ros_topic::stream_full_prefix(s)) != std::string::npos;
225  });
226  bool should_topic_remain = (it == stream_ids.end());
227  if (should_topic_remain)
228  {
229  m_samples_view->addQuery(m_file, rosbag::TopicQuery(topic), curr_time);
230  }
231  }
232  m_samples_itrator = m_samples_view->begin();
233  m_enabled_streams_topics = get_topics(m_samples_view);
234  }
235 
236  const std::string& get_file_name() const override
237  {
238  return m_file_path;
239  }
240 
241  private:
242 
243  template <typename ROS_TYPE>
244  static typename ROS_TYPE::ConstPtr instantiate_msg(const rosbag::MessageInstance& msg)
245  {
246  typename ROS_TYPE::ConstPtr msg_instnance_ptr = msg.instantiate<ROS_TYPE>();
247  if (msg_instnance_ptr == nullptr)
248  {
249  throw io_exception(to_string()
250  << "Invalid file format, expected "
252  << " message but got: " << msg.getDataType()
253  << "(Topic: " << msg.getTopic() << ")");
254  }
255  return msg_instnance_ptr;
256  }
257 
258  std::shared_ptr<serialized_frame> create_frame(const rosbag::MessageInstance& msg)
259  {
260  auto next_msg_topic = msg.getTopic();
261  auto next_msg_time = msg.getTime();
262 
263  nanoseconds timestamp = to_nanoseconds(next_msg_time);
265  if (m_version == legacy_file_format::file_version())
266  {
267  stream_id = legacy_file_format::get_stream_identifier(next_msg_topic);
268  }
269  else
270  {
271  stream_id = ros_topic::get_stream_identifier(next_msg_topic);
272  }
273  frame_holder frame{ nullptr };
274  if (msg.isType<sensor_msgs::Image>())
275  {
276  frame = create_image_from_message(msg);
277  }
278  else if (msg.isType<sensor_msgs::Imu>())
279  {
280  frame = create_motion_sample(msg);
281  }
282  else if (msg.isType<realsense_legacy_msgs::pose>() || msg.isType<geometry_msgs::Transform>())
283  {
284  frame = create_pose_sample(msg);
285  }
286  else
287  {
288  std::string err_msg = to_string() << "Unknown frame type: " << msg.getDataType() << "(Topic: " << next_msg_topic << ")";
289  LOG_ERROR(err_msg);
290  throw invalid_value_exception(err_msg);
291  }
292 
293  if (frame.frame == nullptr)
294  {
295  return std::make_shared<serialized_invalid_frame>(timestamp, stream_id);
296  }
297  return std::make_shared<serialized_frame>(timestamp, stream_id, std::move(frame));
298  }
299 
300  static std::shared_ptr<metadata_parser_map> create_metadata_parser_map()
301  {
302  auto md_parser_map = std::make_shared<metadata_parser_map>();
303  for (int i = 0; i < static_cast<int>(rs2_frame_metadata_value::RS2_FRAME_METADATA_COUNT); ++i)
304  {
305  auto frame_md_type = static_cast<rs2_frame_metadata_value>(i);
306  md_parser_map->insert(std::make_pair(frame_md_type, std::make_shared<md_constant_parser>(frame_md_type)));
307  }
308  return md_parser_map;
309  }
310 
311  static nanoseconds get_file_duration(const rosbag::Bag& file, uint32_t version)
312  {
313  std::function<bool(rosbag::ConnectionInfo const* info)> query;
314  if (version == legacy_file_format::file_version())
316  else
317  query = FrameQuery();
318  rosbag::View all_frames_view(file, query);
319  auto streaming_duration = all_frames_view.getEndTime() - all_frames_view.getBeginTime();
320  return nanoseconds(streaming_duration.toNSec());
321  }
322 
323  static void get_legacy_frame_metadata(const rosbag::Bag& bag,
325  const rosbag::MessageInstance &msg,
326  frame_additional_data& additional_data)
327  {
328  uint32_t total_md_size = 0;
329  rosbag::View frame_metadata_view(bag, legacy_file_format::FrameInfoExt(stream_id), msg.getTime(), msg.getTime());
330  assert(frame_metadata_view.size() <= 1);
331  for (auto message_instance : frame_metadata_view)
332  {
333  auto info = instantiate_msg<realsense_legacy_msgs::frame_info>(message_instance);
334  for (auto&& fmd : info->frame_metadata)
335  {
336  if (fmd.type == legacy_file_format::SYSTEM_TIMESTAMP)
337  {
338  additional_data.system_time = *reinterpret_cast<const int64_t*>(fmd.data.data());
339  }
340  else
341  {
343  if (!legacy_file_format::convert_metadata_type(fmd.type, type))
344  {
345  continue;
346  }
347  rs2_metadata_type value = *reinterpret_cast<const rs2_metadata_type*>(fmd.data.data());
348  auto size_of_enum = sizeof(rs2_frame_metadata_value);
349  auto size_of_data = sizeof(rs2_metadata_type);
350  if (total_md_size + size_of_enum + size_of_data > 255)
351  {
352  continue; //stop adding metadata to frame
353  }
354  memcpy(additional_data.metadata_blob.data() + total_md_size, &type, size_of_enum);
355  total_md_size += static_cast<uint32_t>(size_of_enum);
356  memcpy(additional_data.metadata_blob.data() + total_md_size, &value, size_of_data);
357  total_md_size += static_cast<uint32_t>(size_of_data);
358  }
359  }
360 
361  try
362  {
363  additional_data.timestamp_domain = legacy_file_format::convert(info->time_stamp_domain);
364  }
365  catch (const std::exception& e)
366  {
367  LOG_WARNING("Failed to get timestamp_domain. Error: " << e.what());
368  }
369  }
370  }
371 
372  template <typename T>
373  static bool safe_convert(const std::string& key, T& val)
374  {
375  try
376  {
377  convert(key, val);
378  }
379  catch (const std::exception& e)
380  {
381  LOG_ERROR(e.what());
382  return false;
383  }
384  return true;
385  }
386 
387  static std::map<std::string, std::string> get_frame_metadata(const rosbag::Bag& bag,
388  const std::string& topic,
389  const device_serializer::stream_identifier& stream_id,
390  const rosbag::MessageInstance &msg,
391  frame_additional_data& additional_data)
392  {
393  uint32_t total_md_size = 0;
394  std::map<std::string, std::string> remaining;
395  rosbag::View frame_metadata_view(bag, rosbag::TopicQuery(topic), msg.getTime(), msg.getTime());
396 
397  for (auto message_instance : frame_metadata_view)
398  {
399  auto key_val_msg = instantiate_msg<diagnostic_msgs::KeyValue>(message_instance);
400  if (key_val_msg->key == TIMESTAMP_DOMAIN_MD_STR)
401  {
402  if (!safe_convert(key_val_msg->value, additional_data.timestamp_domain))
403  {
404  remaining[key_val_msg->key] = key_val_msg->value;
405  }
406  }
407  else if (key_val_msg->key == SYSTEM_TIME_MD_STR)
408  {
409  if (!safe_convert(key_val_msg->value, additional_data.system_time))
410  {
411  remaining[key_val_msg->key] = key_val_msg->value;
412  }
413  }
414  else
415  {
417  if (!safe_convert(key_val_msg->key, type))
418  {
419  remaining[key_val_msg->key] = key_val_msg->value;
420  continue;
421  }
423  if (!safe_convert(key_val_msg->value, md))
424  {
425  remaining[key_val_msg->key] = key_val_msg->value;
426  continue;
427  }
428  auto size_of_enum = sizeof(rs2_frame_metadata_value);
429  auto size_of_data = sizeof(rs2_metadata_type);
430  if (total_md_size + size_of_enum + size_of_data > 255)
431  {
432  continue; //stop adding metadata to frame
433  }
434  memcpy(additional_data.metadata_blob.data() + total_md_size, &type, size_of_enum);
435  total_md_size += static_cast<uint32_t>(size_of_enum);
436  memcpy(additional_data.metadata_blob.data() + total_md_size, &md, size_of_data);
437  total_md_size += static_cast<uint32_t>(size_of_data);
438  }
439  }
440  additional_data.metadata_size = total_md_size;
441  return remaining;
442  }
443 
444  frame_holder create_image_from_message(const rosbag::MessageInstance &image_data) const
445  {
446  LOG_DEBUG("Trying to create an image frame from message");
447  auto msg = instantiate_msg<sensor_msgs::Image>(image_data);
448  frame_additional_data additional_data{};
449  std::chrono::duration<double, std::milli> timestamp_ms(std::chrono::duration<double>(msg->header.stamp.toSec()));
450  additional_data.timestamp = timestamp_ms.count();
451  additional_data.frame_number = msg->header.seq;
452  additional_data.fisheye_ae_mode = false;
453 
455  if (m_version == legacy_file_format::file_version())
456  {
457  //Version 1 legacy
458  stream_id = legacy_file_format::get_stream_identifier(image_data.getTopic());
459  get_legacy_frame_metadata(m_file, stream_id, image_data, additional_data);
460  }
461  else
462  {
463  //Version 2 and above
464  stream_id = ros_topic::get_stream_identifier(image_data.getTopic());
465  auto info_topic = ros_topic::frame_metadata_topic(stream_id);
466  get_frame_metadata(m_file, info_topic, stream_id, image_data, additional_data);
467  }
468 
470  msg->data.size(), additional_data, true);
471  if (frame == nullptr)
472  {
473  LOG_WARNING("Failed to allocate new frame");
474  return nullptr;
475  }
477  video_frame->assign(msg->width, msg->height, msg->step, msg->step / msg->width * 8);
478  rs2_format stream_format;
479  convert(msg->encoding, stream_format);
480  //attaching a temp stream to the frame. Playback sensor should assign the real stream
481  frame->set_stream(std::make_shared<video_stream_profile>(platform::stream_profile{}));
482  frame->get_stream()->set_format(stream_format);
483  frame->get_stream()->set_stream_index(stream_id.stream_index);
484  frame->get_stream()->set_stream_type(stream_id.stream_type);
485  video_frame->data = std::move(msg->data);
486  librealsense::frame_holder fh{ video_frame };
487  LOG_DEBUG("Created image frame: " << stream_id << " " << video_frame->get_width() << "x" << video_frame->get_height() << " " << stream_format);
488 
489  return std::move(fh);
490  }
491 
492  frame_holder create_motion_sample(const rosbag::MessageInstance &motion_data) const
493  {
494  LOG_DEBUG("Trying to create a motion frame from message");
495 
496  auto msg = instantiate_msg<sensor_msgs::Imu>(motion_data);
497 
498  frame_additional_data additional_data{};
499  std::chrono::duration<double, std::milli> timestamp_ms(std::chrono::duration<double>(msg->header.stamp.toSec()));
500  additional_data.timestamp = timestamp_ms.count();
501  additional_data.frame_number = msg->header.seq;
502  additional_data.fisheye_ae_mode = false; //TODO: where should this come from?
503 
505  if (m_version == legacy_file_format::file_version())
506  {
507  //Version 1 legacy
508  stream_id = legacy_file_format::get_stream_identifier(motion_data.getTopic());
509  get_legacy_frame_metadata(m_file, stream_id, motion_data, additional_data);
510  }
511  else
512  {
513  //Version 2 and above
514  stream_id = ros_topic::get_stream_identifier(motion_data.getTopic());
515  auto info_topic = ros_topic::frame_metadata_topic(stream_id);
516  get_frame_metadata(m_file, info_topic, stream_id, motion_data, additional_data);
517  }
518 
519  frame_interface* frame = m_frame_source->alloc_frame(RS2_EXTENSION_MOTION_FRAME, 3 * sizeof(float), additional_data, true);
520  if (frame == nullptr)
521  {
522  LOG_WARNING("Failed to allocate new frame");
523  return nullptr;
524  }
526  //attaching a temp stream to the frame. Playback sensor should assign the real stream
527  frame->set_stream(std::make_shared<motion_stream_profile>(platform::stream_profile{}));
528  frame->get_stream()->set_format(RS2_FORMAT_MOTION_XYZ32F);
529  frame->get_stream()->set_stream_index(stream_id.stream_index);
530  frame->get_stream()->set_stream_type(stream_id.stream_type);
531  if (stream_id.stream_type == RS2_STREAM_ACCEL)
532  {
533  auto data = reinterpret_cast<float*>(motion_frame->data.data());
534  data[0] = static_cast<float>(msg->linear_acceleration.x);
535  data[1] = static_cast<float>(msg->linear_acceleration.y);
536  data[2] = static_cast<float>(msg->linear_acceleration.z);
537  }
538  else if (stream_id.stream_type == RS2_STREAM_GYRO)
539  {
540  auto data = reinterpret_cast<float*>(motion_frame->data.data());
541  data[0] = static_cast<float>(msg->angular_velocity.x);
542  data[1] = static_cast<float>(msg->angular_velocity.y);
543  data[2] = static_cast<float>(msg->angular_velocity.z);
544  }
545  else
546  {
547  throw io_exception(to_string() << "Unsupported stream type " << stream_id.stream_type);
548  }
549  librealsense::frame_holder fh{ motion_frame };
550  LOG_DEBUG("Created motion frame: " << stream_id);
551 
552  return std::move(fh);
553  }
554 
555  static inline float3 to_float3(const geometry_msgs::Vector3& v)
556  {
557  float3 f{};
558  f.x = v.x;
559  f.y = v.y;
560  f.z = v.z;
561  return f;
562  }
563 
564  static inline float4 to_float4(const geometry_msgs::Quaternion& q)
565  {
566  float4 f{};
567  f.x = q.x;
568  f.y = q.y;
569  f.z = q.z;
570  f.w = q.w;
571  return f;
572  }
573 
574  frame_holder create_pose_sample(const rosbag::MessageInstance &msg) const
575  {
576  LOG_DEBUG("Trying to create a pose frame from message");
577 
579  std::chrono::duration<double, std::milli> timestamp_ms;
580  if (m_version == legacy_file_format::file_version())
581  {
582  auto pose_msg = instantiate_msg<realsense_legacy_msgs::pose>(msg);
583  pose.rotation = to_float4(pose_msg->rotation);
584  pose.translation = to_float3(pose_msg->translation);
585  pose.angular_acceleration = to_float3(pose_msg->angular_acceleration);
586  pose.acceleration = to_float3(pose_msg->acceleration);
587  pose.angular_velocity = to_float3(pose_msg->angular_velocity);
588  pose.velocity = to_float3(pose_msg->velocity);
589  //pose.confidence = not supported in legacy format
590  timestamp_ms= std::chrono::duration<double, std::milli>(static_cast<double>(pose_msg->timestamp));
591  }
592  else
593  {
594  assert(msg.getTopic().find("pose/transform") != std::string::npos); // Assuming that the samples iterator of the reader only reads the pose/transform topic
595  // and we will be reading the rest in here (see ../readme.md#Topics under Pose-Data for more information
596  auto transform_msg = instantiate_msg<geometry_msgs::Transform>(msg);
597 
598  auto stream_id = ros_topic::get_stream_identifier(msg.getTopic());
599  std::string accel_topic = ros_topic::pose_accel_topic(stream_id);
600  rosbag::View accel_view(m_file, rosbag::TopicQuery(accel_topic), msg.getTime(), msg.getTime());
601  assert(accel_view.size() == 1);
602  auto accel_msg = instantiate_msg<geometry_msgs::Accel>(*accel_view.begin());
603 
604  std::string twist_topic = ros_topic::pose_twist_topic(stream_id);
605  rosbag::View twist_view(m_file, rosbag::TopicQuery(twist_topic), msg.getTime(), msg.getTime());
606  assert(twist_view.size() == 1);
607  auto twist_msg = instantiate_msg<geometry_msgs::Twist>(*twist_view.begin());
608 
609  pose.rotation = to_float4(transform_msg->rotation);
610  pose.translation = to_float3(transform_msg->translation);
611  pose.angular_acceleration = to_float3(accel_msg->angular);
612  pose.acceleration = to_float3(accel_msg->linear);
613  pose.angular_velocity = to_float3(twist_msg->angular);
614  pose.velocity = to_float3(twist_msg->linear);
615  }
616  size_t frame_size = sizeof(pose);
618  frame_additional_data additional_data{};
619 
620  additional_data.frame_number = 0; //No support for frame numbers
621  additional_data.fisheye_ae_mode = false;
622 
624  if (m_version == legacy_file_format::file_version())
625  {
626  //Version 1 legacy
627  stream_id = legacy_file_format::get_stream_identifier(msg.getTopic());
628  get_legacy_frame_metadata(m_file, stream_id, msg, additional_data);
629  }
630  else
631  {
632  //Version 2 and above
633  stream_id = ros_topic::get_stream_identifier(msg.getTopic());
634  auto info_topic = ros_topic::frame_metadata_topic(stream_id);
635  auto remaining = get_frame_metadata(m_file, info_topic, stream_id, msg, additional_data);
636  for (auto&& kvp : remaining)
637  {
638  if (kvp.first == MAPPER_CONFIDENCE_MD_STR)
639  {
640  pose.mapper_confidence = std::stoul(kvp.second);
641  }
642  else if (kvp.first == FRAME_TIMESTAMP_MD_STR)
643  {
644  double ts;
645  std::istringstream iss(kvp.second);
646  iss >> std::hexfloat >> ts;
647  timestamp_ms = std::chrono::duration<double, std::milli>(ts);
648  }
649  else if (kvp.first == TRACKER_CONFIDENCE_MD_STR)
650  {
651  pose.tracker_confidence = std::stoul(kvp.second);
652  }
653  }
654  }
655 
656  additional_data.timestamp = timestamp_ms.count();
657 
658  frame_interface* new_frame = m_frame_source->alloc_frame(frame_type, frame_size, additional_data, true);
659  if (new_frame == nullptr)
660  {
661  LOG_WARNING("Failed to allocate new frame");
662  return nullptr;
663  }
665  //attaching a temp stream to the frame. Playback sensor should assign the real stream
666  new_frame->set_stream(std::make_shared<stream_profile_base>(platform::stream_profile{}));
667  new_frame->get_stream()->set_format(RS2_FORMAT_6DOF);
668  new_frame->get_stream()->set_stream_index(stream_id.stream_index);
669  new_frame->get_stream()->set_stream_type(stream_id.stream_type);
670  byte* data = pose_frame->data.data();
671  memcpy(data, &pose, frame_size);
672  frame_holder fh{ new_frame };
673  LOG_DEBUG("Created new frame " << frame_type);
674  return std::move(fh);
675  }
676 
677  static uint32_t read_file_version(const rosbag::Bag& file)
678  {
679  auto version_topic = ros_topic::file_version_topic();
680  rosbag::View view(file, rosbag::TopicQuery(version_topic));
681 
682  auto legacy_version_topic = legacy_file_format::file_version_topic();
683  rosbag::View legacy_view(file, rosbag::TopicQuery(legacy_version_topic));
684  if(legacy_view.size() == 0 && view.size() == 0)
685  {
686  throw io_exception(to_string() << "Invalid file format, file does not contain topic \"" << version_topic << "\" nor \"" << legacy_version_topic << "\"");
687  }
688  assert((view.size() + legacy_view.size()) == 1); //version message is expected to be a single one
689  if (view.size() != 0)
690  {
691  auto item = *view.begin();
692  auto msg = instantiate_msg<std_msgs::UInt32>(item);
693  if (msg->data < get_minimum_supported_file_version())
694  {
695  throw std::runtime_error(to_string() << "Unsupported file version \"" << msg->data << "\"");
696  }
697  return msg->data;
698  }
699  else if (legacy_view.size() != 0)
700  {
701  auto item = *legacy_view.begin();
702  auto msg = instantiate_msg<std_msgs::UInt32>(item);
704  {
705  throw std::runtime_error(to_string() << "Unsupported legacy file version \"" << msg->data << "\"");
706  }
707  return msg->data;
708  }
709  throw std::logic_error("Unreachable code path");
710  }
711  bool try_read_legacy_stream_extrinsic(const stream_identifier& stream_id, uint32_t& group_id, rs2_extrinsics& extrinsic) const
712  {
713  std::string topic;
714  if (stream_id.stream_type == RS2_STREAM_ACCEL || stream_id.stream_type == RS2_STREAM_GYRO)
715  {
716  topic = to_string() << "/camera/rs_motion_stream_info/" << stream_id.sensor_index;
717  }
718  else if (legacy_file_format::is_camera(stream_id.stream_type))
719  {
720  topic = to_string() << "/camera/rs_stream_info/" << stream_id.sensor_index;
721  }
722  else
723  {
724  return false;
725  }
726  rosbag::View extrinsics_view(m_file, rosbag::TopicQuery(topic));
727  if (extrinsics_view.size() == 0)
728  {
729  return false;
730  }
731  for (auto&& msg : extrinsics_view)
732  {
733  if (msg.isType<realsense_legacy_msgs::motion_stream_info>())
734  {
735  auto msi_msg = instantiate_msg<realsense_legacy_msgs::motion_stream_info>(msg);
736  auto parsed_stream_id = legacy_file_format::parse_stream_type(msi_msg->motion_type);
737  if (stream_id.stream_type != parsed_stream_id.type || stream_id.stream_index != static_cast<uint32_t>(parsed_stream_id.index))
738  {
739  continue;
740  }
741  std::copy(std::begin(msi_msg->stream_extrinsics.extrinsics.rotation),
742  std::end(msi_msg->stream_extrinsics.extrinsics.rotation),
743  std::begin(extrinsic.rotation));
744  std::copy(std::begin(msi_msg->stream_extrinsics.extrinsics.translation),
745  std::end(msi_msg->stream_extrinsics.extrinsics.translation),
746  std::begin(extrinsic.translation));
747  group_id = static_cast<uint32_t>(msi_msg->stream_extrinsics.reference_point_id);
748  return true;
749  }
750  else if (msg.isType<realsense_legacy_msgs::stream_info>())
751  {
752  auto si_msg = instantiate_msg<realsense_legacy_msgs::stream_info>(msg);
753  auto parsed_stream_id = legacy_file_format::parse_stream_type(si_msg->stream_type);
754  if (stream_id.stream_type != parsed_stream_id.type || stream_id.stream_index != static_cast<uint32_t>(parsed_stream_id.index))
755  {
756  continue;
757  }
758  std::copy(std::begin(si_msg->stream_extrinsics.extrinsics.rotation),
759  std::end(si_msg->stream_extrinsics.extrinsics.rotation),
760  std::begin(extrinsic.rotation));
761  std::copy(std::begin(si_msg->stream_extrinsics.extrinsics.translation),
762  std::end(si_msg->stream_extrinsics.extrinsics.translation),
763  std::begin(extrinsic.translation));
764  group_id = static_cast<uint32_t>(si_msg->stream_extrinsics.reference_point_id);
765  return true;
766  }
767  else
768  {
769  throw io_exception(to_string() <<
770  "Expected either \"realsense_legacy_msgs::motion_stream_info\" or \"realsense_legacy_msgs::stream_info\", but got "
771  << msg.getDataType());
772  }
773  }
774  return false;
775  }
776  bool try_read_stream_extrinsic(const stream_identifier& stream_id, uint32_t& group_id, rs2_extrinsics& extrinsic) const
777  {
778  if (m_version == legacy_file_format::file_version())
779  {
780  return try_read_legacy_stream_extrinsic(stream_id, group_id, extrinsic);
781  }
782  rosbag::View tf_view(m_file, ExtrinsicsQuery(stream_id));
783  if (tf_view.size() == 0)
784  {
785  return false;
786  }
787  assert(tf_view.size() == 1); //There should be 1 message per stream
788  auto msg = *tf_view.begin();
789  auto tf_msg = instantiate_msg<geometry_msgs::Transform>(msg);
790  group_id = ros_topic::get_extrinsic_group_index(msg.getTopic());
791  convert(*tf_msg, extrinsic);
792  return true;
793  }
794 
795  static void update_sensor_options(const rosbag::Bag& file, uint32_t sensor_index, const nanoseconds& time, uint32_t file_version, snapshot_collection& sensor_extensions, uint32_t version)
796  {
797  if (version == legacy_file_format::file_version())
798  {
799  LOG_DEBUG("Not updating options from legacy files");
800  return;
801  }
802  auto sensor_options = read_sensor_options(file, { get_device_index(), sensor_index }, time, file_version);
803  sensor_extensions[RS2_EXTENSION_OPTIONS] = sensor_options;
804 
805  if (sensor_options->supports_option(RS2_OPTION_DEPTH_UNITS))
806  {
807  auto&& dpt_opt = sensor_options->get_option(RS2_OPTION_DEPTH_UNITS);
808  sensor_extensions[RS2_EXTENSION_DEPTH_SENSOR] = std::make_shared<depth_sensor_snapshot>(dpt_opt.query());
809 
810  if (sensor_options->supports_option(RS2_OPTION_STEREO_BASELINE))
811  {
812  auto&& bl_opt = sensor_options->get_option(RS2_OPTION_STEREO_BASELINE);
813  sensor_extensions[RS2_EXTENSION_DEPTH_STEREO_SENSOR] = std::make_shared<depth_stereo_sensor_snapshot>(dpt_opt.query(), bl_opt.query());
814  }
815  }
816  }
817 
818  device_snapshot read_device_description(const nanoseconds& time, bool reset = false)
819  {
820  if (time == get_static_file_info_timestamp())
821  {
822  if (reset)
823  {
824  snapshot_collection device_extensions;
825 
826  auto info = read_info_snapshot(ros_topic::device_info_topic(get_device_index()));
827  device_extensions[RS2_EXTENSION_INFO] = info;
828 
829  std::vector<sensor_snapshot> sensor_descriptions;
830  auto sensor_indices = read_sensor_indices(get_device_index());
831  std::map<stream_identifier, std::pair<uint32_t, rs2_extrinsics>> extrinsics_map;
832 
833  for (auto sensor_index : sensor_indices)
834  {
835  snapshot_collection sensor_extensions;
836  auto streams_snapshots = read_stream_info(get_device_index(), sensor_index);
837  for (auto stream_profile : streams_snapshots)
838  {
839  auto stream_id = stream_identifier{ get_device_index(), sensor_index, stream_profile->get_stream_type(), static_cast<uint32_t>(stream_profile->get_stream_index()) };
840  uint32_t reference_id;
841  rs2_extrinsics stream_extrinsic;
842  if (try_read_stream_extrinsic(stream_id, reference_id, stream_extrinsic))
843  {
844  extrinsics_map[stream_id] = std::make_pair(reference_id, stream_extrinsic);
845  }
846  }
847 
848  //Update infos
849  std::shared_ptr<info_container> sensor_info;
850  if (m_version == legacy_file_format::file_version())
851  {
852  sensor_info = read_legacy_info_snapshot(sensor_index);
853  }
854  else
855  {
856  sensor_info = read_info_snapshot(ros_topic::sensor_info_topic({ get_device_index(), sensor_index }));
857  }
858  sensor_extensions[RS2_EXTENSION_INFO] = sensor_info;
859  //Update options
860  update_sensor_options(m_file, sensor_index, time, m_version, sensor_extensions, m_version);
861 
862  sensor_descriptions.emplace_back(sensor_index, sensor_extensions, streams_snapshots);
863  }
864 
865  m_initial_device_description = device_snapshot(device_extensions, sensor_descriptions, extrinsics_map);
866  }
867  return m_initial_device_description;
868  }
869  else
870  {
871  //update only:
872  auto device_snapshot = m_initial_device_description;
873  for (auto& sensor : device_snapshot.get_sensors_snapshots())
874  {
875  auto& sensor_extensions = sensor.get_sensor_extensions_snapshots();
876  update_sensor_options(m_file, sensor.get_sensor_index(), time, m_version, sensor_extensions, m_version);
877  }
878  return device_snapshot;
879  }
880  }
881 
882  std::shared_ptr<info_container> read_legacy_info_snapshot(uint32_t sensor_index) const
883  {
884  std::map<rs2_camera_info, std::string> values;
885  rosbag::View view(m_file, rosbag::TopicQuery(to_string() <<"/info/" << sensor_index));
886  auto infos = std::make_shared<info_container>();
887  //TODO: properly implement, currently assuming TM2 devices
888  infos->register_info(RS2_CAMERA_INFO_NAME, to_string() << "Sensor " << sensor_index);
889  for (auto message_instance : view)
890  {
891  auto info_msg = instantiate_msg<realsense_legacy_msgs::vendor_data>(message_instance);
892  try
893  {
894  rs2_camera_info info;
895  if(legacy_file_format::info_from_string(info_msg->name, info))
896  {
897  infos->register_info(info, info_msg->value);
898  }
899  }
900  catch (const std::exception& e)
901  {
902  std::cerr << e.what() << std::endl;
903  }
904  }
905 
906  return infos;
907  }
908  std::shared_ptr<info_container> read_info_snapshot(const std::string& topic) const
909  {
910  auto infos = std::make_shared<info_container>();
911  if (m_version == legacy_file_format::file_version())
912  {
913  //TODO: properly implement, currently assuming TM2 devices and Movidius PID
914  infos->register_info(RS2_CAMERA_INFO_NAME, "Intel RealSense TM2");
915  infos->register_info(RS2_CAMERA_INFO_PRODUCT_ID, "2150");
916  infos->register_info(RS2_CAMERA_INFO_SERIAL_NUMBER, "N/A");
917  }
918  std::map<rs2_camera_info, std::string> values;
919  rosbag::View view(m_file, rosbag::TopicQuery(topic));
920  for (auto message_instance : view)
921  {
922  diagnostic_msgs::KeyValueConstPtr info_msg = instantiate_msg<diagnostic_msgs::KeyValue>(message_instance);
923  try
924  {
925  rs2_camera_info info;
926  convert(info_msg->key, info);
927  infos->register_info(info, info_msg->value);
928  }
929  catch (const std::exception& e)
930  {
931  std::cerr << e.what() << std::endl;
932  }
933  }
934 
935  return infos;
936  }
937 
938  std::set<uint32_t> read_sensor_indices(uint32_t device_index) const
939  {
940  std::set<uint32_t> sensor_indices;
941  if (m_version == legacy_file_format::file_version())
942  {
943  rosbag::View device_info(m_file, rosbag::TopicQuery("/info/4294967295"));
944  if (device_info.size() == 0)
945  {
946  throw io_exception("Missing sensor count message for legacy file");
947  }
948  for (auto info : device_info)
949  {
950  auto msg = instantiate_msg<realsense_legacy_msgs::vendor_data>(info);
951  if (msg->name == "sensor_count")
952  {
953  int sensor_count = std::stoi(msg->value);
954  while(--sensor_count >= 0)
955  sensor_indices.insert(sensor_count);
956  }
957  }
958  }
959  else
960  {
961  rosbag::View sensor_infos(m_file, SensorInfoQuery(device_index));
962  for (auto sensor_info : sensor_infos)
963  {
964  auto msg = instantiate_msg<diagnostic_msgs::KeyValue>(sensor_info);
965  sensor_indices.insert(static_cast<uint32_t>(ros_topic::get_sensor_index(sensor_info.getTopic())));
966  }
967  }
968  return sensor_indices;
969  }
970  static std::shared_ptr<stream_profile_base> create_pose_profile(uint32_t stream_index, uint32_t fps)
971  {
972  rs2_format format = RS2_FORMAT_6DOF;
973  auto profile = std::make_shared<stream_profile_base>(platform::stream_profile{ 0, 0, fps, static_cast<uint32_t>(format) });
974  profile->set_stream_index(stream_index);
975  profile->set_stream_type(RS2_STREAM_POSE);
976  profile->set_format(format);
977  profile->set_framerate(fps);
978  return profile;
979  }
980 
981  static std::shared_ptr<motion_stream_profile> create_motion_stream(rs2_stream stream_type, uint32_t stream_index, uint32_t fps, rs2_format format, rs2_motion_device_intrinsic intrinsics)
982  {
983  auto profile = std::make_shared<motion_stream_profile>(platform::stream_profile{ 0, 0, fps, static_cast<uint32_t>(format) });
984  profile->set_stream_index(stream_index);
985  profile->set_stream_type(stream_type);
986  profile->set_format(format);
987  profile->set_framerate(fps);
988  profile->set_intrinsics([intrinsics]() {return intrinsics; });
989  return profile;
990  }
991 
992  static std::shared_ptr<video_stream_profile> create_video_stream_profile(const platform::stream_profile& sp,
993  const sensor_msgs::CameraInfo& ci,
994  const stream_descriptor& sd)
995  {
996  auto profile = std::make_shared<video_stream_profile>(sp);
997  rs2_intrinsics intrinsics{};
998  intrinsics.height = ci.height;
999  intrinsics.width = ci.width;
1000  intrinsics.fx = ci.K[0];
1001  intrinsics.ppx = ci.K[2];
1002  intrinsics.fy = ci.K[4];
1003  intrinsics.ppy = ci.K[5];
1004  memcpy(intrinsics.coeffs, ci.D.data(), sizeof(intrinsics.coeffs));
1005  profile->set_intrinsics([intrinsics]() {return intrinsics; });
1006  profile->set_stream_index(sd.index);
1007  profile->set_stream_type(sd.type);
1008  profile->set_dims(ci.width, ci.height);
1009  profile->set_format(static_cast<rs2_format>(sp.format));
1010  profile->set_framerate(sp.fps);
1011  return profile;
1012  }
1013 
1014  stream_profiles read_legacy_stream_info(uint32_t sensor_index) const
1015  {
1016  //legacy files have the form of "/(camera|imu)/<stream type><stream index>/(image_imu)_raw/<sensor_index>
1017  //6DoF streams have no streaming profiles in the file - handling them seperatly
1018  stream_profiles streams;
1019  rosbag::View stream_infos_view(m_file, RegexTopicQuery(to_string() << R"RRR(/camera/(rs_stream_info|rs_motion_stream_info)/)RRR" << sensor_index));
1020  for (auto infos_msg : stream_infos_view)
1021  {
1022  if (infos_msg.isType<realsense_legacy_msgs::motion_stream_info>())
1023  {
1024  auto motion_stream_info_msg = instantiate_msg<realsense_legacy_msgs::motion_stream_info>(infos_msg);
1025  auto fps = motion_stream_info_msg->fps;
1026 
1027  std::string stream_name = motion_stream_info_msg->motion_type;
1030  rs2_motion_device_intrinsic intrinsics{};
1031  //TODO: intrinsics = motion_stream_info_msg->stream_intrinsics;
1032  auto profile = create_motion_stream(stream_id.type, stream_id.index, fps, format, intrinsics);
1033  streams.push_back(profile);
1034  }
1035  else if (infos_msg.isType<realsense_legacy_msgs::stream_info>())
1036  {
1037  auto stream_info_msg = instantiate_msg<realsense_legacy_msgs::stream_info>(infos_msg);
1038  auto fps = stream_info_msg->fps;
1039  rs2_format format;
1040  convert(stream_info_msg->encoding, format);
1041  std::string stream_name = stream_info_msg->stream_type;
1043  auto profile = create_video_stream_profile(
1044  platform::stream_profile{ stream_info_msg->camera_info.width, stream_info_msg->camera_info.height, fps, static_cast<uint32_t>(format) },
1045  stream_info_msg->camera_info,
1046  stream_id);
1047  streams.push_back(profile);
1048  }
1049  else
1050  {
1051  throw io_exception(to_string()
1052  << "Invalid file format, expected "
1055  << " message but got: " << infos_msg.getDataType()
1056  << "(Topic: " << infos_msg.getTopic() << ")");
1057  }
1058  }
1059  std::unique_ptr<rosbag::View> entire_bag = std::unique_ptr<rosbag::View>(new rosbag::View(m_file, rosbag::View::TrueQuery()));
1060  std::vector<uint32_t> indices;
1061  for (auto&& topic : get_topics(entire_bag))
1062  {
1063  std::regex r(R"RRR(/camera/rs_6DoF(\d+)/\d+)RRR");
1064  std::smatch sm;
1065  if(std::regex_search(topic, sm, r))
1066  {
1067  for (int i = 1; i<sm.size(); i++)
1068  {
1069  indices.push_back(std::stoul(sm[i].str()));
1070  }
1071  }
1072  }
1073  for (auto&& index : indices)
1074  {
1075  auto profile = create_pose_profile(index, 0); //TODO: Where to read the fps from?
1076  streams.push_back(profile);
1077  }
1078  return streams;
1079  }
1080 
1081  stream_profiles read_stream_info(uint32_t device_index, uint32_t sensor_index) const
1082  {
1083  if (m_version == legacy_file_format::file_version())
1084  {
1085  return read_legacy_stream_info(sensor_index);
1086  }
1087  stream_profiles streams;
1088  //The below regex matches both stream info messages and also video \ imu stream info (both have the same prefix)
1089  rosbag::View stream_infos_view(m_file, RegexTopicQuery("/device_" + std::to_string(device_index) + "/sensor_" + std::to_string(sensor_index) + R"RRR(/(\w)+_(\d)+/info)RRR"));
1090  for (auto infos_view : stream_infos_view)
1091  {
1092  if (infos_view.isType<realsense_msgs::StreamInfo>() == false)
1093  {
1094  continue;
1095  }
1096 
1097  stream_identifier stream_id = ros_topic::get_stream_identifier(infos_view.getTopic());
1098  auto stream_info_msg = instantiate_msg<realsense_msgs::StreamInfo>(infos_view);
1099  //auto is_recommended = stream_info_msg->is_recommended;
1100  auto fps = stream_info_msg->fps;
1101  rs2_format format;
1102  convert(stream_info_msg->encoding, format);
1103 
1104  auto video_stream_topic = ros_topic::video_stream_info_topic(stream_id);
1105  rosbag::View video_stream_infos_view(m_file, rosbag::TopicQuery(video_stream_topic));
1106  if (video_stream_infos_view.size() > 0)
1107  {
1108  assert(video_stream_infos_view.size() == 1);
1109  auto video_stream_msg_ptr = *video_stream_infos_view.begin();
1110  auto video_stream_msg = instantiate_msg<sensor_msgs::CameraInfo>(video_stream_msg_ptr);
1111  auto profile = create_video_stream_profile(
1112  platform::stream_profile{ video_stream_msg->width ,video_stream_msg->height, fps, static_cast<uint32_t>(format) }
1113  , *video_stream_msg,
1114  { stream_id.stream_type, static_cast<int>(stream_id.stream_index)});
1115  streams.push_back(profile);
1116  }
1117 
1118  auto imu_stream_topic = ros_topic::imu_intrinsic_topic(stream_id);
1119  rosbag::View imu_intrinsic_view(m_file, rosbag::TopicQuery(imu_stream_topic));
1120  if (imu_intrinsic_view.size() > 0)
1121  {
1122  assert(imu_intrinsic_view.size() == 1);
1123  auto motion_intrinsics_msg = instantiate_msg<realsense_msgs::ImuIntrinsic>(*imu_intrinsic_view.begin());
1124  rs2_motion_device_intrinsic intrinsics{};
1125  std::copy(std::begin(motion_intrinsics_msg->bias_variances), std::end(motion_intrinsics_msg->bias_variances), std::begin(intrinsics.bias_variances));
1126  std::copy(std::begin(motion_intrinsics_msg->noise_variances), std::end(motion_intrinsics_msg->noise_variances), std::begin(intrinsics.noise_variances));
1127  std::copy(std::begin(motion_intrinsics_msg->data), std::end(motion_intrinsics_msg->data), &intrinsics.data[0][0]);
1128  auto profile = create_motion_stream(stream_id.stream_type, stream_id.stream_index, fps, format, intrinsics);
1129  streams.push_back(profile);
1130  }
1131 
1132  if (stream_id.stream_type == RS2_STREAM_POSE)
1133  {
1134  auto profile = create_pose_profile(stream_id.stream_index, fps);
1135  streams.push_back(profile);
1136  }
1137 
1138  if (video_stream_infos_view.size() == 0 && imu_intrinsic_view.size() == 0 && stream_id.stream_type != RS2_STREAM_POSE)
1139  {
1140  throw io_exception(to_string() << "Every StreamInfo is expected to have a complementary video/imu message, but none was found");
1141  }
1142  }
1143  return streams;
1144  }
1145 
1146  static std::string read_option_description(const rosbag::Bag& file, const std::string& topic)
1147  {
1148  rosbag::View option_description_view(file, rosbag::TopicQuery(topic));
1149  if (option_description_view.size() == 0)
1150  {
1151  LOG_ERROR("File does not contain topics for: " << topic);
1152  return "N/A";
1153  }
1154  assert(option_description_view.size() == 1); //There should be only 1 message for each option
1155  auto description_message_instance = *option_description_view.begin();
1156  auto option_desc_msg = instantiate_msg<std_msgs::String>(description_message_instance);
1157  return option_desc_msg->data;
1158  }
1159 
1160  /*Until Version 2 (including)*/
1161  static std::pair<rs2_option, std::shared_ptr<librealsense::option>> create_property(const rosbag::MessageInstance& property_message_instance)
1162  {
1163  auto property_msg = instantiate_msg<diagnostic_msgs::KeyValue>(property_message_instance);
1164  rs2_option id;
1165  convert(property_msg->key, id);
1166  float value = std::stof(property_msg->value);
1167  std::string description = to_string() << "Read only option of " << id;
1168  return std::make_pair(id, std::make_shared<const_value_option>(description, value));
1169  }
1170 
1171  /*Starting version 3*/
1172  static std::pair<rs2_option, std::shared_ptr<librealsense::option>> create_option(const rosbag::Bag& file, const rosbag::MessageInstance& value_message_instance)
1173  {
1174  auto option_value_msg = instantiate_msg<std_msgs::Float32>(value_message_instance);
1175  std::string option_name = ros_topic::get_option_name(value_message_instance.getTopic());
1176  device_serializer::sensor_identifier sensor_id = ros_topic::get_sensor_identifier(value_message_instance.getTopic());
1177  rs2_option id;
1178  convert(option_name, id);
1179  float value = option_value_msg->data;
1180  std::string description = read_option_description(file, ros_topic::option_description_topic(sensor_id, id));
1181  return std::make_pair(id, std::make_shared<const_value_option>(description, value));
1182  }
1183 
1184  static notification create_notification(const rosbag::Bag& file, const rosbag::MessageInstance& message_instance)
1185  {
1186  auto notification_msg = instantiate_msg<realsense_msgs::Notification>(message_instance);
1187  rs2_notification_category category;
1188  rs2_log_severity severity;
1189  convert(notification_msg->category, category);
1190  convert(notification_msg->severity, severity);
1191  int type = 0; //TODO: what is this for?
1192  notification n(category, type, severity, notification_msg->description);
1193  n.timestamp = to_nanoseconds(notification_msg->timestamp).count();
1194  n.serialized_data = notification_msg->serialized_data;
1195  return n;
1196  }
1197 
1198  static std::shared_ptr<options_container> read_sensor_options(const rosbag::Bag& file, device_serializer::sensor_identifier sensor_id, const nanoseconds& timestamp, uint32_t file_version)
1199  {
1200  auto options = std::make_shared<options_container>();
1201  if (file_version == 2)
1202  {
1203  rosbag::View sensor_options_view(file, rosbag::TopicQuery(ros_topic::property_topic(sensor_id)));
1204  for (auto message_instance : sensor_options_view)
1205  {
1206  auto id_option = create_property(message_instance);
1207  options->register_option(id_option.first, id_option.second);
1208  }
1209  }
1210  else
1211  {
1212  //Taking all messages from the beginning of the bag until the time point requested
1213  for (int i = 0; i < static_cast<int>(RS2_OPTION_COUNT); i++)
1214  {
1215  rs2_option id = static_cast<rs2_option>(i);
1216  std::string option_topic = ros_topic::option_value_topic(sensor_id, id);
1217  rosbag::View option_view(file, rosbag::TopicQuery(option_topic), to_rostime(get_static_file_info_timestamp()), to_rostime(timestamp));
1218  auto it = option_view.begin();
1219  if (it == option_view.end())
1220  {
1221  continue;
1222  }
1223  rosbag::View::iterator last_item;
1224  while (it != option_view.end())
1225  {
1226  last_item = it++;
1227  }
1228  auto option = create_option(file, *last_item);
1229  assert(id == option.first);
1230  options->register_option(option.first, option.second);
1231  }
1232  }
1233  return options;
1234  }
1235 
1236  static std::vector<std::string> get_topics(std::unique_ptr<rosbag::View>& view)
1237  {
1238  std::vector<std::string> topics;
1239  if(view != nullptr)
1240  {
1241  auto connections = view->getConnections();
1242  std::transform(connections.begin(), connections.end(), std::back_inserter(topics), [](const rosbag::ConnectionInfo* connection) { return connection->topic; });
1243  }
1244  return topics;
1245  }
1246 
1247  device_snapshot m_initial_device_description;
1248  nanoseconds m_total_duration;
1249  std::string m_file_path;
1250  std::shared_ptr<frame_source> m_frame_source;
1251  rosbag::Bag m_file;
1252  std::unique_ptr<rosbag::View> m_samples_view;
1253  rosbag::View::iterator m_samples_itrator;
1254  std::vector<std::string> m_enabled_streams_topics;
1255  std::shared_ptr<metadata_parser_map> m_metadata_parser_map;
1256  std::shared_ptr<context> m_context;
1257  uint32_t m_version;
1258  };
1259 }
uint32_t fps
Definition: backend.h:135
Definition: rs_option.h:66
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
static std::string frame_data_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:346
bool fisheye_ae_mode
Definition: archive.h:28
constexpr uint32_t file_version()
Definition: ros_file_format.h:695
nanoseconds query_duration() const override
Definition: ros_reader.h:141
static std::string sensor_info_topic(const device_serializer::sensor_identifier &sensor_id)
Definition: ros_file_format.h:294
float x
Definition: types.h:415
Definition: backend.h:378
int get_height() const
Definition: archive.h:235
Definition: ros_file_format.h:507
Definition: types.h:418
rs2_option
Defines general configuration controls. These can generally be mapped to camera UVC controls...
Definition: rs_option.h:22
unsigned char byte
Definition: types.h:33
static std::string get_option_name(const std::string &topic)
Definition: ros_file_format.h:282
Definition: streaming.h:63
int get_width() const
Definition: archive.h:234
uint32_t metadata_size
Definition: archive.h:27
Definition: archive.h:435
Definition: rs_types.h:104
Definition: options.h:20
Definition: rs_sensor.h:24
float translation[3]
Definition: rs_sensor.h:84
#define LOG_WARNING(...)
Definition: types.h:110
ros_reader(const std::string &file, const std::shared_ptr< context > &ctx)
Definition: ros_reader.h:20
Definition: rs_sensor.h:29
constexpr const char * FRAME_TIMESTAMP_MD_STR
Definition: ros_file_format.h:192
Definition: rs_sensor.h:74
Definition: rs_option.h:64
std::shared_ptr< serialized_data > read_next_data() override
Definition: ros_reader.h:44
uint32_t format
Definition: backend.h:136
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
frame()
Definition: archive.h:69
sql::statement::iterator end(sql::statement &stmt)
Definition: types.h:489
Definition: rs_sensor.h:47
static std::string video_stream_info_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:302
Definition: archive.h:20
float data[3][4]
Definition: rs_types.h:74
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_sensor.h:23
Definition: rs_types.h:102
constexpr const char * TRACKER_CONFIDENCE_MD_STR
Definition: ros_file_format.h:193
Definition: rs_sensor.h:44
Definition: rs_option.h:52
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:83
Definition: types.h:182
static device_serializer::stream_identifier get_stream_identifier(const std::string &topic)
Definition: ros_file_format.h:272
Definition: archive.h:63
Definition: ros_reader.h:17
Definition: ros_file_format.h:484
Definition: algo.h:16
void reset() override
Definition: ros_reader.h:146
float noise_variances[3]
Definition: rs_types.h:76
Definition: archive.h:422
#define LOG_DEBUG(...)
Definition: types.h:108
Definition: archive.h:431
static device_serializer::sensor_identifier get_sensor_identifier(const std::string &topic)
Definition: ros_file_format.h:267
Definition: rs_types.h:107
rs2_stream stream_type
Definition: serialization.h:26
Definition: rs_types.h:112
Definition: types.h:415
Definition: rs_frame.h:42
sql::statement::iterator begin(sql::statement &stmt)
constexpr uint32_t get_maximum_supported_legacy_file_version()
Definition: ros_file_format.h:700
static std::string pose_accel_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:335
Definition: rs_types.h:99
Definition: context.h:46
rs2_time_t system_time
Definition: archive.h:25
bool is_camera(rs2_stream s)
Definition: ros_file_format.h:809
virtual void enable_stream(const std::vector< device_serializer::stream_identifier > &stream_ids) override
Definition: ros_reader.h:157
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:54
static std::string file_version_topic()
Definition: ros_file_format.h:286
uint32_t sensor_index
Definition: serialization.h:25
static std::string property_topic(const device_serializer::sensor_identifier &sensor_id)
Definition: ros_file_format.h:312
void seek_to_time(const nanoseconds &seek_time) override
Definition: ros_reader.h:90
constexpr const char * SYSTEM_TIME_MD_STR
Definition: ros_file_format.h:190
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_sensor.h:72
Definition: ros_file_format.h:520
std::vector< std::shared_ptr< stream_profile_interface >> stream_profiles
Definition: streaming.h:104
Definition: rs_sensor.h:40
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:37
const std::string & get_file_name() const override
Definition: ros_reader.h:236
constexpr const char * TIMESTAMP_DOMAIN_MD_STR
Definition: ros_file_format.h:189
virtual void disable_stream(const std::vector< device_serializer::stream_identifier > &stream_ids) override
Definition: ros_reader.h:201
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
Definition: ros_file_format.h:513
rs2_time_t timestamp
Definition: archive.h:22
static std::string frame_metadata_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:351
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:45
Definition: types.h:55
float x
Definition: types.h:416
uint32_t width
Definition: backend.h:133
Cross-stream extrinsics: encode the topology describing how the different devices are connected...
Definition: rs_sensor.h:81
Definition: types.h:416
Definition: rs_types.h:103
std::vector< sensor_snapshot > get_sensors_snapshots() const
Definition: serialization.h:277
virtual std::shared_ptr< stream_profile_interface > get_stream() const =0
stream_descriptor parse_stream_type(const std::string &source)
Definition: ros_file_format.h:736
void assign(int width, int height, int stride, int bpp)
Definition: archive.h:239
Definition: ros_file_format.h:530
Definition: ros_file_format.h:458
Definition: types.h:470
constexpr device_serializer::nanoseconds get_static_file_info_timestamp()
Definition: ros_file_format.h:571
static std::string option_value_topic(const device_serializer::sensor_identifier &sensor_id, rs2_option option_type)
Definition: ros_file_format.h:318
rs2_extension
Specifies advanced interfaces (capabilities) objects may implement.
Definition: rs_types.h:93
rs2_notification_category
Category of the librealsense notifications.
Definition: rs_types.h:17
int stream_id
Definition: sync.h:17
long long rs2_metadata_type
Definition: rs_types.h:180
double timestamp
Definition: types.h:909
Definition: types.h:637
constexpr uint32_t get_minimum_supported_file_version()
Definition: ros_file_format.h:561
rs2_timestamp_domain convert(uint64_t source)
Definition: ros_file_format.h:658
static uint32_t get_extrinsic_group_index(const std::string &topic)
Definition: ros_file_format.h:277
Definition: rs_types.h:97
rs2_timestamp_domain timestamp_domain
Definition: archive.h:24
std::string serialized_data
Definition: types.h:910
unsigned long long frame_number
Definition: archive.h:23
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
device_snapshot query_device_description(const nanoseconds &time) override
Definition: ros_reader.h:39
Definition: ros_file_format.h:539
Definition: types.h:896
Motion device intrinsics: scale, bias, and variances.
Definition: rs_types.h:68
std::array< uint8_t, MAX_META_DATA_SIZE > metadata_blob
Definition: archive.h:29
Definition: rs_types.h:115
rs2_log_severity
Severity of the librealsense logger.
Definition: rs_types.h:81
ros::Time to_rostime(const device_serializer::nanoseconds &t)
Definition: ros_file_format.h:584
float bias_variances[3]
Definition: rs_types.h:77
Definition: ros_file_format.h:796
std::vector< std::shared_ptr< serialized_data > > fetch_last_frames(const nanoseconds &seek_time) override
Definition: ros_reader.h:111
Definition: ros_file_format.h:832
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: ros_file_format.h:819
#define LOG_ERROR(...)
Definition: types.h:111
void copy(void *dst, void const *src, size_t size)
constexpr uint32_t SYSTEM_TIMESTAMP
Definition: ros_file_format.h:627
Definition: serialization.h:336
constexpr uint32_t get_device_index()
Definition: ros_file_format.h:566
std::vector< byte > data
Definition: archive.h:66
virtual void set_stream(std::shared_ptr< stream_profile_interface > sp)=0
static std::string stream_full_prefix(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:366