PR #12879 from Eran: DDS metadata: add ToA, backend, actual FPS, raw-…

…frame-size
IntelRealSense · May 2, 2024 · 6b9e947 · 6b9e947
2 parents 9d83388 + 2d6eca1
commit 6b9e947
Show file tree

Hide file tree

Showing 30 changed files with 261 additions and 213 deletions.
diff --git a/src/dds/rs-dds-sensor-proxy.cpp b/src/dds/rs-dds-sensor-proxy.cpp
@@ -8,6 +8,7 @@
 
 #include <realdds/dds-device.h>
 #include <realdds/dds-time.h>
+#include <realdds/dds-sample.h>
 
 #include <realdds/topics/device-info-msg.h>
 #include <realdds/topics/image-msg.h>
@@ -17,6 +18,7 @@
 #include <src/core/options-registry.h>
 #include <src/core/frame-callback.h>
 #include <src/core/roi.h>
+#include <src/core/time-service.h>
 #include <src/stream.h>
 
 #include <src/proc/color-formats-converter.h>
@@ -286,15 +288,21 @@ void dds_sensor_proxy::open( const stream_profiles & profiles )
 
 
 void dds_sensor_proxy::handle_video_data( realdds::topics::image_msg && dds_frame,
+                                          realdds::dds_sample && dds_sample,
                                           const std::shared_ptr< stream_profile_interface > & profile,
                                           streaming_impl & streaming )
 {
     frame_additional_data data;  // with NO metadata by default!
+    data.system_time = time_service::get_time();  // time of arrival in system clock
+    data.backend_timestamp                        // time when the underlying backend (DDS) received it
+        = static_cast<rs2_time_t>(realdds::time_to_double( dds_sample.reception_timestamp ) * 1e3);
     data.timestamp               // in ms
         = static_cast< rs2_time_t >( realdds::time_to_double( dds_frame.timestamp ) * 1e3 );
+    data.last_timestamp = streaming.last_timestamp.exchange( data.timestamp );
     data.timestamp_domain;  // from metadata, or leave default (hardware domain)
     data.depth_units;       // from metadata
     data.frame_number;      // filled in only once metadata is known
+    data.raw_size = static_cast< uint32_t >( dds_frame.raw_data.size() );
 
     auto vid_profile = dynamic_cast< video_stream_profile_interface * >( profile.get() );
     if( ! vid_profile )
@@ -325,15 +333,21 @@ void dds_sensor_proxy::handle_video_data( realdds::topics::image_msg && dds_fram
 
 
 void dds_sensor_proxy::handle_motion_data( realdds::topics::imu_msg && imu,
+                                           realdds::dds_sample && sample,
                                            const std::shared_ptr< stream_profile_interface > & profile,
                                            streaming_impl & streaming )
 {
     frame_additional_data data;  // with NO metadata by default!
+    data.system_time = time_service::get_time();  // time of arrival in system clock
+    data.backend_timestamp                        // time when the underlying backend (DDS) received it
+        = static_cast<rs2_time_t>(realdds::time_to_double( sample.reception_timestamp ) * 1e3);
     data.timestamp               // in ms
         = static_cast< rs2_time_t >( realdds::time_to_double( imu.timestamp() ) * 1e3 );
+    data.last_timestamp = streaming.last_timestamp.exchange( data.timestamp );
     data.timestamp_domain;  // leave default (hardware domain)
     data.last_frame_number = streaming.last_frame_number.fetch_add( 1 );
     data.frame_number = data.last_frame_number + 1;
+    data.raw_size = sizeof( rs2_combined_motion );
 
     auto new_frame_interface = allocate_new_frame( RS2_EXTENSION_MOTION_FRAME, profile.get(), std::move( data ) );
     if( ! new_frame_interface )
@@ -478,19 +492,19 @@ void dds_sensor_proxy::start( rs2_frame_callback_sptr callback )
         if( auto dds_video_stream = std::dynamic_pointer_cast< realdds::dds_video_stream >( dds_stream ) )
         {
             dds_video_stream->on_data_available(
-                [profile, this, &streaming]( realdds::topics::image_msg && dds_frame )
+                [profile, this, &streaming]( realdds::topics::image_msg && dds_frame, realdds::dds_sample && sample )
                 {
                     if( _is_streaming )
-                        handle_video_data( std::move( dds_frame ), profile, streaming );
+                        handle_video_data( std::move( dds_frame ), std::move( sample ), profile, streaming );
                 } );
         }
         else if( auto dds_motion_stream = std::dynamic_pointer_cast< realdds::dds_motion_stream >( dds_stream ) )
         {
             dds_motion_stream->on_data_available(
-                [profile, this, &streaming]( realdds::topics::imu_msg && imu )
+                [profile, this, &streaming]( realdds::topics::imu_msg && imu, realdds::dds_sample && sample )
                 {
                     if( _is_streaming )
-                        handle_motion_data( std::move( imu ), profile, streaming );
+                        handle_motion_data( std::move( imu ), std::move( sample ), profile, streaming );
                 } );
         }
         else

diff --git a/src/dds/rs-dds-sensor-proxy.h b/src/dds/rs-dds-sensor-proxy.h
@@ -1,5 +1,5 @@
 // License: Apache 2.0. See LICENSE file in root directory.
-// Copyright(c) 2023 Intel Corporation. All Rights Reserved.
+// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved.
 #pragma once
 
 #include "sid_index.h"
@@ -8,6 +8,7 @@
 #include <src/proc/formats-converter.h>
 #include <src/core/options-watcher.h>
 
+#include <realdds/dds-defines.h>
 #include <realdds/dds-metadata-syncer.h>
 
 #include <rsutils/json-fwd.h>
@@ -54,6 +55,7 @@ class dds_sensor_proxy : public software_sensor
     {
         syncer_type syncer;
         std::atomic< unsigned long long > last_frame_number{ 0 };
+        std::atomic< rs2_time_t > last_timestamp;
     };
 
 private:
@@ -99,10 +101,12 @@ class dds_sensor_proxy : public software_sensor
     std::shared_ptr< realdds::dds_motion_stream_profile >
     find_profile( sid_index sidx, realdds::dds_motion_stream_profile const & profile ) const;
 
-    void handle_video_data( realdds::topics::image_msg && dds_frame,
+    void handle_video_data( realdds::topics::image_msg &&,
+                            realdds::dds_sample &&,
                             const std::shared_ptr< stream_profile_interface > &,
                             streaming_impl & streaming );
     void handle_motion_data( realdds::topics::imu_msg &&,
+                             realdds::dds_sample &&,
                              const std::shared_ptr< stream_profile_interface > &,
                              streaming_impl & );
     void handle_new_metadata( std::string const & stream_name,

diff --git a/src/metadata-parser.h b/src/metadata-parser.h
@@ -69,10 +69,12 @@ namespace librealsense
     class md_array_parser : public md_attribute_parser_base
     {
         rs2_frame_metadata_value _key;
+        std::shared_ptr< md_attribute_parser_base > _fallback;
 
     public:
-        md_array_parser( rs2_frame_metadata_value key )
+        md_array_parser( rs2_frame_metadata_value key, std::shared_ptr< md_attribute_parser_base > fallback = {} )
             : _key( key )
+            , _fallback( std::move( fallback ) )
         {
         }
 
@@ -81,7 +83,7 @@ namespace librealsense
             auto pmd = reinterpret_cast< metadata_array_value const * >( frm.additional_data.metadata_blob.data() );
             metadata_array_value const & value = pmd[_key];
             if( ! value.is_valid )
-                return false;
+                return _fallback ? _fallback->find( frm, p_value ) : false;
             if( p_value )
                 *p_value = value.value;
             return true;
@@ -93,17 +95,6 @@ namespace librealsense
      *  e.g change auto_exposure enum to boolean, change units from nano->ms,etc'*/
     typedef std::function<rs2_metadata_type(const rs2_metadata_type& param)> attrib_modifyer;
 
-    class md_time_of_arrival_parser : public md_attribute_parser_base
-    {
-    public:
-        bool find( const frame & frm, rs2_metadata_type * p_value ) const override
-        {
-            if( p_value )
-                *p_value = (rs2_metadata_type)frm.get_frame_system_time();
-            return true;
-        }
-    };
-
     /**\brief The metadata parser class directly access the metadata attribute in the blob received from HW.
     *   Given the metadata-nested construct, and the c++ lack of pointers
     *   to the inner struct, we pre-calculate and store the attribute offset internally
@@ -273,7 +264,7 @@ namespace librealsense
     {
     public:
         md_additional_parser(Attribute St::* attribute_name) :
-            _md_attribute(attribute_name) {};
+            _md_attribute(attribute_name) {}
 
         bool find( const frame & frm, rs2_metadata_type * p_value ) const override
         {
@@ -297,6 +288,45 @@ namespace librealsense
         return parser;
     }
 
+    /**
+     * provide attributes generated and stored internally by the library, unless set to a specific (default?)
+     * value
+     */
+    template< class St, class Attribute >
+    class md_additional_parser_unless : public md_additional_parser< St, Attribute >
+    {
+        using super = md_additional_parser< St, Attribute >;
+
+        Attribute _unless;  // value for which we return "not found"
+
+    public:
+        md_additional_parser_unless( Attribute St::*attribute_name, Attribute unless )
+            : super( attribute_name )
+            , _unless( unless )
+        {
+        }
+
+        bool find( const frame & frm, rs2_metadata_type * p_value ) const override
+        {
+            rs2_metadata_type value;
+            if( ! super::find( frm, &value ) )
+                return false;
+            if( value == _unless )
+                return false;
+            if( p_value )
+                *p_value = value;
+            return true;
+        }
+    };
+
+    /**\brief A utility function to create additional_data parser*/
+    template< class St, class Attribute >
+    std::shared_ptr< md_attribute_parser_base > make_additional_data_parser_unless( Attribute St::*attribute,
+                                                                                    Attribute unless )
+    {
+        return std::make_shared< md_additional_parser_unless< St, Attribute > >( attribute, unless );
+    }
+
     /**\brief Optical timestamp for RS4xx devices is calculated internally*/
     class md_rs400_sensor_timestamp : public md_attribute_parser_base
     {

diff --git a/src/sensor.cpp b/src/sensor.cpp
@@ -73,7 +73,8 @@ void log_callback_end( uint32_t fps,
     {
         register_option(RS2_OPTION_FRAMES_QUEUE_SIZE, _source.get_published_size_option());
 
-        register_metadata(RS2_FRAME_METADATA_TIME_OF_ARRIVAL, std::make_shared<librealsense::md_time_of_arrival_parser>());
+        register_metadata( RS2_FRAME_METADATA_TIME_OF_ARRIVAL,
+                           make_additional_data_parser_unless( &frame_additional_data::system_time, rs2_time_t( 0 ) ) );
 
         register_info(RS2_CAMERA_INFO_NAME, name);
     }

diff --git a/src/software-sensor.cpp b/src/software-sensor.cpp
@@ -25,7 +25,23 @@ static std::shared_ptr< metadata_parser_map > create_software_metadata_parser_ma
     for( int i = 0; i < static_cast< int >( rs2_frame_metadata_value::RS2_FRAME_METADATA_COUNT ); ++i )
     {
         auto key = static_cast< rs2_frame_metadata_value >( i );
-        md_parser_map->emplace( key, std::make_shared< md_array_parser >( key ) );
+        std::shared_ptr< md_attribute_parser_base > fallback;
+        switch( key )
+        {
+        case RS2_FRAME_METADATA_TIME_OF_ARRIVAL:
+            fallback = make_additional_data_parser_unless( &frame_additional_data::system_time, rs2_time_t( 0 ) );
+            break;
+        case RS2_FRAME_METADATA_BACKEND_TIMESTAMP:
+            fallback = make_additional_data_parser_unless( &frame_additional_data::backend_timestamp, rs2_time_t(0) );
+            break;
+        case RS2_FRAME_METADATA_RAW_FRAME_SIZE:
+            fallback = make_additional_data_parser_unless( &frame_additional_data::raw_size, uint32_t(0) );
+            break;
+        case RS2_FRAME_METADATA_ACTUAL_FPS:
+            fallback = std::make_shared< ds_md_attribute_actual_fps >();
+            break;
+        }
+        md_parser_map->emplace( key, std::make_shared< md_array_parser >( key, fallback ) );
     }
     return md_parser_map;
 }

diff --git a/third-party/realdds/include/realdds/dds-defines.h b/third-party/realdds/include/realdds/dds-defines.h
@@ -1,5 +1,5 @@
 // License: Apache 2.0. See LICENSE file in root directory.
-// Copyright(c) 2022 Intel Corporation. All Rights Reserved.
+// Copyright(c) 2022-4 Intel Corporation. All Rights Reserved.
 
 #pragma once
 
@@ -18,8 +18,14 @@ namespace types {
     class DynamicType_ptr;
 }  // namespace types
 }  // namespace fastrtps
+namespace fastdds {
+namespace dds {
+    struct SampleInfo;
+}  // namespace dds
+}  // namespace fastdds
 }  // namespace eprosima
 
+
 namespace realdds {
 
 
@@ -30,6 +36,7 @@ using dds_guid_prefix = eprosima::fastrtps::rtps::GuidPrefix_t;
 using dds_entity_id = eprosima::fastrtps::rtps::EntityId_t;
 typedef int dds_domain_id;
 typedef uint64_t dds_sequence_number;  // sample_identity.sequence_number()
+using dds_sample = eprosima::fastdds::dds::SampleInfo;
 
 
 }  // namespace realdds
diff --git a/third-party/realdds/include/realdds/dds-device-server.h b/third-party/realdds/include/realdds/dds-device-server.h
@@ -18,15 +18,6 @@
 #include <functional>
 
 
-namespace eprosima {
-namespace fastdds {
-namespace dds {
-struct SampleInfo;
-}  // namespace dds
-}  // namespace fastdds
-}  // namespace eprosima
-
-
 namespace realdds {
 
 

diff --git a/third-party/realdds/include/realdds/dds-sample.h b/third-party/realdds/include/realdds/dds-sample.h
@@ -0,0 +1,5 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright(c) 2024 Intel Corporation. All Rights Reserved.
+#pragma once
+
+#include <fastdds/dds/subscriber/SampleInfo.hpp>
diff --git a/third-party/realdds/include/realdds/dds-stream.h b/third-party/realdds/include/realdds/dds-stream.h
@@ -67,7 +67,7 @@ class dds_video_stream : public dds_stream
 
     void open( std::string const & topic_name, std::shared_ptr< dds_subscriber > const & ) override;
 
-    typedef std::function< void( topics::image_msg && f ) > on_data_available_callback;
+    typedef std::function< void( topics::image_msg &&, dds_sample && ) > on_data_available_callback;
     void on_data_available( on_data_available_callback cb ) { _on_data_available = cb; }
 
     void set_intrinsics( const std::set< video_intrinsics > & intrinsics ) { _intrinsics = intrinsics; }
@@ -132,7 +132,7 @@ class dds_motion_stream : public dds_stream
 
     void open( std::string const & topic_name, std::shared_ptr< dds_subscriber > const & ) override;
 
-    typedef std::function< void( topics::imu_msg && f ) > on_data_available_callback;
+    typedef std::function< void( topics::imu_msg &&, dds_sample && ) > on_data_available_callback;
     void on_data_available( on_data_available_callback cb ) { _on_data_available = cb; }
 
     void set_accel_intrinsics( const motion_intrinsics & intrinsics ) { _accel_intrinsics = intrinsics; }

diff --git a/third-party/realdds/include/realdds/dds-time.h b/third-party/realdds/include/realdds/dds-time.h
@@ -50,6 +50,15 @@ inline long double time_to_double( dds_time const & t )
 }
 
 
+inline long double time_to_double( eprosima::fastrtps::rtps::Time_t const & t )
+{
+    long double sec = t.seconds();
+    long double nsec = t.nanosec();
+    nsec /= 1000000000ULL;
+    return sec + nsec;
+}
+
+
 std::string time_to_string( dds_time const & t );
 
 

diff --git a/third-party/realdds/include/realdds/topics/blob-msg.h b/third-party/realdds/include/realdds/topics/blob-msg.h
@@ -1,5 +1,5 @@
 // License: Apache 2.0. See LICENSE file in root directory.
-// Copyright(c) 2023 Intel Corporation. All Rights Reserved.
+// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved.
 #pragma once
 
 #include "blob/blob.h"
@@ -10,15 +10,6 @@
 #include <vector>
 
 
-namespace eprosima {
-namespace fastdds {
-namespace dds {
-struct SampleInfo;
-}
-}  // namespace fastdds
-}  // namespace eprosima
-
-
 namespace udds {
 class blobPubSubType;
 }  // namespace raw
@@ -63,7 +54,7 @@ class blob_msg : public udds::blob
     //
     static bool take_next( dds_topic_reader &,
                            blob_msg * output,
-                           eprosima::fastdds::dds::SampleInfo * optional_info = nullptr );
+                           dds_sample * optional_sample = nullptr );
 
     // Returns some unique (to the writer) identifier for the sample that was sent, or 0 if unsuccessful
     dds_sequence_number write_to( dds_topic_writer & ) const;