15 #include "rosbag/bag.h" 20 using namespace device_serializer;
25 explicit ros_writer(
const std::string& file) : m_file_path(file)
27 m_bag.open(file, rosbag::BagMode::Write);
28 m_bag.setCompression(rosbag::CompressionType::LZ4);
41 for (
auto&& sensor_extension_snapshot : sensors_snapshot.get_sensor_extensions_snapshots().get_snapshots())
52 write_video_frame(stream_id, timestamp, std::move(
frame));
58 write_motion_frame(stream_id, timestamp, std::move(
frame));
64 write_pose_frame(stream_id, timestamp, std::move(
frame));
71 write_extension_snapshot(device_index, -1, timestamp, type, snapshot);
85 void write_file_version()
96 diagnostic_msgs::KeyValue system_time;
99 write_message(metadata_topic, timestamp, system_time);
101 diagnostic_msgs::KeyValue timestamp_domain;
104 write_message(metadata_topic, timestamp, timestamp_domain);
112 diagnostic_msgs::KeyValue md_msg;
114 md_msg.value = std::to_string(md);
115 write_message(metadata_topic, timestamp, md_msg);
122 if (m_extrinsics_msgs.find(stream_id) != m_extrinsics_msgs.end())
126 auto& dev = frame->
get_sensor()->get_device();
127 uint32_t reference_id = 0;
129 std::tie(reference_id, ext) = dev.get_extrinsics(*frame->
get_stream());
130 geometry_msgs::Transform tf_msg;
136 realsense_msgs::Notification to_notification_msg(
const notification& n)
138 realsense_msgs::Notification noti_msg;
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());
150 realsense_msgs::Notification noti_msg = to_notification_msg(n);
158 write_frame_metadata(stream_id, timestamp, frame);
160 catch (std::exception
const& e)
162 LOG_WARNING(
"Failed to write frame metadata for " << stream_id <<
". Exception: " << e.what());
167 write_extrinsics(stream_id, frame);
169 catch (std::exception
const& e)
171 LOG_WARNING(
"Failed to write stream extrinsics for " << stream_id <<
". Exception: " << e.what());
177 sensor_msgs::Image image;
179 assert(vid_frame !=
nullptr);
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;
195 write_message(image_topic, timestamp, image);
196 write_additional_frame_messages(stream_id, timestamp, frame);
201 sensor_msgs::Imu imu_msg;
204 throw io_exception(
"Null frame passed to write_motion_frame");
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());
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];
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];
228 throw io_exception(
"Unsupported stream type for a motion frame");
232 write_message(topic, timestamp, imu_msg);
233 write_additional_frame_messages(stream_id, timestamp, frame);
236 inline geometry_msgs::Vector3 to_vector3(
const float3& f)
238 geometry_msgs::Vector3 v;
245 inline geometry_msgs::Quaternion to_quaternion(
const float4& f)
247 geometry_msgs::Quaternion q;
257 auto pose = As<librealsense::pose_frame>(frame.frame);
260 throw io_exception(
"Null frame passed to write_motion_frame");
262 auto rotation =
pose->get_rotation();
264 geometry_msgs::Transform transform;
265 geometry_msgs::Accel accel;
266 geometry_msgs::Twist twist;
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());
280 write_message(transform_topic, timestamp, transform);
281 write_message(accel_topic, timestamp, accel);
282 write_message(twist_topic, timestamp, twist);
287 diagnostic_msgs::KeyValue tracker_confidence_msg;
289 tracker_confidence_msg.value = std::to_string(
pose->get_tracker_confidence());
290 write_message(md_topic, timestamp, tracker_confidence_msg);
292 diagnostic_msgs::KeyValue mapper_confidence_msg;
294 mapper_confidence_msg.value = std::to_string(
pose->get_mapper_confidence());
295 write_message(md_topic, timestamp, mapper_confidence_msg);
298 diagnostic_msgs::KeyValue frame_timestamp_msg;
300 frame_timestamp_msg.value =
to_string() << std::hexfloat <<
pose->get_frame_timestamp();
301 write_message(md_topic, timestamp, frame_timestamp_msg);
304 write_additional_frame_messages(stream_id, timestamp, frame);
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();
316 void write_streaming_info(
nanoseconds timestamp,
const sensor_identifier& sensor_id, std::shared_ptr<video_stream_profile_interface> profile)
318 write_stream_info(timestamp, sensor_id, profile);
320 sensor_msgs::CameraInfo camera_info_msg;
321 camera_info_msg.width = profile->get_width();
322 camera_info_msg.height = profile->get_height();
325 intrinsics = profile->get_intrinsics();
329 LOG_ERROR(
"Error trying to get intrinsc data for stream " << profile->get_stream_type() <<
", " << profile->get_stream_index());
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;
341 void write_streaming_info(
nanoseconds timestamp,
const sensor_identifier& sensor_id, std::shared_ptr<motion_stream_profile_interface> profile)
343 write_stream_info(timestamp, sensor_id, profile);
345 realsense_msgs::ImuIntrinsic motion_info_msg;
349 intrinsics = profile->get_intrinsics();
353 LOG_ERROR(
"Error trying to get intrinsc data for stream " << profile->get_stream_type() <<
", " << profile->get_stream_index());
356 std::copy(&intrinsics.data[0][0], &intrinsics.data[0][0] + motion_info_msg.data.size(),
std::begin(motion_info_msg.data));
361 write_message(topic, timestamp, motion_info_msg);
363 void write_streaming_info(
nanoseconds timestamp,
const sensor_identifier& sensor_id, std::shared_ptr<pose_stream_profile_interface> profile)
365 write_stream_info(timestamp, sensor_id, profile);
367 void write_extension_snapshot(uint32_t device_id,
const nanoseconds& timestamp,
rs2_extension type, std::shared_ptr<librealsense::extension_snapshot> snapshot)
369 const auto ignored = 0u;
370 write_extension_snapshot(device_id, ignored, timestamp, type, snapshot,
true);
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)
375 write_extension_snapshot(device_id, sensor_id, timestamp, type, snapshot,
false);
378 template <rs2_extension E>
379 std::shared_ptr<typename ExtensionToType<E>::type> SnapshotAs(std::shared_ptr<librealsense::extension_snapshot> snapshot)
381 auto as_type = As<typename ExtensionToType<E>::type>(snapshot);
382 if (as_type ==
nullptr)
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)
394 auto info = SnapshotAs<RS2_EXTENSION_INFO>(snapshot);
415 auto options = SnapshotAs<RS2_EXTENSION_OPTIONS>(snapshot);
416 write_sensor_options({ device_id, sensor_id }, timestamp, options);
426 auto profile = SnapshotAs<RS2_EXTENSION_VIDEO_PROFILE>(snapshot);
427 write_streaming_info(timestamp, { device_id, sensor_id }, profile);
432 auto profile = SnapshotAs<RS2_EXTENSION_MOTION_PROFILE>(snapshot);
433 write_streaming_info(timestamp, { device_id, sensor_id }, profile);
438 auto profile = SnapshotAs<RS2_EXTENSION_POSE_PROFILE>(snapshot);
439 write_streaming_info(timestamp, { device_id, sensor_id }, profile);
447 void write_vendor_info(
const std::string& topic,
nanoseconds timestamp, std::shared_ptr<info_interface> info_snapshot)
452 if (info_snapshot->supports_info(camera_info))
454 diagnostic_msgs::KeyValue msg;
456 msg.value = info_snapshot->get_info(camera_info);
457 write_message(topic, timestamp, msg);
469 std_msgs::Float32 option_msg;
470 option_msg.data =
value;
475 if (m_written_options_descriptions[sensor_id.
sensor_index].find(type) == m_written_options_descriptions[sensor_id.
sensor_index].end())
477 std_msgs::String option_msg_desc;
478 option_msg_desc.data = description;
480 m_written_options_descriptions[sensor_id.
sensor_index].insert(type);
491 if (options->supports_option(option_id))
493 write_sensor_option(sensor_id, timestamp, option_id, options->get_option(option_id));
496 catch (std::exception& e)
498 LOG_WARNING(
"Failed to get or write option " << option_id <<
" for sensor " << sensor_id.
sensor_index <<
". Exception: " << e.what());
502 template <
typename T>
503 void write_message(std::string
const& topic,
nanoseconds const& time, T
const& msg)
508 LOG_DEBUG(
"Recorded: \"" << topic <<
"\" . TS: " << time.count());
510 catch (rosbag::BagIOException& e)
512 throw io_exception(
to_string() <<
"Ros Writer failed to write topic: \"" << topic <<
"\" to file. (Exception message: " << e.what() <<
")");
516 static uint8_t is_big_endian()
519 return (*reinterpret_cast<char*>(&num) == 1) ? 0 : 1;
522 std::map<stream_identifier, geometry_msgs::Transform> m_extrinsics_msgs;
523 std::string m_file_path;
525 std::map<uint32_t, std::set<rs2_option>> m_written_options_descriptions;
virtual rs2_metadata_type get_frame_metadata(const rs2_frame_metadata_value &frame_metadata) const =0
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
constexpr uint32_t get_file_version()
Definition: ros_file_format.h:556
static std::string frame_data_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:346
virtual const byte * get_frame_data() const =0
virtual rs2_timestamp_domain get_frame_timestamp_domain() const =0
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:415
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:929
Definition: serialization.h:324
float w
Definition: types.h:416
#define LOG_WARNING(...)
Definition: types.h:110
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:415
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: serialization.h:22
Definition: rs_types.h:103
constexpr const char * TRACKER_CONFIDENCE_MD_STR
Definition: ros_file_format.h:193
Definition: rs_sensor.h:45
virtual float query() const =0
std::map< rs2_extension, std::shared_ptr< extension_snapshot > > get_snapshots() const
Definition: serialization.h:208
Definition: rs_types.h:115
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:108
Definition: rs_types.h:102
rs2_stream stream_type
Definition: serialization.h:26
float z
Definition: types.h:415
Definition: rs_types.h:113
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:100
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 ×tamp, 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
virtual rs2_time_t get_frame_timestamp() const =0
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:33
static std::string imu_intrinsic_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:306
Definition: serialization.h:266
constexpr const char * MAPPER_CONFIDENCE_MD_STR
Definition: ros_file_format.h:191
Definition: rs_types.h:101
virtual std::shared_ptr< sensor_interface > get_sensor() const =0
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:46
float x
Definition: types.h:416
Cross-stream extrinsics: encode the topology describing how the different devices are connected...
Definition: rs_sensor.h:82
void write_snapshot(uint32_t device_index, const nanoseconds ×tamp, 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
virtual std::shared_ptr< stream_profile_interface > get_stream() const =0
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:416
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
Definition: rs_types.h:97
virtual rs2_time_t get_frame_system_time() const =0
rs2_extension
Specifies advanced interfaces (capabilities) objects may implement.
Definition: rs_types.h:94
int stream_id
Definition: sync.h:17
double timestamp
Definition: types.h:933
Definition: rs_types.h:111
Definition: rs_types.h:98
std::string serialized_data
Definition: types.h:934
Video stream intrinsics.
Definition: rs_types.h:56
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:416
std::string description
Definition: types.h:932
Motion device intrinsics: scale, bias, and variances.
Definition: rs_types.h:69
void write_frame(const stream_identifier &stream_id, const nanoseconds ×tamp, 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: serialization.h:17
Definition: rs_types.h:117
ros::Time to_rostime(const device_serializer::nanoseconds &t)
Definition: ros_file_format.h:584
rs2_log_severity severity
Definition: types.h:931
virtual unsigned long long get_frame_number() const =0
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
#define LOG_ERROR(...)
Definition: types.h:111
void copy(void *dst, void const *src, size_t size)
virtual bool supports_frame_metadata(const rs2_frame_metadata_value &frame_metadata) const =0
constexpr uint32_t get_device_index()
Definition: ros_file_format.h:566
static std::string stream_info_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:298