-
Notifications
You must be signed in to change notification settings - Fork 4.8k
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
Changes from 5 commits
80fbbee
947f067
a1d0aa1
57bc1e5
f298178
5d320d9
f9f314c
cdee6b0
1217402
50c1099
ad74523
5609b49
b693574
fde3cd6
0065af3
208b390
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -190,7 +190,7 @@ namespace librealsense | |
image.header.stamp = rs2rosinternal::Time(std::chrono::duration<double>(timestamp_ms).count()); | ||
std::string TODO_CORRECT_ME = "0"; | ||
image.header.frame_id = TODO_CORRECT_ME; | ||
image.header.depth_units = static_cast<float>(((depth_frame*)vid_frame)->get_units()); | ||
image.depth_units = static_cast<float>(((depth_frame*)vid_frame)->get_units()); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Let's review the record/playback mechanims There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Must be dynamically checked |
||
auto image_topic = ros_topic::frame_data_topic(stream_id); | ||
write_message(image_topic, timestamp, image); | ||
write_additional_frame_messages(stream_id, timestamp, frame); | ||
|
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -72,6 +72,19 @@ namespace librealsense | |
{ | ||
if (should_process(f)) | ||
{ | ||
// For old playback sensors | ||
if (!((depth_frame*)f.get())->get_units()) | ||
{ | ||
auto snr = ((frame_interface*)f.get())->get_sensor().get(); | ||
auto depth_sensor = As< librealsense::depth_sensor >(snr); | ||
auto extendable = As< librealsense::extendable_interface >(snr); | ||
if (extendable && extendable->extend_to(TypeToExtension< librealsense::depth_sensor >::value, (void**)(&depth_sensor))) | ||
{ | ||
auto du = depth_sensor->get_depth_scale(); | ||
((depth_frame*)f.get())->set_units(du); | ||
} | ||
} | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. The DU injection should occur during the frame construction in |
||
|
||
auto res = process_frame(source, f); | ||
if (!res) continue; | ||
if (auto composite = res.as<rs2::frameset>()) | ||
|
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -31,6 +31,7 @@ namespace librealsense | |
_is_opened(false), | ||
_notifications_processor(std::shared_ptr<notifications_processor>(new notifications_processor())), | ||
_on_open(nullptr), | ||
_on_frame(nullptr), | ||
_metadata_parsers(std::make_shared<metadata_parser_map>()), | ||
_owner(dev), | ||
_profiles([this]() { | ||
|
@@ -241,8 +242,7 @@ namespace librealsense | |
frame_timestamp_reader* timestamp_reader, | ||
const rs2_time_t& last_timestamp, | ||
const unsigned long long& last_frame_number, | ||
std::shared_ptr<stream_profile_interface> profile, | ||
float depth_units = -1) | ||
std::shared_ptr<stream_profile_interface> profile) | ||
{ | ||
auto system_time = environment::get_instance().get_time_service()->get_time(); | ||
auto fr = std::make_shared<frame>(); | ||
|
@@ -251,7 +251,9 @@ namespace librealsense | |
fr->data = pixels; | ||
fr->set_stream(profile); | ||
|
||
// generate additional data | ||
float depth_units = 0; | ||
if(_on_frame) | ||
_on_frame(depth_units);//_additional_data.depth_units; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. The callback should get the |
||
frame_additional_data additional_data(0, | ||
0, | ||
system_time, | ||
|
@@ -320,7 +322,7 @@ namespace librealsense | |
[this, req_profile_base, req_profile, last_frame_number, last_timestamp](platform::stream_profile p, platform::frame_object f, std::function<void()> continuation) mutable | ||
{ | ||
const auto&& system_time = environment::get_instance().get_time_service()->get_time(); | ||
const auto&& fr = generate_frame_from_data(f, _timestamp_reader.get(), last_timestamp, last_frame_number, req_profile_base, _depth_units); | ||
const auto&& fr = generate_frame_from_data(f, _timestamp_reader.get(), last_timestamp, last_frame_number, req_profile_base); | ||
const auto&& requires_processing = true; // TODO - Ariel add option | ||
const auto&& timestamp_domain = _timestamp_reader->get_frame_timestamp_domain(fr); | ||
const auto&& bpp = get_image_bpp(req_profile_base->get_format()); | ||
|
@@ -648,11 +650,6 @@ namespace librealsense | |
register_option(id, std::make_shared<uvc_pu_option>(*this, id)); | ||
} | ||
|
||
void uvc_sensor::set_depth_units(float value) // TODO for refactoring | ||
{ | ||
_depth_units = value; | ||
} | ||
|
||
////////////////////////////////////////////////////// | ||
/////////////////// HID Sensor /////////////////////// | ||
////////////////////////////////////////////////////// | ||
|
@@ -942,7 +939,6 @@ namespace librealsense | |
: sensor_base(name, dev, (recommended_proccesing_blocks_interface*)this), | ||
_device(move(uvc_device)), | ||
_user_count(0), | ||
_depth_units(-1), | ||
_timestamp_reader(std::move(timestamp_reader)) | ||
{ | ||
register_metadata(RS2_FRAME_METADATA_BACKEND_TIMESTAMP, make_additional_data_parser(&frame_additional_data::backend_timestamp)); | ||
|
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -30,6 +30,7 @@ namespace librealsense | |
class option; | ||
|
||
typedef std::function<void(std::vector<platform::stream_profile>)> on_open; | ||
typedef std::function<void(float &val)> on_frame_md; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. The function should receive a reference to |
||
|
||
struct frame_timestamp_reader | ||
{ | ||
|
@@ -68,6 +69,7 @@ namespace librealsense | |
{ | ||
_on_open = callback; | ||
} | ||
virtual void update_params(on_frame_md callback) {} | ||
device_interface& get_device() override; | ||
|
||
// Make sensor inherit its owning device info by default | ||
|
@@ -98,15 +100,15 @@ namespace librealsense | |
frame_timestamp_reader* timestamp_reader, | ||
const rs2_time_t& last_timestamp, | ||
const unsigned long long& last_frame_number, | ||
std::shared_ptr<stream_profile_interface> profile, | ||
float depth_units); | ||
std::shared_ptr<stream_profile_interface> profile); | ||
|
||
std::vector<platform::stream_profile> _internal_config; | ||
|
||
std::atomic<bool> _is_streaming; | ||
std::atomic<bool> _is_opened; | ||
std::shared_ptr<notifications_processor> _notifications_processor; | ||
on_open _on_open; | ||
on_frame_md _on_frame; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Rename to |
||
std::shared_ptr<metadata_parser_map> _metadata_parsers = nullptr; | ||
|
||
sensor_base* _source_owner = nullptr; | ||
|
@@ -338,7 +340,6 @@ namespace librealsense | |
void stop() override; | ||
void register_xu(platform::extension_unit xu); | ||
void register_pu(rs2_option id); | ||
void set_depth_units(float value); | ||
|
||
std::vector<platform::stream_profile> get_configuration() const { return _internal_config; } | ||
std::shared_ptr<platform::uvc_device> get_uvc_device() { return _device; } | ||
|
@@ -353,6 +354,10 @@ namespace librealsense | |
return action(*_device); | ||
} | ||
|
||
void update_params(on_frame_md callback) override | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Probably should be called |
||
{ | ||
_on_frame = callback; | ||
} | ||
protected: | ||
stream_profiles init_stream_profiles() override; | ||
rs2_extension stream_to_frame_types(rs2_stream stream) const; | ||
|
@@ -396,7 +401,6 @@ namespace librealsense | |
std::vector<platform::extension_unit> _xus; | ||
std::unique_ptr<power> _power; | ||
std::unique_ptr<frame_timestamp_reader> _timestamp_reader; | ||
mutable std::atomic<float> _depth_units; | ||
}; | ||
|
||
processing_blocks get_color_recommended_proccesing_blocks(); | ||
|
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -25,24 +25,26 @@ struct Image_ | |
typedef Image_<ContainerAllocator> Type; | ||
|
||
Image_() | ||
: header() | ||
, height(0) | ||
, width(0) | ||
, encoding() | ||
, is_bigendian(0) | ||
, step(0) | ||
, data() { | ||
} | ||
: header() | ||
, height(0) | ||
, width(0) | ||
, encoding() | ||
, is_bigendian(0) | ||
, step(0) | ||
, depth_units(0) | ||
, data() { | ||
} | ||
Image_(const ContainerAllocator& _alloc) | ||
: header(_alloc) | ||
, height(0) | ||
, width(0) | ||
, encoding(_alloc) | ||
, is_bigendian(0) | ||
, step(0) | ||
, data(_alloc) { | ||
(void)_alloc; | ||
} | ||
: header(_alloc) | ||
, height(0) | ||
, width(0) | ||
, encoding(_alloc) | ||
, is_bigendian(0) | ||
, step(0) | ||
, depth_units(0) | ||
, data(_alloc) { | ||
(void)_alloc; | ||
} | ||
|
||
|
||
|
||
|
@@ -67,6 +69,8 @@ struct Image_ | |
typedef std::vector<uint8_t, typename ContainerAllocator::template rebind<uint8_t>::other > _data_type; | ||
_data_type data; | ||
|
||
typedef float _depth_units_type; | ||
_depth_units_type depth_units; | ||
|
||
|
||
|
||
|
@@ -238,6 +242,14 @@ namespace serialization | |
stream.next(m.is_bigendian); | ||
stream.next(m.step); | ||
stream.next(m.data); | ||
try | ||
{ | ||
stream.next(m.depth_units); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. There is a header that should be used to differentiate according to version number |
||
} | ||
catch (...) | ||
{ | ||
LOG_WARNING("Old rosbag file doesn't contain Depth Units for each frame!"); | ||
} | ||
} | ||
|
||
ROS_DECLARE_ALLINONE_SERIALIZER | ||
|
@@ -275,6 +287,8 @@ struct Printer< ::sensor_msgs::Image_<ContainerAllocator> > | |
s << indent << " data[" << i << "]: "; | ||
Printer<uint8_t>::stream(s, indent + " ", v.data[i]); | ||
} | ||
s << indent << "depth_units: "; | ||
Printer<float>::stream(s, indent + " ", v.depth_units); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Print only for depth frame |
||
} | ||
}; | ||
|
||
|
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -156,11 +156,11 @@ namespace rosbag_inspector | |
os << " frame_id : " << image->header.frame_id << std::endl; | ||
os << " Frame Number (seq) : " << image->header.seq << std::endl; | ||
os << " stamp : " << image->header.stamp << std::endl; | ||
os << " Depth Units : " << image->header.depth_units << std::endl; | ||
os << "Encoding : " << image->encoding << std::endl; | ||
os << "Width : " << image->width << std::endl; | ||
os << "Height : " << image->height << std::endl; | ||
os << "Step : " << image->step << std::endl; | ||
//os << "Frame Number : " << image->header.seq << std::endl; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. remove the comment |
||
//os << "Timestamp : " << pretty_time(std::chrono::nanoseconds(image->header.stamp.toNSec())) << std::endl; | ||
} | ||
else if (auto data = try_instantiate<sensor_msgs::Imu>(m)) | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Shouldn't be needed