Change interface: add function rs2_get_frame_sensor() for C interface…

…, frame::get_sensor(), sensor_from_frame() for C++ interface and Frame::Sensor for C# interface.

Add demo usage in rs-data-collect and cs-tutorial-2-capture.

include/librealsense2/h/rs_frame.h

@@ -99,6 +99,14 @@ rs2_timestamp_domain rs2_get_frame_timestamp_domain(const rs2_frame* frameset, r
 */
 rs2_time_t rs2_get_frame_timestamp(const rs2_frame* frame, rs2_error** error);
 
+/**
+* retrieve frame parent sensor from frame handle
+* \param[in] frame      handle returned from a callback
+* \param[out] error     if non-null, receives any error that occurs during this call, otherwise, errors are ignored
+* \return               the parent sensor of the frame
+*/
+rs2_sensor* rs2_get_frame_sensor(const rs2_frame* frame, rs2_error** error);
+
 /**
 * retrieve frame number from frame handle
 * \param[in] frame      handle returned from a callback

include/librealsense2/hpp/rs_frame.hpp

@@ -398,6 +398,15 @@ namespace rs2
         * retrieve the time at which the frame was captured
         * \return            the timestamp of the frame, in milliseconds since the device was started
         */
+
+        rs2_sensor* get_sensor()
+        {
+            rs2_error* e = nullptr;
+            auto r = rs2_get_frame_sensor(frame_ref, &e);
+            error::handle(e);
+            return r;
+        }
+
         double get_timestamp() const
         {
             rs2_error* e = nullptr;

include/librealsense2/hpp/rs_sensor.hpp

@@ -329,6 +329,12 @@ namespace rs2
 
     };
 
+    inline std::shared_ptr<sensor> sensor_from_frame(frame f)
+    {
+        std::shared_ptr<rs2_sensor> psens(f.get_sensor(), rs2_delete_sensor);
+        return std::make_shared<sensor>(psens);
+    }
+
     inline bool operator==(const sensor& lhs, const sensor& rhs)
     {
         if (!(lhs.supports(RS2_CAMERA_INFO_NAME) && lhs.supports(RS2_CAMERA_INFO_SERIAL_NUMBER)

src/core/streaming.h

@@ -227,7 +227,7 @@ namespace librealsense
         virtual frame_callback_ptr get_frames_callback() const = 0;
         virtual void set_frames_callback(frame_callback_ptr cb) = 0;
         virtual bool is_streaming() const = 0;
-        virtual const device_interface& get_device() = 0;
+        virtual device_interface& get_device() = 0;
 
         virtual ~sensor_interface() = default;
     };

src/media/playback/playback_sensor.cpp

@@ -25,7 +25,7 @@ std::string profile_to_string(std::shared_ptr<stream_profile_interface> s)
     return os.str();
 }
 
-playback_sensor::playback_sensor(const device_interface& parent_device, const device_serializer::sensor_snapshot& sensor_description):
+playback_sensor::playback_sensor(device_interface& parent_device, const device_serializer::sensor_snapshot& sensor_description):
     m_is_started(false),
     m_sensor_description(sensor_description),
     m_sensor_id(sensor_description.get_sensor_index()),
@@ -167,7 +167,7 @@ bool playback_sensor::extend_to(rs2_extension extension_type, void** ext)
     return playback_device::try_extend_snapshot(e, extension_type, ext);
 }
 
-const device_interface& playback_sensor::get_device()
+device_interface& playback_sensor::get_device()
 {
     return m_parent_device;
 }

src/media/playback/playback_sensor.h

@@ -26,7 +26,7 @@ namespace librealsense
         signal<playback_sensor, const std::vector<device_serializer::stream_identifier>& > opened;
         signal<playback_sensor, const std::vector<device_serializer::stream_identifier>& > closed;
 
-        playback_sensor(const device_interface& parent_device, const device_serializer::sensor_snapshot& sensor_description);
+        playback_sensor(device_interface& parent_device, const device_serializer::sensor_snapshot& sensor_description);
         virtual ~playback_sensor();
 
         stream_profiles get_stream_profiles(int tag = profile_tag::PROFILE_TAG_ANY) const override;
@@ -38,7 +38,7 @@ namespace librealsense
         void stop() override;
         bool is_streaming() const override;
         bool extend_to(rs2_extension extension_type, void** ext) override;
-        const device_interface& get_device() override;
+        device_interface& get_device() override;
         void update_option(rs2_option id, std::shared_ptr<option> option);
         void stop(bool invoke_required);
         void flush_pending_frames();
@@ -78,7 +78,7 @@ namespace librealsense
         uint32_t m_sensor_id;
         std::mutex m_mutex;
         std::map<std::pair<rs2_stream, uint32_t>, std::shared_ptr<stream_profile_interface>> m_streams;
-        const device_interface& m_parent_device;
+        device_interface& m_parent_device;
         stream_profiles m_available_profiles;
         stream_profiles m_active_streams;
         const unsigned int _default_queue_size;

src/media/record/record_sensor.cpp

@@ -7,7 +7,7 @@
 
 using namespace librealsense;
 
-librealsense::record_sensor::record_sensor(const device_interface& device,
+librealsense::record_sensor::record_sensor( device_interface& device,
                                             sensor_interface& sensor) :
     m_sensor(sensor),
     m_is_recording(false),
@@ -168,7 +168,7 @@ bool librealsense::record_sensor::extend_to(rs2_extension extension_type, void**
     }
 }
 
-const device_interface& record_sensor::get_device()
+device_interface& record_sensor::get_device()
 {
     return m_parent_device;
 }

src/media/record/record_sensor.h

@@ -18,7 +18,7 @@ namespace librealsense
                           public options_container
     {
     public:
-        record_sensor(const device_interface& device,
+        record_sensor(device_interface& device,
                       sensor_interface& sensor);
         virtual ~record_sensor();
         void init();
@@ -36,7 +36,7 @@ namespace librealsense
         void stop() override;
         bool is_streaming() const override;
         bool extend_to(rs2_extension extension_type, void** ext) override;
-        const device_interface& get_device() override;
+        device_interface& get_device() override;
         frame_callback_ptr get_frames_callback() const override;
         void set_frames_callback(frame_callback_ptr callback) override;
         stream_profiles get_active_streams() const override;
@@ -71,7 +71,7 @@ namespace librealsense
         frame_callback_ptr m_frame_callback;
         frame_callback_ptr m_original_callback;
         int m_before_start_callback_token;
-        const device_interface& m_parent_device;
+        device_interface& m_parent_device;
         bool m_is_sensor_hooked;
         std::mutex m_mutex;
     };

src/realsense.def

@@ -55,6 +55,7 @@ EXPORTS
     rs2_supports_frame_metadata
     rs2_get_frame_timestamp
     rs2_get_frame_timestamp_domain
+    rs2_get_frame_sensor
     rs2_get_frame_number
     rs2_get_frame_data
     rs2_get_frame_width

src/rs.cpp

@@ -59,12 +59,11 @@ struct rs2_sensor : public rs2_options
         librealsense::sensor_interface* sensor,
         size_t index)
         : rs2_options((librealsense::options_interface*)sensor),
-        parent(parent), sensor(sensor), index(index)
+        parent(parent), sensor(sensor)
     {}
 
     rs2_device parent;
     librealsense::sensor_interface* sensor;
-    size_t index;
 
     rs2_sensor& operator=(const rs2_sensor&) = delete;
     rs2_sensor(const rs2_sensor&) = delete;
@@ -764,6 +763,20 @@ rs2_timestamp_domain rs2_get_frame_timestamp_domain(const rs2_frame* frame_ref,
 }
 HANDLE_EXCEPTIONS_AND_RETURN(RS2_TIMESTAMP_DOMAIN_COUNT, frame_ref)
 
+rs2_sensor* rs2_get_frame_sensor(const rs2_frame* frame, rs2_error** error) BEGIN_API_CALL
+{
+    VALIDATE_NOT_NULL(frame);
+    std::shared_ptr<librealsense::sensor_interface> sensor( ((frame_interface*)frame)->get_sensor() );
+    device_interface& dev = sensor->get_device();
+    auto dev_info = std::make_shared<librealsense::readonly_device_info>(dev.shared_from_this());
+    rs2_device dev2{ dev.get_context(), dev_info, dev.shared_from_this() };
+    return new rs2_sensor(
+        dev2,
+        (((frame_interface*)frame)->get_sensor()).get(),
+        (size_t)0);
+}
+HANDLE_EXCEPTIONS_AND_RETURN(nullptr, frame)
+
 const void* rs2_get_frame_data(const rs2_frame* frame_ref, rs2_error** error) BEGIN_API_CALL
 {
     VALIDATE_NOT_NULL(frame_ref);

src/sensor.cpp

@@ -394,7 +394,7 @@ namespace librealsense
         }
     }
 
