Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adding depth units value to frame metadata #9154

Merged
merged 16 commits into from
Jun 21, 2021
4 changes: 3 additions & 1 deletion src/ds5/ds5-device.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -377,7 +377,9 @@ namespace librealsense
{
_metadata_modifier = callback;
auto s = get_raw_sensor().get();
As< librealsense::uvc_sensor >(s)->set_frame_metadata_modifier(callback);
auto uvc = As< librealsense::uvc_sensor >(s);
if(uvc)
uvc->set_frame_metadata_modifier(callback);
}

void open(const stream_profiles& requests) override
Expand Down
4 changes: 2 additions & 2 deletions src/media/ros/ros_writer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -188,7 +188,7 @@ namespace librealsense
image.header.seq = static_cast<uint32_t>(vid_frame->get_frame_number());
std::chrono::duration<double, std::milli> timestamp_ms(vid_frame->get_frame_timestamp());
image.header.stamp = rs2rosinternal::Time(std::chrono::duration<double>(timestamp_ms).count());
image.header.version = "1"; // used to distinguish between old rosbag and new rosbag that contains depth units in frame metadata
image.header.version = "1"; // the field is unused and therefore assigned for ROSbag versions control
auto df = dynamic_cast<librealsense::depth_frame*>(frame.frame);
if(df)
image.depth_units = df->get_units();
Expand All @@ -208,7 +208,7 @@ namespace librealsense
imu_msg.header.seq = static_cast<uint32_t>(frame.frame->get_frame_number());
std::chrono::duration<double, std::milli> timestamp_ms(frame.frame->get_frame_timestamp());
imu_msg.header.stamp = rs2rosinternal::Time(std::chrono::duration<double>(timestamp_ms).count());
imu_msg.header.version = "1"; // used to distinguish between old rosbag and new rosbag that contains depth units in frame metadata
imu_msg.header.version = "1"; // the field is unused and therefore assigned for ROSbag versions control
auto data_ptr = reinterpret_cast<const float*>(frame.frame->get_frame_data());
if (stream_id.stream_type == RS2_STREAM_ACCEL)
{
Expand Down
2 changes: 1 addition & 1 deletion third-party/realsense-file/rosbag/msgs/sensor_msgs/Image.h
Original file line number Diff line number Diff line change
Expand Up @@ -281,7 +281,7 @@ struct Printer< ::sensor_msgs::Image_<ContainerAllocator> >
s << indent << " data[" << i << "]: ";
Printer<uint8_t>::stream(s, indent + " ", v.data[i]);
}
if (v.depth_units)
if (v.depth_units != 0)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Use std::numeric_limits<float>::min() as in original comment

{
s << indent << "depth_units: ";
Printer<float>::stream(s, indent + " ", v.depth_units);
Expand Down
2 changes: 1 addition & 1 deletion third-party/realsense-file/rosbag/msgs/std_msgs/Header.h
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ struct Header_
_stamp_type stamp;

typedef std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > _frame_id_type;
_frame_id_type version;
_frame_id_type version; // the field is used for ROSbag versions control



Expand Down