Adding depth units value to frame metadata

src/archive.h

@@ -339,6 +339,11 @@ namespace librealsense
             return additional_data.depth_units;
         }
 
+        void set_units(float depth_units)
+        {
+            additional_data.depth_units = depth_units;
+        }
+
         void set_original(frame_holder h)
         {
             _original = std::move(h);

src/ds5/ds5-device.cpp

@@ -373,11 +373,17 @@ namespace librealsense
             }
         }
 
+        void update_params(on_frame_md callback) override
+        {
+            _on_frame = callback;
+            auto s = get_raw_sensor().get();
+            As< librealsense::uvc_sensor >(s)->update_params(callback);
+        }
+
         void open(const stream_profiles& requests) override
         {
             _depth_units = get_option(RS2_OPTION_DEPTH_UNITS).query();
-            auto s = get_raw_sensor().get();
-            As< librealsense::uvc_sensor >(s)->set_depth_units(_depth_units);
+            update_params([&](float& du) {du = _depth_units.load(); });
 
             synthetic_sensor::open(requests);
 
@@ -463,8 +469,7 @@ namespace librealsense
         void set_depth_scale(float val)
         {
             _depth_units = val;
-            auto s = get_raw_sensor().get();
-            As< librealsense::uvc_sensor >(s)->set_depth_units(val);
+            update_params([&](float& du) {du = _depth_units; });
         }
 
         void init_hdr_config(const option_range& exposure_range, const option_range& gain_range)

src/media/ros/ros_reader.cpp

@@ -412,7 +412,7 @@ namespace librealsense
         additional_data.timestamp = timestamp_ms.count();
         additional_data.frame_number = msg->header.seq;
         additional_data.fisheye_ae_mode = false;
-        additional_data.depth_units = msg->header.depth_units;
+        additional_data.depth_units = msg->depth_units;
 
         stream_identifier stream_id;
         if (m_version == legacy_file_format::file_version())

src/media/ros/ros_writer.cpp

@@ -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());
         auto image_topic = ros_topic::frame_data_topic(stream_id);
         write_message(image_topic, timestamp, image);
         write_additional_frame_messages(stream_id, timestamp, frame);

src/proc/synthetic-stream.cpp

@@ -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);
+                        }
+                    }
+
                     auto res = process_frame(source, f);
                     if (!res) continue;
                     if (auto composite = res.as<rs2::frameset>())

src/sensor.cpp

@@ -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;
         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));

src/sensor.h

@@ -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;
 
     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;
         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
+        {
+            _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();

third-party/realsense-file/rosbag/msgs/sensor_msgs/Image.h

@@ -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);
+      }
+      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);
   }
 };
 

third-party/realsense-file/rosbag/msgs/std_msgs/Header.h

@@ -26,13 +26,11 @@ struct Header_
   Header_()
     : seq(0)
     , stamp()
-    , depth_units(-1)
     , frame_id()  {
     }
   Header_(const ContainerAllocator& _alloc)
     : seq(0)
     , stamp()
-    , depth_units(-1)
     , frame_id(_alloc)  {
   (void)_alloc;
     }
@@ -48,8 +46,6 @@ struct Header_
    typedef std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other >  _frame_id_type;
   _frame_id_type frame_id;
 
-  typedef float _depth_units_type;
-  _depth_units_type depth_units;
 
 
 
@@ -187,7 +183,6 @@ namespace serialization
       stream.next(m.seq);
       stream.next(m.stamp);
       stream.next(m.frame_id);
-      stream.next(m.depth_units);
     }
 
     ROS_DECLARE_ALLINONE_SERIALIZER
@@ -212,8 +207,6 @@ struct Printer< ::std_msgs::Header_<ContainerAllocator> >
     Printer<rs2rosinternal::Time>::stream(s, indent + "  ", v.stamp);
     s << indent << "frame_id: ";
     Printer<std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > >::stream(s, indent + "  ", v.frame_id);
-    s << indent << "depth units: ";
-    Printer<float>::stream(s, indent + "  ", v.depth_units);
   }
 };
 

third-party/realsense-file/rosbag/msgs/std_msgs/header_deprecated_def.h

@@ -57,7 +57,6 @@ struct Header_ : public std_msgs::Header_<ContainerAllocator>
     this->seq = rhs.seq;
     this->stamp = rhs.stamp;
     this->frame_id = rhs.frame_id;
-    this->depth_units = rhs.depth_units;
     return *this;
   }
 
@@ -67,7 +66,6 @@ struct Header_ : public std_msgs::Header_<ContainerAllocator>
     h.seq = this->seq;
     h.stamp = this->stamp;
     h.frame_id = this->frame_id;
-    h.depth_units = this->depth_units;
     return h;
   }
 
@@ -114,7 +112,6 @@ string frame_id\n\
     rs2rosinternal::serialization::serialize(stream, this->seq);
     rs2rosinternal::serialization::serialize(stream, this->stamp);
     rs2rosinternal::serialization::serialize(stream, this->frame_id);
-    rs2rosinternal::serialization::serialize(stream, this->depth_units);
     return stream.getData();
   }
 
@@ -124,7 +121,6 @@ string frame_id\n\
     rs2rosinternal::serialization::deserialize(stream, this->seq);
     rs2rosinternal::serialization::deserialize(stream, this->stamp);
     rs2rosinternal::serialization::deserialize(stream, this->frame_id);
-    rs2rosinternal::serialization::deserialize(stream, this->depth_units);
     return stream.getData();
   }
 
@@ -134,7 +130,6 @@ string frame_id\n\
     size += rs2rosinternal::serialization::serializationLength(this->seq);
     size += rs2rosinternal::serialization::serializationLength(this->stamp);
     size += rs2rosinternal::serialization::serializationLength(this->frame_id);
-    size += rs2rosinternal::serialization::deserialize(stream, this->depth_units);
     return size;
   }
 
@@ -222,7 +217,6 @@ template<class ContainerAllocator> struct Serializer< ::roslib::Header_<Containe
     stream.next(m.seq);
     stream.next(m.stamp);
     stream.next(m.frame_id);
-    stream.next(m.depth_units);
   }
 
   ROS_DECLARE_ALLINONE_SERIALIZER
@@ -246,8 +240,6 @@ struct Printer< ::roslib::Header_<ContainerAllocator> >
     Printer<rs2rosinternal::Time>::stream(s, indent + "  ", v.stamp);
     s << indent << "frame_id: ";
     Printer<std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > >::stream(s, indent + "  ", v.frame_id);
-    s << indent << "depth units: ";
-    Printer<float>::stream(s, indent + "  ", v.depth_units);
   }
 };
 

tools/rosbag-inspector/print_helpers.h

@@ -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;
             //os << "Timestamp    : " << pretty_time(std::chrono::nanoseconds(image->header.stamp.toNSec())) << std::endl;
         }
         else if (auto data = try_instantiate<sensor_msgs::Imu>(m))