10 #include "rosbag/view.h" 15 using namespace device_serializer;
20 ros_reader(
const std::string& file,
const std::shared_ptr<context>& ctx) :
25 m_metadata_parser_map(create_metadata_parser_map())
30 m_total_duration = get_file_duration(m_file, m_version);
32 catch (
const std::exception& e)
41 return read_device_description(time);
46 if (m_samples_view ==
nullptr || m_samples_itrator == m_samples_view->end())
49 return std::make_shared<serialized_end_of_file>();
52 rosbag::MessageInstance next_msg = *m_samples_itrator;
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>())
61 return create_frame(next_msg);
66 if (next_msg.isType<std_msgs::Float32>())
71 auto option = create_option(m_file, next_msg);
72 return std::make_shared<serialized_option>(timestamp, sensor_id,
option.first,
option.second);
75 if (next_msg.isType<realsense_msgs::Notification>())
77 LOG_DEBUG(
"Next message is a notification");
80 auto notification = create_notification(m_file, next_msg);
81 return std::make_shared<serialized_notification>(timestamp, sensor_id,
notification);
85 std::string err_msg =
to_string() <<
"Unknown message type: " << next_msg.getDataType() <<
"(Topic: " << next_msg.getTopic() <<
")";
92 if (seek_time > m_total_duration)
94 throw invalid_value_exception(
to_string() <<
"Requested time is out of playback length. (Requested = " << seek_time.count() <<
", Duration = " << m_total_duration.count() <<
")");
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());
99 m_samples_view.reset(
new rosbag::View(m_file,
FalseQuery()));
104 for (
auto topic : m_enabled_streams_topics)
106 m_samples_view->addQuery(m_file, rosbag::TopicQuery(topic), seek_time_as_rostime);
108 m_samples_itrator = m_samples_view->begin();
113 std::vector<std::shared_ptr<serialized_data>> result;
118 for (
auto topic : m_enabled_streams_topics)
120 view.addQuery(m_file, rosbag::TopicQuery(topic), start_time, as_rostime);
122 std::map<device_serializer::stream_identifier, ros::Time> last_frames;
123 for (
auto&& m : view)
125 if (m.isType<sensor_msgs::Image>() || m.isType<sensor_msgs::Imu>())
128 last_frames[id] = m.getTime();
131 for (
auto&& kvp : last_frames)
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);
143 return m_total_duration;
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);
157 virtual void enable_stream(
const std::vector<device_serializer::stream_identifier>& stream_ids)
override 159 ros::Time start_time = ros::TIME_MIN + ros::Duration{ 0, 1 };
160 if (m_samples_view ==
nullptr)
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);
165 m_samples_itrator = m_samples_view->begin();
169 if (m_samples_itrator != m_samples_view->end())
171 rosbag::MessageInstance sample_msg = *m_samples_itrator;
172 start_time = sample_msg.getTime();
175 auto currently_streaming = get_topics(m_samples_view);
177 m_samples_view = std::unique_ptr<rosbag::View>(
new rosbag::View(m_file,
FalseQuery()));
193 for (
auto topic : currently_streaming)
195 m_samples_view->addQuery(m_file, rosbag::TopicQuery(topic), start_time);
197 m_samples_itrator = m_samples_view->begin();
198 m_enabled_streams_topics = get_topics(m_samples_view);
201 virtual void disable_stream(
const std::vector<device_serializer::stream_identifier>& stream_ids)
override 203 if (m_samples_view ==
nullptr)
208 if (m_samples_itrator == m_samples_view->end())
210 curr_time = m_samples_view->getEndTime();
214 rosbag::MessageInstance sample_msg = *m_samples_itrator;
215 curr_time = sample_msg.getTime();
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)
226 bool should_topic_remain = (it == stream_ids.end());
227 if (should_topic_remain)
229 m_samples_view->addQuery(m_file, rosbag::TopicQuery(topic), curr_time);
232 m_samples_itrator = m_samples_view->begin();
233 m_enabled_streams_topics = get_topics(m_samples_view);
243 template <
typename ROS_TYPE>
244 static typename ROS_TYPE::ConstPtr instantiate_msg(
const rosbag::MessageInstance& msg)
246 typename ROS_TYPE::ConstPtr msg_instnance_ptr = msg.instantiate<ROS_TYPE>();
247 if (msg_instnance_ptr ==
nullptr)
250 <<
"Invalid file format, expected " 252 <<
" message but got: " << msg.getDataType()
253 <<
"(Topic: " << msg.getTopic() <<
")");
255 return msg_instnance_ptr;
258 std::shared_ptr<serialized_frame> create_frame(
const rosbag::MessageInstance& msg)
260 auto next_msg_topic = msg.getTopic();
261 auto next_msg_time = msg.getTime();
274 if (msg.isType<sensor_msgs::Image>())
276 frame = create_image_from_message(msg);
278 else if (msg.isType<sensor_msgs::Imu>())
280 frame = create_motion_sample(msg);
282 else if (msg.isType<realsense_legacy_msgs::pose>() || msg.isType<geometry_msgs::Transform>())
284 frame = create_pose_sample(msg);
288 std::string err_msg =
to_string() <<
"Unknown frame type: " << msg.getDataType() <<
"(Topic: " << next_msg_topic <<
")";
295 return std::make_shared<serialized_invalid_frame>(timestamp,
stream_id);
297 return std::make_shared<serialized_frame>(timestamp,
stream_id, std::move(
frame));
300 static std::shared_ptr<metadata_parser_map> create_metadata_parser_map()
302 auto md_parser_map = std::make_shared<metadata_parser_map>();
306 md_parser_map->insert(std::make_pair(frame_md_type, std::make_shared<md_constant_parser>(frame_md_type)));
308 return md_parser_map;
311 static nanoseconds get_file_duration(
const rosbag::Bag& file, uint32_t version)
313 std::function<bool(rosbag::ConnectionInfo const* info)> query;
318 rosbag::View all_frames_view(file, query);
319 auto streaming_duration = all_frames_view.getEndTime() - all_frames_view.getBeginTime();
323 static void get_legacy_frame_metadata(
const rosbag::Bag& bag,
325 const rosbag::MessageInstance &msg,
328 uint32_t total_md_size = 0;
330 assert(frame_metadata_view.size() <= 1);
331 for (
auto message_instance : frame_metadata_view)
333 auto info = instantiate_msg<realsense_legacy_msgs::frame_info>(message_instance);
334 for (
auto&& fmd : info->frame_metadata)
338 additional_data.
system_time = *
reinterpret_cast<const int64_t*
>(fmd.data.data());
350 if (total_md_size + size_of_enum + size_of_data > 255)
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);
357 total_md_size +=
static_cast<uint32_t
>(size_of_data);
365 catch (
const std::exception& e)
367 LOG_WARNING(
"Failed to get timestamp_domain. Error: " << e.what());
372 template <
typename T>
373 static bool safe_convert(
const std::string& key, T& val)
379 catch (
const std::exception& e)
387 static std::map<std::string, std::string> get_frame_metadata(
const rosbag::Bag& bag,
388 const std::string& topic,
390 const rosbag::MessageInstance &msg,
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());
397 for (
auto message_instance : frame_metadata_view)
399 auto key_val_msg = instantiate_msg<diagnostic_msgs::KeyValue>(message_instance);
404 remaining[key_val_msg->key] = key_val_msg->value;
409 if (!safe_convert(key_val_msg->value, additional_data.
system_time))
411 remaining[key_val_msg->key] = key_val_msg->value;
417 if (!safe_convert(key_val_msg->key, type))
419 remaining[key_val_msg->key] = key_val_msg->value;
423 if (!safe_convert(key_val_msg->value, md))
425 remaining[key_val_msg->key] = key_val_msg->value;
430 if (total_md_size + size_of_enum + size_of_data > 255)
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);
444 frame_holder create_image_from_message(
const rosbag::MessageInstance &image_data)
const 446 LOG_DEBUG(
"Trying to create an image frame from message");
447 auto msg = instantiate_msg<sensor_msgs::Image>(image_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();
459 get_legacy_frame_metadata(m_file,
stream_id, image_data, additional_data);
466 get_frame_metadata(m_file, info_topic,
stream_id, image_data, additional_data);
470 msg->data.size(), additional_data,
true);
471 if (
frame ==
nullptr)
477 video_frame->
assign(msg->width, msg->height, msg->step, msg->step / msg->width * 8);
479 convert(msg->encoding, stream_format);
489 return std::move(fh);
492 frame_holder create_motion_sample(
const rosbag::MessageInstance &motion_data)
const 494 LOG_DEBUG(
"Trying to create a motion frame from message");
496 auto msg = instantiate_msg<sensor_msgs::Imu>(motion_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();
509 get_legacy_frame_metadata(m_file,
stream_id, motion_data, additional_data);
516 get_frame_metadata(m_file, info_topic,
stream_id, motion_data, additional_data);
520 if (
frame ==
nullptr)
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);
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);
552 return std::move(fh);
555 static inline float3 to_float3(
const geometry_msgs::Vector3& v)
564 static inline float4 to_float4(
const geometry_msgs::Quaternion& q)
574 frame_holder create_pose_sample(
const rosbag::MessageInstance &msg)
const 576 LOG_DEBUG(
"Trying to create a pose frame from message");
579 std::chrono::duration<double, std::milli> timestamp_ms;
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);
590 timestamp_ms= std::chrono::duration<double, std::milli>(
static_cast<double>(pose_msg->timestamp));
594 assert(msg.getTopic().find(
"pose/transform") != std::string::npos);
596 auto transform_msg = instantiate_msg<geometry_msgs::Transform>(msg);
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());
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());
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);
616 size_t frame_size =
sizeof(
pose);
628 get_legacy_frame_metadata(m_file,
stream_id, msg, additional_data);
635 auto remaining = get_frame_metadata(m_file, info_topic,
stream_id, msg, additional_data);
636 for (
auto&& kvp : remaining)
640 pose.mapper_confidence = std::stoul(kvp.second);
645 std::istringstream iss(kvp.second);
646 iss >> std::hexfloat >> ts;
647 timestamp_ms = std::chrono::duration<double, std::milli>(ts);
651 pose.tracker_confidence = std::stoul(kvp.second);
656 additional_data.
timestamp = timestamp_ms.count();
658 frame_interface* new_frame = m_frame_source->alloc_frame(frame_type, frame_size, additional_data,
true);
659 if (new_frame ==
nullptr)
671 memcpy(data, &
pose, frame_size);
673 LOG_DEBUG(
"Created new frame " << frame_type);
674 return std::move(fh);
677 static uint32_t read_file_version(
const rosbag::Bag& file)
680 rosbag::View view(file, rosbag::TopicQuery(version_topic));
683 rosbag::View legacy_view(file, rosbag::TopicQuery(legacy_version_topic));
684 if(legacy_view.size() == 0 && view.size() == 0)
686 throw io_exception(
to_string() <<
"Invalid file format, file does not contain topic \"" << version_topic <<
"\" nor \"" << legacy_version_topic <<
"\"");
688 assert((view.size() + legacy_view.size()) == 1);
689 if (view.size() != 0)
691 auto item = *view.begin();
692 auto msg = instantiate_msg<std_msgs::UInt32>(item);
695 throw std::runtime_error(
to_string() <<
"Unsupported file version \"" << msg->data <<
"\"");
699 else if (legacy_view.size() != 0)
701 auto item = *legacy_view.begin();
702 auto msg = instantiate_msg<std_msgs::UInt32>(item);
705 throw std::runtime_error(
to_string() <<
"Unsupported legacy file version \"" << msg->data <<
"\"");
709 throw std::logic_error(
"Unreachable code path");
726 rosbag::View extrinsics_view(m_file, rosbag::TopicQuery(topic));
727 if (extrinsics_view.size() == 0)
731 for (
auto&& msg : extrinsics_view)
733 if (msg.isType<realsense_legacy_msgs::motion_stream_info>())
735 auto msi_msg = instantiate_msg<realsense_legacy_msgs::motion_stream_info>(msg);
737 if (
stream_id.stream_type != parsed_stream_id.type ||
stream_id.stream_index != static_cast<uint32_t>(parsed_stream_id.index))
742 std::end(msi_msg->stream_extrinsics.extrinsics.rotation),
745 std::end(msi_msg->stream_extrinsics.extrinsics.translation),
747 group_id =
static_cast<uint32_t
>(msi_msg->stream_extrinsics.reference_point_id);
750 else if (msg.isType<realsense_legacy_msgs::stream_info>())
752 auto si_msg = instantiate_msg<realsense_legacy_msgs::stream_info>(msg);
754 if (
stream_id.stream_type != parsed_stream_id.type ||
stream_id.stream_index != static_cast<uint32_t>(parsed_stream_id.index))
759 std::end(si_msg->stream_extrinsics.extrinsics.rotation),
762 std::end(si_msg->stream_extrinsics.extrinsics.translation),
764 group_id =
static_cast<uint32_t
>(si_msg->stream_extrinsics.reference_point_id);
770 "Expected either \"realsense_legacy_msgs::motion_stream_info\" or \"realsense_legacy_msgs::stream_info\", but got " 771 << msg.getDataType());
780 return try_read_legacy_stream_extrinsic(
stream_id, group_id, extrinsic);
783 if (tf_view.size() == 0)
787 assert(tf_view.size() == 1);
788 auto msg = *tf_view.begin();
789 auto tf_msg = instantiate_msg<geometry_msgs::Transform>(msg);
799 LOG_DEBUG(
"Not updating options from legacy files");
829 std::vector<sensor_snapshot> sensor_descriptions;
831 std::map<stream_identifier, std::pair<uint32_t, rs2_extrinsics>> extrinsics_map;
833 for (
auto sensor_index : sensor_indices)
836 auto streams_snapshots = read_stream_info(
get_device_index(), sensor_index);
840 uint32_t reference_id;
842 if (try_read_stream_extrinsic(
stream_id, reference_id, stream_extrinsic))
844 extrinsics_map[
stream_id] = std::make_pair(reference_id, stream_extrinsic);
849 std::shared_ptr<info_container> sensor_info;
852 sensor_info = read_legacy_info_snapshot(sensor_index);
860 update_sensor_options(m_file, sensor_index, time, m_version, sensor_extensions, m_version);
862 sensor_descriptions.emplace_back(sensor_index, sensor_extensions, streams_snapshots);
865 m_initial_device_description =
device_snapshot(device_extensions, sensor_descriptions, extrinsics_map);
867 return m_initial_device_description;
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);
882 std::shared_ptr<info_container> read_legacy_info_snapshot(uint32_t sensor_index)
const 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>();
889 for (
auto message_instance : view)
891 auto info_msg = instantiate_msg<realsense_legacy_msgs::vendor_data>(message_instance);
897 infos->register_info(info, info_msg->value);
900 catch (
const std::exception& e)
902 std::cerr << e.what() << std::endl;
908 std::shared_ptr<info_container> read_info_snapshot(
const std::string& topic)
const 910 auto infos = std::make_shared<info_container>();
918 std::map<rs2_camera_info, std::string> values;
919 rosbag::View view(m_file, rosbag::TopicQuery(topic));
920 for (
auto message_instance : view)
922 diagnostic_msgs::KeyValueConstPtr info_msg = instantiate_msg<diagnostic_msgs::KeyValue>(message_instance);
927 infos->register_info(info, info_msg->value);
929 catch (
const std::exception& e)
931 std::cerr << e.what() << std::endl;
938 std::set<uint32_t> read_sensor_indices(uint32_t device_index)
const 940 std::set<uint32_t> sensor_indices;
943 rosbag::View
device_info(m_file, rosbag::TopicQuery(
"/info/4294967295"));
946 throw io_exception(
"Missing sensor count message for legacy file");
950 auto msg = instantiate_msg<realsense_legacy_msgs::vendor_data>(info);
951 if (msg->name ==
"sensor_count")
953 int sensor_count = std::stoi(msg->value);
954 while(--sensor_count >= 0)
955 sensor_indices.insert(sensor_count);
962 for (
auto sensor_info : sensor_infos)
964 auto msg = instantiate_msg<diagnostic_msgs::KeyValue>(sensor_info);
968 return sensor_indices;
970 static std::shared_ptr<stream_profile_base> create_pose_profile(uint32_t stream_index, uint32_t fps)
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);
976 profile->set_format(format);
977 profile->set_framerate(fps);
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; });
993 const sensor_msgs::CameraInfo& ci,
996 auto profile = std::make_shared<video_stream_profile>(sp);
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);
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)
1022 if (infos_msg.isType<realsense_legacy_msgs::motion_stream_info>())
1024 auto motion_stream_info_msg = instantiate_msg<realsense_legacy_msgs::motion_stream_info>(infos_msg);
1025 auto fps = motion_stream_info_msg->fps;
1027 std::string stream_name = motion_stream_info_msg->motion_type;
1032 auto profile = create_motion_stream(
stream_id.type,
stream_id.index, fps, format, intrinsics);
1033 streams.push_back(profile);
1035 else if (infos_msg.isType<realsense_legacy_msgs::stream_info>())
1037 auto stream_info_msg = instantiate_msg<realsense_legacy_msgs::stream_info>(infos_msg);
1038 auto fps = stream_info_msg->fps;
1040 convert(stream_info_msg->encoding, format);
1041 std::string stream_name = stream_info_msg->stream_type;
1043 auto profile = create_video_stream_profile(
1045 stream_info_msg->camera_info,
1047 streams.push_back(profile);
1052 <<
"Invalid file format, expected " 1055 <<
" message but got: " << infos_msg.getDataType()
1056 <<
"(Topic: " << infos_msg.getTopic() <<
")");
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))
1063 std::regex r(R
"RRR(/camera/rs_6DoF(\d+)/\d+)RRR"); 1065 if(std::regex_search(topic, sm, r))
1067 for (
int i = 1; i<sm.size(); i++)
1069 indices.push_back(std::stoul(sm[i].str()));
1073 for (
auto&& index : indices)
1075 auto profile = create_pose_profile(index, 0);
1076 streams.push_back(profile);
1081 stream_profiles read_stream_info(uint32_t device_index, uint32_t sensor_index)
const 1085 return read_legacy_stream_info(sensor_index);
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)
1092 if (infos_view.isType<realsense_msgs::StreamInfo>() ==
false)
1098 auto stream_info_msg = instantiate_msg<realsense_msgs::StreamInfo>(infos_view);
1100 auto fps = stream_info_msg->fps;
1102 convert(stream_info_msg->encoding, format);
1105 rosbag::View video_stream_infos_view(m_file, rosbag::TopicQuery(video_stream_topic));
1106 if (video_stream_infos_view.size() > 0)
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(
1113 , *video_stream_msg,
1115 streams.push_back(profile);
1119 rosbag::View imu_intrinsic_view(m_file, rosbag::TopicQuery(imu_stream_topic));
1120 if (imu_intrinsic_view.size() > 0)
1122 assert(imu_intrinsic_view.size() == 1);
1123 auto motion_intrinsics_msg = instantiate_msg<realsense_msgs::ImuIntrinsic>(*imu_intrinsic_view.begin());
1128 auto profile = create_motion_stream(
stream_id.stream_type,
stream_id.stream_index, fps, format, intrinsics);
1129 streams.push_back(profile);
1134 auto profile = create_pose_profile(
stream_id.stream_index, fps);
1135 streams.push_back(profile);
1138 if (video_stream_infos_view.size() == 0 && imu_intrinsic_view.size() == 0 &&
stream_id.stream_type !=
RS2_STREAM_POSE)
1140 throw io_exception(
to_string() <<
"Every StreamInfo is expected to have a complementary video/imu message, but none was found");
1146 static std::string read_option_description(
const rosbag::Bag& file,
const std::string& topic)
1148 rosbag::View option_description_view(file, rosbag::TopicQuery(topic));
1149 if (option_description_view.size() == 0)
1151 LOG_ERROR(
"File does not contain topics for: " << topic);
1154 assert(option_description_view.size() == 1);
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;
1161 static std::pair<rs2_option, std::shared_ptr<librealsense::option>> create_property(
const rosbag::MessageInstance& property_message_instance)
1163 auto property_msg = instantiate_msg<diagnostic_msgs::KeyValue>(property_message_instance);
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));
1172 static std::pair<rs2_option, std::shared_ptr<librealsense::option>> create_option(
const rosbag::Bag& file,
const rosbag::MessageInstance& value_message_instance)
1174 auto option_value_msg = instantiate_msg<std_msgs::Float32>(value_message_instance);
1179 float value = option_value_msg->data;
1181 return std::make_pair(
id, std::make_shared<const_value_option>(description,
value));
1184 static notification create_notification(
const rosbag::Bag& file,
const rosbag::MessageInstance& message_instance)
1186 auto notification_msg = instantiate_msg<realsense_msgs::Notification>(message_instance);
1189 convert(notification_msg->category, category);
1190 convert(notification_msg->severity, severity);
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;
1200 auto options = std::make_shared<options_container>();
1204 for (
auto message_instance : sensor_options_view)
1206 auto id_option = create_property(message_instance);
1207 options->register_option(id_option.first, id_option.second);
1218 auto it = option_view.begin();
1219 if (it == option_view.end())
1223 rosbag::View::iterator last_item;
1224 while (it != option_view.end())
1228 auto option = create_option(file, *last_item);
1229 assert(
id ==
option.first);
1236 static std::vector<std::string> get_topics(std::unique_ptr<rosbag::View>& view)
1238 std::vector<std::string> topics;
1241 auto connections = view->getConnections();
1242 std::transform(connections.begin(), connections.end(), std::back_inserter(topics), [](
const rosbag::ConnectionInfo* connection) {
return connection->topic; });
1249 std::string m_file_path;
1250 std::shared_ptr<frame_source> m_frame_source;
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;
std::shared_ptr< stream_profile_interface > get_stream() const override
Definition: archive.h:107
Definition: rs_option.h:65
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
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:414
int get_height() const
Definition: archive.h:235
Definition: ros_file_format.h:506
rs2_option
Defines general configuration controls. These can generally be mapped to camera UVC controls...
Definition: rs_option.h:22
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: rs_sensor.h:24
float translation[3]
Definition: rs_sensor.h:82
#define LOG_WARNING(...)
Definition: types.h:109
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:72
Definition: rs_option.h:64
std::shared_ptr< serialized_data > read_next_data() override
Definition: ros_reader.h:44
static uint32_t get_sensor_index(const std::string &topic)
Definition: ros_file_format.h:248
Definition: ros_file_format.h:545
frame()
Definition: archive.h:69
sql::statement::iterator end(sql::statement &stmt)
Definition: rs_sensor.h:46
static std::string video_stream_info_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:302
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: serialization.h:22
Definition: rs_types.h:102
constexpr const char * TRACKER_CONFIDENCE_MD_STR
Definition: ros_file_format.h:193
Definition: rs_sensor.h:43
Definition: rs_option.h:52
float rotation[9]
Definition: rs_sensor.h:81
static device_serializer::stream_identifier get_stream_identifier(const std::string &topic)
Definition: ros_file_format.h:272
Definition: ros_reader.h:17
Definition: ros_file_format.h:483
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:107
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
Definition: rs_types.h:112
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
void set_stream(std::shared_ptr< stream_profile_interface > sp) override
Definition: archive.h:108
rs2_time_t system_time
Definition: archive.h:25
virtual void enable_stream(const std::vector< device_serializer::stream_identifier > &stream_ids) override
Definition: ros_reader.h:157
rs2_format
Format identifies how binary data is encoded within a frame.
Definition: rs_sensor.h:52
static std::string file_version_topic()
Definition: ros_file_format.h:286
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
Definition: serialization.h:266
constexpr const char * MAPPER_CONFIDENCE_MD_STR
Definition: ros_file_format.h:191
Definition: rs_sensor.h:70
Definition: ros_file_format.h:519
Definition: rs_sensor.h:39
device_serializer::nanoseconds to_nanoseconds(const ros::Time &t)
Definition: ros_file_format.h:575
rs2_stream
Streams are different types of data provided by RealSense devices.
Definition: rs_sensor.h:36
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:512
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
int index
Definition: types.h:494
Definition: rs_sensor.h:44
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: 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
void assign(int width, int height, int stride, int bpp)
Definition: archive.h:239
Definition: ros_file_format.h:529
Definition: ros_file_format.h:457
constexpr device_serializer::nanoseconds get_static_file_info_timestamp()
Definition: ros_file_format.h:570
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
constexpr uint32_t get_minimum_supported_file_version()
Definition: ros_file_format.h:560
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
unsigned long long frame_number
Definition: archive.h:23
std::vector< std::shared_ptr< stream_profile_interface > > stream_profiles
Definition: streaming.h:104
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:538
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: serialization.h:17
Definition: rs_types.h:115
rs2_log_severity
Severity of the librealsense logger.
Definition: rs_types.h:81
Definition: serialization.h:190
ros::Time to_rostime(const device_serializer::nanoseconds &t)
Definition: ros_file_format.h:583
float bias_variances[3]
Definition: rs_types.h:77
std::vector< std::shared_ptr< serialized_data > > fetch_last_frames(const nanoseconds &seek_time) override
Definition: ros_reader.h:111
rs2_frame_metadata_value
Per-Frame-Metadata are set of read-only properties that might be exposed for each individual frame...
Definition: rs_frame.h:28
rs2_stream type
Definition: types.h:493
#define LOG_ERROR(...)
Definition: types.h:110
void copy(void *dst, void const *src, size_t size)
Definition: serialization.h:336
constexpr uint32_t get_device_index()
Definition: ros_file_format.h:565
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