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 #include "rosbag/bag.h"
6 #include "ros_file_format.h"
7 
8 namespace librealsense
9 {
10  using namespace device_serializer;
11 
12  class ros_writer: public writer
13  {
14  public:
15  explicit ros_writer(const std::string& file, bool compress_while_record);
16  void write_device_description(const librealsense::device_snapshot& device_description) override;
17  void write_frame(const stream_identifier& stream_id, const nanoseconds& timestamp, frame_holder&& frame) override;
18  void write_snapshot(uint32_t device_index, const nanoseconds& timestamp, rs2_extension type, const std::shared_ptr<extension_snapshot>& snapshot) override;
19  void write_snapshot(const sensor_identifier& sensor_id, const nanoseconds& timestamp, rs2_extension type, const std::shared_ptr<extension_snapshot>& snapshot) override;
20  const std::string& get_file_name() const override;
21 
22  private:
23  void write_file_version();
24  void write_frame_metadata(const stream_identifier& stream_id, const nanoseconds& timestamp, frame_interface* frame);
25  void write_extrinsics(const stream_identifier& stream_id, frame_interface* frame);
26  realsense_msgs::Notification to_notification_msg(const notification& n);
27  void write_notification(const sensor_identifier& sensor_id, const nanoseconds& timestamp, const notification& n) override;
28  void write_additional_frame_messages(const stream_identifier& stream_id, const nanoseconds& timestamp, frame_interface* frame);
29  void write_video_frame(const stream_identifier& stream_id, const nanoseconds& timestamp, frame_holder&& frame);
30  void write_motion_frame(const stream_identifier& stream_id, const nanoseconds& timestamp, frame_holder&& frame);
31  inline geometry_msgs::Vector3 to_vector3(const float3& f);
32  inline geometry_msgs::Quaternion to_quaternion(const float4& f);
33  void write_pose_frame(const stream_identifier& stream_id, const nanoseconds& timestamp, frame_holder&& frame);
34  void write_stream_info(nanoseconds timestamp, const sensor_identifier& sensor_id, std::shared_ptr<stream_profile_interface> profile);
35  void write_streaming_info(nanoseconds timestamp, const sensor_identifier& sensor_id, std::shared_ptr<video_stream_profile_interface> profile);
36  void write_streaming_info(nanoseconds timestamp, const sensor_identifier& sensor_id, std::shared_ptr<motion_stream_profile_interface> profile);
37  void write_streaming_info(nanoseconds timestamp, const sensor_identifier& sensor_id, std::shared_ptr<pose_stream_profile_interface> profile);
38  void write_extension_snapshot(uint32_t device_id, const nanoseconds& timestamp, rs2_extension type, std::shared_ptr<librealsense::extension_snapshot> snapshot);
39  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);
40 
41  template <rs2_extension E>
42  std::shared_ptr<typename ExtensionToType<E>::type> SnapshotAs(std::shared_ptr<librealsense::extension_snapshot> snapshot)
43  {
44  auto as_type = As<typename ExtensionToType<E>::type>(snapshot);
45  if (as_type == nullptr)
46  {
47  throw invalid_value_exception(to_string() << "Failed to cast snapshot to \"" << E << "\" (as \"" << ExtensionToType<E>::to_string() << "\")");
48  }
49  return as_type;
50  }
51 
52  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);
53  void write_vendor_info(const std::string& topic, nanoseconds timestamp, std::shared_ptr<info_interface> info_snapshot);
54  void write_sensor_option(device_serializer::sensor_identifier sensor_id, const nanoseconds& timestamp, rs2_option type, const librealsense::option& option);
55  void write_sensor_options(device_serializer::sensor_identifier sensor_id, const nanoseconds& timestamp, std::shared_ptr<options_interface> options);
56  void write_l500_data(device_serializer::sensor_identifier sensor_id, const nanoseconds& timestamp, std::shared_ptr<l500_depth_sensor_interface> l500_depth_sensor);
57 
58  rs2_extension get_processing_block_extension(const std::shared_ptr<processing_block_interface> block);
59  void write_sensor_processing_blocks(device_serializer::sensor_identifier sensor_id, const nanoseconds& timestamp, std::shared_ptr<recommended_proccesing_blocks_interface> proccesing_blocks);
60 
61  template <typename T>
62  void write_message(std::string const& topic, nanoseconds const& time, T const& msg)
63  {
64  try
65  {
66  m_bag.write(topic, to_rostime(time), msg);
67  LOG_DEBUG("Recorded: \"" << topic << "\" . TS: " << time.count());
68  }
69  catch (rosbag::BagIOException& e)
70  {
71  throw io_exception(to_string() << "Ros Writer failed to write topic: \"" << topic << "\" to file. (Exception message: " << e.what() << ")");
72  }
73  }
74 
75  static uint8_t is_big_endian();
76  std::map<stream_identifier, geometry_msgs::Transform> m_extrinsics_msgs;
79  std::map<uint32_t, std::set<rs2_option>> m_written_options_descriptions;
80  };
81 }
rs2_option
Defines general configuration controls. These can generally be mapped to camera UVC controls...
Definition: rs_option.h:22
rs2rosinternal::Time to_rostime(const device_serializer::nanoseconds &t)
std::map< uint32_t, std::set< rs2_option > > m_written_options_descriptions
Definition: ros_writer.h:79
std::shared_ptr< typename ExtensionToType< E >::type > SnapshotAs(std::shared_ptr< librealsense::extension_snapshot > snapshot)
Definition: ros_writer.h:42
GLsizei const GLchar *const * string
GLdouble n
Definition: glext.h:1966
unsigned char uint8_t
Definition: stdint.h:78
e
Definition: rmse.py:177
GLdouble f
std::map< stream_identifier, geometry_msgs::Transform > m_extrinsics_msgs
Definition: ros_writer.h:76
std::string m_file_path
Definition: ros_writer.h:77
unsigned int uint32_t
Definition: stdint.h:80
Exception thrown when on IO problems.
Definition: exceptions.h:50
std::chrono::duration< uint64_t, std::nano > nanoseconds
std::string get_file_name(const std::string &path)
Definition: os.cpp:224
rs2_extension
Specifies advanced interfaces (capabilities) objects may implement.
Definition: rs_types.h:166
int stream_id
Definition: sync.h:17
GLenum type
#define LOG_DEBUG(...)
Definition: src/types.h:239
void write_message(std::string const &topic, nanoseconds const &time, T const &msg)
Definition: ros_writer.h:62
std::string to_string(T value)


librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:47:40