8 #include "sensor_msgs/image_encodings.h" 9 #include "sensor_msgs/Imu.h" 10 #include "sensor_msgs/Image.h" 11 #include "diagnostic_msgs/KeyValue.h" 12 #include "std_msgs/UInt32.h" 13 #include "std_msgs/Float32.h" 14 #include "std_msgs/String.h" 15 #include "realsense_msgs/StreamInfo.h" 16 #include "realsense_msgs/ImuIntrinsic.h" 17 #include "realsense_msgs/Notification.h" 18 #include "realsense_legacy_msgs/legacy_headers.h" 19 #include "sensor_msgs/CameraInfo.h" 20 #include "geometry_msgs/Transform.h" 21 #include "geometry_msgs/Twist.h" 22 #include "geometry_msgs/Accel.h" 25 #include "rosbag/structures.h" 37 case RS2_FORMAT_Z16: target = sensor_msgs::image_encodings::MONO16;
return;
38 case RS2_FORMAT_RGB8: target = sensor_msgs::image_encodings::RGB8;
return;
39 case RS2_FORMAT_BGR8: target = sensor_msgs::image_encodings::BGR8;
return;
42 case RS2_FORMAT_Y8: target = sensor_msgs::image_encodings::TYPE_8UC1;
return;
43 case RS2_FORMAT_Y16: target = sensor_msgs::image_encodings::TYPE_16UC1;
return;
44 case RS2_FORMAT_RAW8: target = sensor_msgs::image_encodings::MONO8;
return;
45 case RS2_FORMAT_UYVY: target = sensor_msgs::image_encodings::YUV422;
return;
52 if (source == sensor_msgs::image_encodings::MONO16) { target =
RS2_FORMAT_Z16;
return; }
53 if (source == sensor_msgs::image_encodings::RGB8) { target =
RS2_FORMAT_RGB8;
return; }
54 if (source == sensor_msgs::image_encodings::BGR8) { target =
RS2_FORMAT_BGR8;
return; }
55 if (source == sensor_msgs::image_encodings::RGBA8) { target =
RS2_FORMAT_RGBA8;
return; }
56 if (source == sensor_msgs::image_encodings::BGRA8) { target =
RS2_FORMAT_BGRA8;
return; }
57 if (source == sensor_msgs::image_encodings::TYPE_8UC1) { target =
RS2_FORMAT_Y8;
return; }
58 if (source == sensor_msgs::image_encodings::TYPE_16UC1) { target =
RS2_FORMAT_Y16;
return; }
59 if (source == sensor_msgs::image_encodings::MONO8) { target =
RS2_FORMAT_RAW8;
return; }
60 if (source == sensor_msgs::image_encodings::YUV422) { target =
RS2_FORMAT_UYVY;
return; }
61 if (!try_parse(source, target))
63 throw std::runtime_error(
to_string() <<
"Failed to convert source: \"" <<
"\" to matching rs2_format");
69 if(!try_parse(source, target))
71 throw std::runtime_error(
to_string() <<
"Failed to convert source: \"" <<
"\" to matching rs2_stream");
77 if (!try_parse(source, target))
79 throw std::runtime_error(
to_string() <<
"Failed to convert source: \"" <<
"\" to matching rs2_distortion");
85 if (!try_parse(source, target))
87 throw std::runtime_error(
to_string() <<
"Failed to convert source: \"" <<
"\" to matching rs2_optin");
93 if (!try_parse(source, target))
95 throw std::runtime_error(
to_string() <<
"Failed to convert source: \"" <<
"\" to matching rs2_frame_metadata");
101 if (!try_parse(source, target))
103 throw std::runtime_error(
to_string() <<
"Failed to convert source: \"" <<
"\" to matching rs2_camera_info");
109 if (!try_parse(source, target))
111 throw std::runtime_error(
to_string() <<
"Failed to convert source: \"" <<
"\" to matching rs2_timestamp_domain");
117 if (!try_parse(source, target))
119 throw std::runtime_error(
to_string() <<
"Failed to convert source: \"" <<
"\" to matching rs2_notification_category");
125 if (!try_parse(source, target))
127 throw std::runtime_error(
to_string() <<
"Failed to convert source: \"" <<
"\" to matching rs2_log_severity");
131 inline void convert(
const std::string& source,
double& target)
133 target = std::stod(source);
136 inline void convert(
const std::string& source,
long long& target)
138 target = std::stoll(source);
152 inline void quat2rot(
const geometry_msgs::Transform::_rotation_type& q,
float(&r)[9])
154 r[0] = 1 - 2 * q.y*q.y - 2 * q.z*q.z;
155 r[3] = 2 * q.x*q.y - 2 * q.z*q.w;
156 r[6] = 2 * q.x*q.z + 2 * q.y*q.w;
157 r[1] = 2 * q.x*q.y + 2 * q.z*q.w;
158 r[4] = 1 - 2 * q.x*q.x - 2 * q.z*q.z;
159 r[7] = 2 * q.y*q.z - 2 * q.x*q.w;
160 r[2] = 2 * q.x*q.z - 2 * q.y*q.w;
161 r[5] = 2 * q.y*q.z + 2 * q.x*q.w;
162 r[8] = 1 - 2 * q.x*q.x - 2 * q.y*q.y;
165 inline void rot2quat(
const float(&r)[9], geometry_msgs::Transform::_rotation_type& q)
167 q.w = sqrtf(1 + r[0] + r[4] + r[8]) / 2;
168 q.x = (r[5] - r[7]) / (4 * q.w);
169 q.y = (r[6] - r[2]) / (4 * q.w);
170 q.z = (r[1] - r[3]) / (4 * q.w);
202 if (try_get(frm, v) ==
false)
211 return try_get(frm, v);
245 return get_id(
"device_", get<1>(topic));
250 return get_id(
"sensor_", get<2>(topic));
255 auto stream_with_id = get<3>(topic);
257 convert(stream_with_id.substr(0, stream_with_id.find_first_of(
"_")), type);
263 auto stream_with_id = get<3>(topic);
264 return get_id(stream_with_id.substr(0, stream_with_id.find_first_of(
"_") + 1), get<3>(topic));
279 return std::stoul(get<5>(topic));
284 return get<4>(topic);
288 return create_from({
"file_version" });
292 return create_from({ device_prefix(device_id),
"info" });
363 return create_from({
"additional_info" });
376 template<u
int32_t index>
377 static std::string
get(
const std::string&
value)
379 size_t current_pos = 0;
380 std::string value_copy =
value;
381 uint32_t elements_iterator = 0;
385 auto token = value_copy.substr(0, current_pos);
386 if (elements_iterator == index)
390 value_copy.erase(0, current_pos + seperator_length);
394 if (elements_iterator == index)
397 throw std::out_of_range(
to_string() <<
"Requeted index \"" << index <<
"\" is out of bound of topic: \"" <<
value <<
"\"");
400 static std::string stream_to_ros_type(
rs2_stream type)
419 static std::string create_from(
const std::vector<std::string>& parts)
421 std::ostringstream oss;
423 if (parts.empty() ==
false)
432 static uint32_t get_id(
const std::string& prefix,
const std::string& str)
434 if (str.compare(0, prefix.size(), prefix) != 0)
436 throw std::runtime_error(
"Failed to get id after prefix \"" + prefix +
"\"from string \"" + str +
"\"");
439 std::string id_str = str.substr(prefix.size());
440 return static_cast<uint32_t
>(std::stoll(id_str));
443 static std::string device_prefix(uint32_t device_id)
445 return "device_" + std::to_string(device_id);
447 static std::string sensor_prefix(uint32_t sensor_id)
449 return "sensor_" + std::to_string(sensor_id);
459 bool operator()(rosbag::ConnectionInfo
const* info)
const {
return false; }
467 for (
auto&& regexp : regexps)
469 LOG_DEBUG(
"RegexTopicQuery with expression: " << regexp);
470 _exps.emplace_back(regexp);
476 return std::any_of(
std::begin(_exps),
std::end(_exps), [info](
const std::regex& exp) {
return std::regex_search(info->topic, exp); });
480 std::vector<std::regex> _exps;
572 return device_serializer::nanoseconds::min();
577 if (t == ros::TIME_MIN)
586 return ros::TIME_MIN;
588 auto secs = std::chrono::duration_cast<std::chrono::duration<double>>(t);
589 return ros::Time(secs.count());
592 namespace legacy_file_format
594 constexpr
const char*
USB_DESCRIPTOR =
"{ 0x94b5fb99, 0x79f2, 0x4d66,{ 0x85, 0x06, 0xb1, 0x5e, 0x8b, 0x8c, 0x9d, 0xa1 } }";
595 constexpr
const char*
DEVICE_INTERFACE_VERSION =
"{ 0xafcd9c11, 0x52e3, 0x4258,{ 0x8d, 0x23, 0xbe, 0x86, 0xfa, 0x97, 0xa0, 0xab } }";
596 constexpr
const char*
FW_VERSION =
"{ 0x7b79605b, 0x5e36, 0x4abe,{ 0xb1, 0x01, 0x94, 0x86, 0xb8, 0x9a, 0xfe, 0xbe } }";
597 constexpr
const char*
CENTRAL_VERSION =
"{ 0x5652ffdb, 0xacac, 0x47ea,{ 0xbf, 0x65, 0x73, 0x3e, 0xf3, 0xd9, 0xe2, 0x70 } }";
598 constexpr
const char*
CENTRAL_PROTOCOL_VERSION =
"{ 0x50376dea, 0x89f4, 0x4d70,{ 0xb1, 0xb0, 0x05, 0xf6, 0x07, 0xb6, 0xae, 0x8a } }";
599 constexpr
const char*
EEPROM_VERSION =
"{ 0x4617d177, 0xb546, 0x4747,{ 0x9d, 0xbf, 0x4f, 0xf8, 0x99, 0x0c, 0x45, 0x6b } }";
600 constexpr
const char*
ROM_VERSION =
"{ 0x16a64010, 0xfee4, 0x4c67,{ 0xae, 0xc5, 0xa0, 0x4d, 0xff, 0x06, 0xeb, 0x0b } }";
601 constexpr
const char*
TM_DEVICE_TYPE =
"{ 0x1212e1d5, 0xfa3e, 0x4273,{ 0x9e, 0xbf, 0xe4, 0x43, 0x87, 0xbe, 0xe5, 0xe8 } }";
602 constexpr
const char*
HW_VERSION =
"{ 0x4439fcca, 0x8673, 0x4851,{ 0x9b, 0xb6, 0x1a, 0xab, 0xbd, 0x74, 0xbd, 0xdc } }";
603 constexpr
const char*
STATUS =
"{ 0x5d6c6637, 0x28c7, 0x4a90,{ 0xab, 0x35, 0x90, 0xb2, 0x1f, 0x1a, 0xe6, 0xb8 } }";
604 constexpr
const char*
STATUS_CODE =
"{ 0xe22a94a6, 0xed64, 0x46ea,{ 0x81, 0x52, 0x6a, 0xb3, 0x0b, 0x0e, 0x2a, 0x18 } }";
605 constexpr
const char*
EXTENDED_STATUS =
"{ 0xff6e50db, 0x3c5f, 0x43e7,{ 0xb4, 0x82, 0xb8, 0xc3, 0xa6, 0x8e, 0x78, 0xcd } }";
606 constexpr
const char*
SERIAL_NUMBER =
"{ 0x7d3e44e7, 0x8970, 0x4a32,{ 0x8e, 0xee, 0xe8, 0xd1, 0xd1, 0x32, 0xa3, 0x22 } }";
607 constexpr
const char*
TIMESTAMP_SORT_TYPE =
"{ 0xb409b217, 0xe5cd, 0x4a04,{ 0x9e, 0x85, 0x1a, 0x7d, 0x59, 0xd7, 0xe5, 0x61 } }";
609 constexpr
const char*
DEPTH =
"DEPTH";
610 constexpr
const char*
COLOR =
"COLOR";
613 constexpr
const char*
ACCEL =
"ACCLEROMETER";
614 constexpr
const char*
GYRO =
"GYROMETER";
615 constexpr
const char*
POSE =
"rs_6DoF";
666 throw std::runtime_error(
to_string() <<
"Unknown timestamp domain: " << source);
671 const size_t number_of_hexadecimal_values_in_a_guid = 11;
672 const std::string left_group = R
"RE(\s*(0x[0-9a-fA-F]{1,8})\s*,\s*(0x[0-9a-fA-F]{1,4})\s*,\s*(0x[0-9a-fA-F]{1,4})\s*,\s*)RE"; 673 const std::string right_group = R
"RE(\s*(0x[0-9a-fA-F]{1,2})\s*,\s*(0x[0-9a-fA-F]{1,2})\s*,\s*(0x[0-9a-fA-F]{1,2})\s*,\s*(0x[0-9a-fA-F]{1,2})\s*,\s*(0x[0-9a-fA-F]{1,2})\s*,\s*(0x[0-9a-fA-F]{1,2})\s*,\s*(0x[0-9a-fA-F]{1,2})\s*,\s*(0x[0-9a-fA-F]{1,2})\s*)RE"; 674 const std::string guid_regex_pattern = R
"RE(\{)RE" + left_group + R"RE(\{)RE" + right_group + R"RE(\}\s*\})RE"; 677 std::regex reg(guid_regex_pattern, std::regex_constants::icase);
678 const std::map<rs2_camera_info, const char*> legacy_infos{
683 for (
auto&& s : legacy_infos)
685 if (std::regex_match(s.second, reg))
723 return name + std::to_string(source.
index);
727 if (source.
index == 1)
731 return name + (source.
index == 0 ?
"" : std::to_string(source.
index));
738 auto starts_with = [source](
const std::string& s) {
return source.find(s) == 0; };
739 std::string type_str;
740 if (starts_with(
DEPTH))
745 else if (starts_with(
COLOR))
760 else if (starts_with(
ACCEL))
765 else if (starts_with(
GYRO))
770 else if (starts_with(
POSE))
773 auto index_str = source.substr(std::string(
POSE).length());
774 retval.index = std::stoi(index_str);
780 auto index_str = source.substr(type_str.length());
781 if (index_str.empty())
788 int index = std::stoi(index_str);
790 retval.index = index;
804 to_string() << R
"RRR(/(camera|imu)/.*/(image|imu)_raw/\d+)RRR" , 805 to_string() << R"RRR(/camera/rs_6DoF\d+/\d+)RRR" }) {} 838 <<
"/rs_frame_info_ext/" <<
stream_id.sensor_index)
845 uint32_t sensor_index;
847 sensor_index =
static_cast<uint32_t
>(std::stoll(ros_topic::get<3>(topic)));
849 sensor_index =
static_cast<uint32_t
>(std::stoll(ros_topic::get<4>(topic)));
856 return "/FILE_VERSION";
rs2_camera_info
Read-only strings that can be queried from the device. Not all information attributes are available o...
Definition: rs_sensor.h:22
constexpr uint32_t get_file_version()
Definition: ros_file_format.h:555
static std::string frame_data_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:346
const char * rs2_format_to_string(rs2_format format)
Definition: rs_sensor.h:60
Definition: rs_frame.h:33
void rot2quat(const float(&r)[9], geometry_msgs::Transform::_rotation_type &q)
Definition: ros_file_format.h:165
Definition: rs_frame.h:32
Definition: ros_file_format.h:195
Definition: rs_sensor.h:26
Base class that establishes the interface for retrieving metadata attributes.
Definition: metadata-parser.h:28
static std::string sensor_info_topic(const device_serializer::sensor_identifier &sensor_id)
Definition: ros_file_format.h:294
OptionsQuery()
Definition: ros_file_format.h:541
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
static std::string get_option_name(const std::string &topic)
Definition: ros_file_format.h:282
Definition: rs_frame.h:21
rs2_distortion
Distortion model: defines how pixel coordinates should be mapped to sensor coordinates.
Definition: rs_types.h:43
Definition: rs_sensor.h:24
float translation[3]
Definition: rs_sensor.h:82
const char * rs2_option_to_string(rs2_option option)
constexpr const char * FRAME_TIMESTAMP_MD_STR
Definition: ros_file_format.h:192
uint32_t sensor_index
Definition: serialization.h:20
bool operator()(rosbag::ConnectionInfo const *info) const
Definition: ros_file_format.h:474
static std::string stream_extrinsic_topic(const device_serializer::stream_identifier &stream_id, uint32_t ref_id)
Definition: ros_file_format.h:356
static uint32_t get_device_index(const std::string &topic)
Definition: ros_file_format.h:243
FrameQuery()
Definition: ros_file_format.h:516
bool operator()(rosbag::ConnectionInfo const *info) const
Definition: ros_file_format.h:459
Definition: rs_sensor.h:42
Definition: rs_sensor.h:55
static uint32_t get_sensor_index(const std::string &topic)
Definition: ros_file_format.h:248
Definition: ros_file_format.h:545
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
void convert(rs2_format source, std::string &target)
Definition: ros_file_format.h:33
static std::string additional_info_topic()
Definition: ros_file_format.h:361
static std::string device_info_topic(uint32_t device_id)
Definition: ros_file_format.h:290
RegexTopicQuery(std::string const ®exp)
Definition: ros_file_format.h:486
Definition: serialization.h:22
constexpr const char * TRACKER_CONFIDENCE_MD_STR
Definition: ros_file_format.h:193
Definition: rs_sensor.h:43
Definition: ros_file_format.h:235
Exposes librealsense functionality for C compilers.
float rotation[9]
Definition: rs_sensor.h:81
Definition: rs_sensor.h:40
frame_additional_data additional_data
Definition: archive.h:67
static device_serializer::stream_identifier get_stream_identifier(const std::string &topic)
Definition: ros_file_format.h:272
Definition: rs_sensor.h:59
Definition: ros_file_format.h:462
Definition: ros_file_format.h:483
Definition: rs_frame.h:36
uint32_t device_index
Definition: serialization.h:19
#define LOG_DEBUG(...)
Definition: types.h:107
Definition: rs_frame.h:30
static device_serializer::sensor_identifier get_sensor_identifier(const std::string &topic)
Definition: ros_file_format.h:267
sql::statement::iterator begin(sql::statement &stmt)
static constexpr const char * ros_pose_type_str()
Definition: ros_file_format.h:241
static std::string pose_accel_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:335
static std::string notification_topic(const device_serializer::sensor_identifier &sensor_id, rs2_notification_category nc)
Definition: ros_file_format.h:371
Definition: rs_frame.h:37
NotificationsQuery()
Definition: ros_file_format.h:548
rs2_format
Format identifies how binary data is encoded within a frame.
Definition: rs_sensor.h:52
static std::string stream_prefix(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:495
static std::string file_version_topic()
Definition: ros_file_format.h:286
Definition: rs_sensor.h:25
void quat2rot(const geometry_msgs::Transform::_rotation_type &q, float(&r)[9])
Definition: ros_file_format.h:152
static constexpr const char * ros_imu_type_str()
Definition: ros_file_format.h:240
static std::string property_topic(const device_serializer::sensor_identifier &sensor_id)
Definition: ros_file_format.h:312
Definition: rs_sensor.h:68
constexpr const char * SYSTEM_TIME_MD_STR
Definition: ros_file_format.h:190
Definition: rs_frame.h:34
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: 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
constexpr const char * TIMESTAMP_DOMAIN_MD_STR
Definition: ros_file_format.h:189
std::chrono::duration< uint64_t, std::nano > nanoseconds
Definition: serialization.h:46
ExtrinsicsQuery(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:532
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
static std::string frame_metadata_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:351
Definition: rs_frame.h:31
Definition: rs_sensor.h:41
int index
Definition: types.h:494
Definition: rs_sensor.h:44
Cross-stream extrinsics: encode the topology describing how the different devices are connected...
Definition: rs_sensor.h:79
static constexpr const char * ros_image_type_str()
Definition: ros_file_format.h:239
std::string get_string(perc::Status value)
Definition: controller_event_serializer.h:26
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 uint32_t get_stream_index(const std::string &topic)
Definition: ros_file_format.h:261
static std::string option_value_topic(const device_serializer::sensor_identifier &sensor_id, rs2_option option_type)
Definition: ros_file_format.h:318
const char * rs2_stream_to_string(rs2_stream stream)
rs2_notification_category
Category of the librealsense notifications.
Definition: rs_types.h:17
int stream_id
Definition: sync.h:17
bool supports(const frame &frm) const override
Definition: ros_file_format.h:208
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
static rs2_stream get_stream_type(const std::string &topic)
Definition: ros_file_format.h:253
static std::string option_description_topic(const device_serializer::sensor_identifier &sensor_id, rs2_option option_type)
Definition: ros_file_format.h:324
Definition: ros_file_format.h:538
md_constant_parser(rs2_frame_metadata_value type)
Definition: ros_file_format.h:198
Definition: rs_sensor.h:62
MultipleRegexTopicQuery(const std::vector< std::string > ®exps)
Definition: ros_file_format.h:465
StreamQuery(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:522
std::array< uint8_t, MAX_META_DATA_SIZE > metadata_blob
Definition: archive.h:29
static constexpr const char * elements_separator()
Definition: ros_file_format.h:238
static std::string pose_transform_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:329
Definition: serialization.h:17
const char * rs2_notification_category_to_string(rs2_notification_category category)
static std::string data_msg_types()
Definition: ros_file_format.h:490
rs2_log_severity
Severity of the librealsense logger.
Definition: rs_types.h:81
Definition: rs_sensor.h:64
ros::Time to_rostime(const device_serializer::nanoseconds &t)
Definition: ros_file_format.h:583
SensorInfoQuery(uint32_t device_index)
Definition: ros_file_format.h:509
Definition: rs_sensor.h:67
Definition: rs_frame.h:38
Definition: rs_frame.h:22
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
Definition: rs_sensor.h:61
Definition: rs_sensor.h:63
void copy(void *dst, void const *src, size_t size)
constexpr uint32_t get_device_index()
Definition: ros_file_format.h:565
static std::string stream_info_topic(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:298
static std::string stream_full_prefix(const device_serializer::stream_identifier &stream_id)
Definition: ros_file_format.h:366
rs2_timestamp_domain
Specifies the clock in relation to which the frame timestamp was measured.
Definition: rs_frame.h:19