-    const device_interface& sensor_base::get_device()
+    device_interface& sensor_base::get_device()
     {
         return *_owner;
     }

src/sensor.h

@@ -70,7 +70,7 @@ namespace librealsense
             _on_before_frame_callback = callback;
         }
 
-        const device_interface& get_device() override;
+        device_interface& get_device() override;
 
         void register_pixel_format(native_pixel_format pf);
         void remove_pixel_format(native_pixel_format pf);

tools/data-collect/rs-data-collect.cpp

@@ -124,7 +124,7 @@ void data_collector::save_data_to_file(const string& out_filename)
 
     for (const auto& elem : data_collection)
     {
-        csv << "\n\nStream Type,Index,F#,HW Timestamp (ms),Host Timestamp(ms)"
+        csv << "\n\nStream Type,Index,F#,HW Timestamp (ms),Host Timestamp(ms), SerialNumber"
             << (val_in_range(elem.first.first, { RS2_STREAM_GYRO,RS2_STREAM_ACCEL }) ? ",3DOF_x,3DOF_y,3DOF_z" : "")
             << (val_in_range(elem.first.first, { RS2_STREAM_POSE }) ? ",t_x,t_y,t_z,r_x,r_y,r_z,r_w" : "")
             << std::endl;
@@ -161,6 +161,7 @@ void data_collector::collect_frame_attributes(rs2::frame f, std::chrono::time_po
             rec._params = { pose.translation.x, pose.translation.y, pose.translation.z,
                     pose.rotation.x,pose.rotation.y,pose.rotation.z,pose.rotation.w };
         }
+        rec._dev_serial_no = sensor_from_frame(f)->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
 
         data_collection[stream_uid].emplace_back(rec);
     }

