PR #13840 from OhadMeir: Better handling of DDS streams information

Author: OhadMeir

include/librealsense2/h/rs_frame.h

@@ -262,6 +262,14 @@ int rs2_get_frame_points_count(const rs2_frame* frame, rs2_error** error);
 */
 const rs2_stream_profile* rs2_get_frame_stream_profile(const rs2_frame* frame, rs2_error** error);
 
+/**
+ * Returns the stream profile name
+ * \param[in] profile     stream profile whose name we want to get
+ * \param[out] error      If non-null, receives any error that occurs during this call, otherwise, errors are ignored
+ * \return                Stream name, as a null terminated string
+ */
+const char * rs2_get_stream_profile_name( const rs2_stream_profile * profile, rs2_error ** error );
+
 /**
 * Test if the given frame can be extended to the requested extension
 * \param[in]  frame             Realsense frame

include/librealsense2/hpp/rs_frame.hpp

@@ -112,10 +112,19 @@ namespace rs2
         */
         std::string stream_name() const
         {
-            std::stringstream ss;
-            ss << rs2_stream_to_string(stream_type());
-            if (stream_index() != 0) ss << " " << stream_index();
-            return ss.str();
+            rs2_error * e = nullptr;
+            std::string name = rs2_get_stream_profile_name( _profile, &e );
+
+            if( name.empty() )
+            {
+                std::stringstream ss;
+                ss << rs2_stream_to_string( stream_type() );
+                if( stream_index() != 0 )
+                    ss << " " << stream_index();
+                name = ss.str();
+            }
+
+            return name;
         }
 
         /**

src/core/stream-profile-interface.h

@@ -10,6 +10,7 @@
 #include <memory>
 #include <vector>
 #include <iosfwd>
+#include <string>
 
 
 namespace librealsense {
@@ -29,6 +30,9 @@ class stream_profile_interface
     virtual int get_tag() const = 0;
     virtual void tag_profile( int tag ) = 0;
 
+    virtual const char * get_name() = 0;
+    virtual void set_name( const std::string & name ) = 0;
+
     virtual std::shared_ptr< stream_profile_interface > clone() const = 0;
     virtual rs2_stream_profile * get_c_wrapper() const = 0;
     virtual void set_c_wrapper( rs2_stream_profile * wrapper ) = 0;

src/dds/rs-dds-device-proxy.cpp

@@ -229,7 +229,7 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const &
                 _software_sensors.push_back( sensor_info.proxy );
             }
             auto stream_type = to_rs2_stream_type( stream->type_string() );
-            int index = get_index_from_stream_name( stream->name() );
+            int index = get_unique_index_in_sensor( sensor_info.proxy, stream->name(), stream_type );
             sid_index sidx( environment::get_instance().generate_stream_id(), index);
             sid_index type_and_index( stream_type, index );
             _stream_name_to_librs_stream[stream->name()]
@@ -253,29 +253,44 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const &
             for( auto & profile : profiles )
             {
                 //LOG_DEBUG( "    " << profile->details_to_string() );
-                if( video_stream )
+                try
                 {
-                    auto video_profile = std::static_pointer_cast< realdds::dds_video_stream_profile >( profile );
-                    auto raw_stream_profile = sensor.add_video_stream(
-                        to_rs2_video_stream( stream_type, sidx, video_profile, video_stream->get_intrinsics() ),
-                        profile == default_profile );
-                    _stream_name_to_profiles[stream->name()].push_back( raw_stream_profile );
+                    if( video_stream )
+                    {
+                        auto video_profile = std::static_pointer_cast< realdds::dds_video_stream_profile >( profile );
+                        auto raw_stream_profile = sensor.add_video_stream(
+                            to_rs2_video_stream( stream_type, sidx, video_profile, video_stream->get_intrinsics() ),
+                            profile == default_profile, stream->name() );
+                        _stream_name_to_profiles[stream->name()].push_back( raw_stream_profile );
+                    }
+                    else if( motion_stream )
+                    {
+                        auto motion_profile = std::static_pointer_cast< realdds::dds_motion_stream_profile >( profile );
+                        auto raw_stream_profile = sensor.add_motion_stream(
+                            to_rs2_motion_stream( stream_type, sidx, motion_profile, motion_stream->get_gyro_intrinsics() ),
+                            profile == default_profile, stream->name() );
+                        _stream_name_to_profiles[stream->name()].push_back( raw_stream_profile );
+                    }
                 }
-                else if( motion_stream )
+                catch( std::exception const& e )
                 {
-                    auto motion_profile = std::static_pointer_cast< realdds::dds_motion_stream_profile >( profile );
-                    auto raw_stream_profile = sensor.add_motion_stream(
-                        to_rs2_motion_stream( stream_type, sidx, motion_profile, motion_stream->get_gyro_intrinsics() ),
-                        profile == default_profile );
-                    _stream_name_to_profiles[stream->name()].push_back( raw_stream_profile );
+                    LOG_ERROR( "Cannot add profile " << profile->details_to_string() << " to stream " << stream->name()
+                                                     << ". " << e.what() );
                 }
                 // NOTE: the raw profile will be cloned and overriden by the format converter!
             }
 
             auto & options = stream->options();
             for( auto & option : options )
             {
-                sensor_info.proxy->add_option( option );
+                try
+                {
+                    sensor_info.proxy->add_option( option );
+                }
+                catch( std::exception const& e )
+                {
+                    LOG_ERROR( e.what() );
+                }
             }
 
             auto & recommended_filters = stream->recommended_filters();
@@ -302,28 +317,32 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const &
             auto source_profile = source_profiles[0];
 
             sid_index type_and_index( source_profile->get_stream_type(), source_profile->get_stream_index() );
-            sid_index sidx = sensor_info.type_and_index_to_dds_stream_sidx.at( type_and_index );
-
-            auto & streams = sensor_proxy->streams();
-            auto stream_iter = streams.find( sidx );
-            if( stream_iter == streams.end() )
+            const auto & it = sensor_info.type_and_index_to_dds_stream_sidx.find( type_and_index );
+            if( it != sensor_info.type_and_index_to_dds_stream_sidx.end())
             {
-                LOG_ERROR( "No dds stream " << sidx.to_string() << " found for '" << sensor_name << "' " << profile
-                                            << " -> " << source_profile << " " << type_and_index.to_string() );
-                continue;
-            }
+                sid_index sidx = (*it).second;
+
+                auto & streams = sensor_proxy->streams();
+                auto stream_iter = streams.find( sidx );
+                if( stream_iter == streams.end() )
+                {
+                    LOG_ERROR( "No dds stream " << sidx.to_string() << " found for '" << sensor_name << "' " << profile
+                                                << " -> " << source_profile << " " << type_and_index.to_string() );
+                    continue;
+                }
 
-            //LOG_DEBUG( "    " << profile << " -> " << source_profile << " " << type_and_index.to_string() );
-            profile->set_unique_id( sidx.sid );  // Was lost on clone
+                //LOG_DEBUG( "    " << profile << " -> " << source_profile << " " << type_and_index.to_string() );
+                profile->set_unique_id( sidx.sid );  // Was lost on clone
 
-            // NOTE: the 'initialization_done' call above creates target profiles from the raw profiles we supplied it.
-            // The raw profile intrinsics will be overriden to call the target's intrinsics function (which by default
-            // calls the raw again, creating an infinite loop), so we must override the target:
-            set_profile_intrinsics( profile, stream_iter->second );
+                // NOTE: the 'get_stream_profiles' call above creates target profiles from the raw profiles we supplied it.
+                // The raw profile intrinsics will be overriden to call the target's intrinsics function (which by default
+                // calls the raw again, creating an infinite loop), so we must override the target:
+                set_profile_intrinsics( profile, stream_iter->second );
 
-            _stream_name_to_profiles.at( stream_iter->second->name() ).push_back( profile );  // For extrinsics
+                _stream_name_to_profiles.at( stream_iter->second->name() ).push_back( profile );  // For extrinsics
 
-            tag_default_profile_of_stream( profile, stream_iter->second );
+                tag_default_profile_of_stream( profile, stream_iter->second );
+            }
         }
     }
 
@@ -432,12 +451,48 @@ int dds_device_proxy::get_index_from_stream_name( const std::string & name ) con
 {
     int index = 0;
 
-    auto delimiter_pos = name.find( '_' );
+    // If camera sends stream index the info is appended to the stream name with underscore after the name
+    // e.g 'Infrared_1'. However, there might be other underscores in the name, e.g. 'Compressed_Color_1'
+    auto delimiter_pos = name.find_last_of( '_' );
     if( delimiter_pos != std::string::npos )
     {
         std::string index_as_string = name.substr( delimiter_pos + 1, name.size() );
-        index = std::stoi( index_as_string );
+        try
+        {
+            index = std::stoi(index_as_string);
+        }
+        catch (...)
+        {
+            //Do nothing, return default index
+        }
+    }
+
+    return index;
+}
+
+
+int dds_device_proxy::get_unique_index_in_sensor( const std::shared_ptr< dds_sensor_proxy > & sensor,
+                                                  const std::string & stream_name, rs2_stream stream_type ) const
+{
+    bool index_adjusted = false;
+    int index = get_index_from_stream_name( stream_name );
+
+    // Sending stream index as part of stream name is optional. We want to distinguish streams of same type in the
+    // sensor so we create an internal, running index for it.
+    do
+    {
+        index_adjusted = false;
+        for( auto & stream : sensor->streams() )
+        {
+            // We can have multiple streams with same index (e.g. 0) but not two of the same type
+            if( index == stream.first.index && stream_type == to_rs2_stream_type( stream.second->type_string() ) )
+            {
+                index++;
+                index_adjusted = true;
+            }
+        }
     }
+    while( index_adjusted );
 
     return index;
 }
@@ -558,7 +613,8 @@ void dds_device_proxy::tag_default_profile_of_stream(
         // Superset = picked up in pipeline with enable_all_stream() config
         // We want color and depth to be default, and add infrared as superset
         int tag = PROFILE_TAG_SUPERSET;
-        if( strcmp( stream->type_string(), "color" ) == 0 || strcmp( stream->type_string(), "depth" ) == 0 )
+        if( ( strcmp( stream->type_string(), "color" ) == 0 && profile->get_stream_index() == 0 ) || // Now we have cameras with more then one color stream, take the first
+              strcmp( stream->type_string(), "depth" ) == 0 )
             tag |= PROFILE_TAG_DEFAULT;
         else if( strcmp( stream->type_string(), "ir" ) != 0 || profile->get_stream_index() >= 2 )
             return;  // leave untagged

src/dds/rs-dds-device-proxy.h

@@ -55,6 +55,8 @@ class dds_device_proxy
     rsutils::subscription _metadata_subscription;
 
     int get_index_from_stream_name( const std::string & name ) const;
+    int get_unique_index_in_sensor( const std::shared_ptr< dds_sensor_proxy > & sensor,
+                                    const std::string & stream_name, rs2_stream stream_type ) const;
     void set_profile_intrinsics( std::shared_ptr< stream_profile_interface > const & profile,
                                  const std::shared_ptr< realdds::dds_stream > & stream ) const;
     void set_video_profile_intrinsics( std::shared_ptr< stream_profile_interface > const & profile,

src/dds/rs-dds-sensor-proxy.cpp

@@ -79,45 +79,6 @@ void dds_sensor_proxy::add_dds_stream( sid_index sidx, std::shared_ptr< realdds:
 }
 
 
-std::shared_ptr< stream_profile_interface > dds_sensor_proxy::add_video_stream( rs2_video_stream video_stream,
-                                                                                bool is_default )
-{
-    auto profile = std::make_shared< video_stream_profile >();
-    profile->set_dims( video_stream.width, video_stream.height );
-    profile->set_format( video_stream.fmt );
-    profile->set_framerate( video_stream.fps );
-    profile->set_stream_index( video_stream.index );
-    profile->set_stream_type( video_stream.type );
-    profile->set_unique_id( video_stream.uid );
-    profile->set_intrinsics( [=]() {  //
-        return video_stream.intrinsics;
-    } );
-    if( is_default )
-        profile->tag_profile( profile_tag::PROFILE_TAG_DEFAULT );
-    _sw_profiles.push_back( profile );
-
-    return profile;
-}
-
-
-std::shared_ptr< stream_profile_interface > dds_sensor_proxy::add_motion_stream( rs2_motion_stream motion_stream,
-                                                                                 bool is_default )
-{
-    auto profile = std::make_shared< motion_stream_profile >();
-    profile->set_format( motion_stream.fmt );
-    profile->set_framerate( motion_stream.fps );
-    profile->set_stream_index( motion_stream.index );
-    profile->set_stream_type( motion_stream.type );
-    profile->set_unique_id( motion_stream.uid );
-    profile->set_intrinsics( [=]() { return motion_stream.intrinsics; } );
-    if( is_default )
-        profile->tag_profile( profile_tag::PROFILE_TAG_DEFAULT );
-    _sw_profiles.push_back( profile );
-
-    return profile;
-}
-
-
 stream_profiles dds_sensor_proxy::init_stream_profiles()
 {
     auto profiles = get_raw_stream_profiles();

src/dds/rs-dds-sensor-proxy.h

@@ -75,8 +75,6 @@ class dds_sensor_proxy : public software_sensor
     const std::string & get_name() const { return _name; }
 
     void add_dds_stream( sid_index sidx, std::shared_ptr< realdds::dds_stream > const & stream );
-    std::shared_ptr<stream_profile_interface> add_video_stream( rs2_video_stream video_stream, bool is_default ) override;
-    std::shared_ptr<stream_profile_interface> add_motion_stream( rs2_motion_stream motion_stream, bool is_default ) override;
 
     void open( const stream_profiles & profiles ) override;
     void start( rs2_frame_callback_sptr callback ) override;

src/proc/formats-converter.cpp

@@ -123,6 +123,11 @@ stream_profiles formats_converter::get_all_possible_profiles( const stream_profi
                         cloned_profile->set_format( target.format );
                         cloned_profile->set_stream_index( target.index );
                         cloned_profile->set_stream_type( target.stream );
+                        // UVC raw profile name is not set, default name can be created based on type and index, but raw UVC index is always 0.
+                        // Use temporary variable to generate name without changing original raw_profile object.
+                        auto && tmp_raw_profile = std::dynamic_pointer_cast< stream_profile_base >( raw_profile ).get();
+                        tmp_raw_profile->set_stream_index( target.index );
+                        cloned_profile->set_name( tmp_raw_profile->get_name() );
 
                         auto cloned_vsp = As< video_stream_profile, stream_profile_interface >( cloned_profile );
                         if( cloned_vsp )

src/realsense.def

@@ -76,6 +76,7 @@ EXPORTS
     rs2_get_frame_stride_in_bytes
     rs2_get_frame_bits_per_pixel
     rs2_get_frame_stream_profile
+    rs2_get_stream_profile_name
     rs2_get_frame_vertices
     rs2_get_frame_texture_coordinates
     rs2_get_frame_points_count

src/rs.cpp

@@ -1422,6 +1422,13 @@ const rs2_stream_profile* rs2_get_frame_stream_profile(const rs2_frame* frame_re
 }
 HANDLE_EXCEPTIONS_AND_RETURN(nullptr, frame_ref)
 
+const char * rs2_get_stream_profile_name( const rs2_stream_profile * profile, rs2_error ** error ) BEGIN_API_CALL
+{
+    VALIDATE_NOT_NULL( profile );
+    return profile->profile->get_name();
+}
+HANDLE_EXCEPTIONS_AND_RETURN( nullptr, profile )
+
 int rs2_get_frame_bits_per_pixel(const rs2_frame* frame_ref, rs2_error** error) BEGIN_API_CALL
 {
     VALIDATE_NOT_NULL(frame_ref);