tools/data-collect/rs-data-collect.h

@@ -194,14 +194,16 @@ namespace rs_data_collect
             frame_record(unsigned long long frame_number, double frame_ts, double host_ts,
                        rs2_timestamp_domain domain, rs2_stream stream_type,int stream_index,
                        double _p1=0., double _p2=0., double _p3=0.,
-                       double _p4=0., double _p5=0., double _p6=0., double _p7=0.):
+                       double _p4=0., double _p5=0., double _p6=0., double _p7=0.,
+                       std::string dev_serial=std::string("")):
             _frame_number(frame_number),
             _ts(frame_ts),
             _arrival_time(host_ts),
             _domain(domain),
             _stream_type(stream_type),
             _stream_idx(stream_index),
-            _params({_p1,_p2,_p3,_p4,_p5,_p6,_p7})
+            _params({_p1,_p2,_p3,_p4,_p5,_p6,_p7}),
+            _dev_serial_no(dev_serial)
             {};
 
             std::string to_string() const
@@ -219,9 +221,10 @@ namespace rs_data_collect
                 if (val_in_range(_stream_type,{RS2_STREAM_POSE}))
                     specific_attributes = 7;
 
-                    for (auto i=0; i<specific_attributes; i++)
-                        ss << "," << _params[i];
+                for (auto i=0; i<specific_attributes; i++)
+                    ss << "," << _params[i];
 
+                ss << "," << _dev_serial_no;
                 return ss.str().c_str();
             }
 
@@ -232,6 +235,7 @@ namespace rs_data_collect
             rs2_stream              _stream_type;
             int                     _stream_idx;
             std::array<double,7>    _params;            // |The parameters are optional and sensor specific
+            std::string             _dev_serial_no;
         };
 
     private:

wrappers/csharp/Intel.RealSense/Frames/Frame.cs

@@ -181,6 +181,17 @@ public double Timestamp
             }
         }
 
+        /// <summary>Gets the sensor owning the frame</summary>
+        /// <value>the pointer to the sensor owning the frame</value>
+        public IntPtr Sensor
+        {
+            get
+            {
+                object error;
+                return NativeMethods.rs2_get_frame_sensor(Handle, out error);
+            }
+        }
+
         /// <summary>Gets the timestamp domain from frame handle. timestamps can only be comparable if they are in common domain</summary>
         /// <remarks>
         /// (for example, depth timestamp might come from system time while color timestamp might come from the device)

wrappers/csharp/Intel.RealSense/NativeMethods.cs

@@ -225,6 +225,9 @@ internal static MemCpyDelegate GetMethod()
         [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)]
         internal static extern double rs2_get_frame_timestamp(IntPtr frame, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error);
 
+        [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)]
+        internal static extern IntPtr rs2_get_frame_sensor(IntPtr frame, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error);
+
         [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)]
         internal static extern ulong rs2_get_frame_number(IntPtr frame, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error);
 

wrappers/csharp/cs-tutorial-2-capture/Window.xaml.cs

@@ -78,9 +78,10 @@ public CaptureWindow()
                             Dispatcher.Invoke(DispatcherPriority.Render, updateDepth, colorizedDepth);
                             Dispatcher.Invoke(DispatcherPriority.Render, updateColor, colorFrame);
 
-                            Dispatcher.Invoke(new Action(() =>
-                            {
-                                txtTimeStamp.Text = String.Format("{0,-20:0.00}", depthFrame.Timestamp) + "(" + depthFrame.TimestampDomain.ToString() + ")";
+                            Dispatcher.Invoke(new Action(() =>
+                            {
+                                String color_dev_sn = new Sensor.CameraInfos(depthFrame.Sensor)[CameraInfo.SerialNumber];
+                                txtTimeStamp.Text = color_dev_sn + " : " + String.Format("{0,-20:0.00}", depthFrame.Timestamp) + "(" + depthFrame.TimestampDomain.ToString() + ")";
                             }));
                         }
                     }