fix problems with type-index mapping

Author: maloel

src/core/stream-profile-interface.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 "stream-interface.h"
@@ -9,7 +9,7 @@
 #include <functional>
 #include <memory>
 #include <vector>
-#include <ostream>
+#include <iosfwd>
 
 
 namespace librealsense {
@@ -38,14 +38,8 @@ class stream_profile_interface
 using stream_profiles = std::vector< std::shared_ptr< stream_profile_interface > >;
 
 
-inline std::ostream & operator<<( std::ostream & os, const stream_profiles & profiles )
-{
-    for( auto & p : profiles )
-    {
-        os << rs2_format_to_string( p->get_format() ) << " " << rs2_stream_to_string( p->get_stream_type() ) << ", ";
-    }
-    return os;
-}
+std::ostream & operator<<( std::ostream &, const std::shared_ptr< stream_profile_interface > & );
+std::ostream & operator<<( std::ostream &, const stream_profiles & );
 
 
 }  // namespace librealsense

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

@@ -176,6 +176,12 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const &
         std::shared_ptr< dds_sensor_proxy > proxy;
         int sensor_index = 0;
         rs2_stream type = RS2_STREAM_ANY;
+        // dds_streams bear stream type and index information, we add it to a dds_sensor_proxy mapped by a newly generated
+        // unique ID. After the sensor initialization we get all the "final" profiles from formats-converter with type and
+        // index but without IDs. We need to find the dds_stream that each profile was created from so we create a map from
+        // type and index to dds_stream ID and index, because the dds_sensor_proxy holds a map from sidx to dds_stream. We
+        // need both the ID from that map key and the stream itself (for intrinsics information)
+        std::map< sid_index, sid_index > type_and_index_to_dds_stream_sidx;
     };
     std::map< std::string, sensor_info > sensor_name_to_info;
 
@@ -185,7 +191,7 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const &
         {
             auto & sensor = sensor_name_to_info[stream->sensor_name()];
             if( stream->type_string() == "depth"
-                || stream->type_string() == "infrared" )
+                || stream->type_string() == "ir" )
             {
                 // If there's depth or infrared, it is a depth sensor regardless of what else is in there
                 // E.g., the D405 has a color stream in the depth sensor
@@ -208,13 +214,6 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const &
             }
         } );  // End foreach_stream lambda
 
-    // dds_streams bear stream type and index information, we add it to a dds_sensor_proxy mapped by a newly generated
-    // unique ID. After the sensor initialization we get all the "final" profiles from formats-converter with type and
-    // index but without IDs. We need to find the dds_stream that each profile was created from so we create a map from
-    // type and index to dds_stream ID and index, because the dds_sensor_proxy holds a map from sidx to dds_stream. We
-    // need both the ID from that map key and the stream itself (for intrinsics information)
-    std::map< sid_index, sid_index > type_and_index_to_dds_stream_sidx;
-
     _dds_dev->foreach_stream(
         [&]( std::shared_ptr< realdds::dds_stream > const & stream )
         {
@@ -235,9 +234,14 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const &
                 = std::make_shared< librealsense::stream >( stream_type, sidx.index );
             sensor_info.proxy->add_dds_stream( sidx, stream );
             _stream_name_to_owning_sensor[stream->name()] = sensor_info.proxy;
-            type_and_index_to_dds_stream_sidx.insert( { type_and_index, sidx }  );
-            LOG_DEBUG( sidx.to_string() << " " << get_string( sensor_info.type ) << " '" << stream->sensor_name()
-                                        << "' : '" << stream->name() << "' " << get_string( stream_type ) );
+            if( ! sensor_info.type_and_index_to_dds_stream_sidx.insert( { type_and_index, sidx } ).second )
+                LOG_ERROR( "Failed to insert '" << stream->sensor_name() << "' " << type_and_index.to_string() << " "
+                                                << get_string( sensor_info.type ) << " -> " << get_string( stream_type )
+                                                << " " << sidx.to_string() << " '" << stream->name() << "' mapping" );
+            //else
+            //    LOG_DEBUG( "'" << stream->sensor_name() << "' " << type_and_index.to_string() << " "
+            //                   << get_string( sensor_info.type ) << " -> " << get_string( stream_type ) << " "
+            //                   << sidx.to_string() << " '" << stream->name() << "'" );
 
             software_sensor & sensor = get_software_sensor( sensor_info.sensor_index );
             auto video_stream = std::dynamic_pointer_cast< realdds::dds_video_stream >( stream );
@@ -279,36 +283,35 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const &
             }
         } );  // End foreach_stream lambda
 
-    for( auto & sensor_info : sensor_name_to_info )
+    for( auto & name_info : sensor_name_to_info )
     {
-        LOG_DEBUG( sensor_info.first );
+        auto & sensor_name = name_info.first;
+        auto & sensor_info = name_info.second;
+        auto & sensor_proxy = sensor_info.proxy;
+        //LOG_DEBUG( sensor_name );
 
         // Set profile's ID based on the dds_stream's ID (index already set). Connect the profile to the extrinsics graph.
-        for( auto & profile : sensor_info.second.proxy->get_stream_profiles() )
+        // The get_stream_profiles() call will initialize the profiles (calling dds_sensor_proxy::init_stream_profiles())
+        for( auto & profile : sensor_proxy->get_stream_profiles() )
         {
-            //if( auto p = std::dynamic_pointer_cast< librealsense::video_stream_profile_interface >( profile ) )
-            //{
-            //    LOG_DEBUG( "    " << get_string( p->get_stream_type() ) << ' ' << p->get_stream_index() << ' '
-            //                      << get_string( p->get_format() ) << ' ' << p->get_width() << 'x' << p->get_height()
-            //                      << " @ " << p->get_framerate() );
-            //}
-            //else if( auto p = std::dynamic_pointer_cast<librealsense::motion_stream_profile_interface>( profile ) )
-            //{
-            //    LOG_DEBUG( "    " << get_string( p->get_stream_type() ) << ' ' << p->get_stream_index() << ' '
-            //                      << get_string( p->get_format() ) << " @ " << p->get_framerate() );
-            //}
-            sid_index type_and_index( profile->get_stream_type(), profile->get_stream_index() );
-            
-            auto & streams = sensor_info.second.proxy->streams();
-            
-            sid_index sidx = type_and_index_to_dds_stream_sidx.at( type_and_index );
+            auto & source_profiles = sensor_proxy->_formats_converter.get_source_profiles_from_target( profile );
+            if( source_profiles.size() != 1 )
+                LOG_ERROR( "More than one source profile available for [" << profile << "]: " << source_profiles );
+            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() )
             {
-                LOG_DEBUG( "        no dds stream" );
+                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
 
             // NOTE: the 'initialization_done' call above creates target profiles from the raw profiles we supplied it.
@@ -348,7 +351,7 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const &
                     auto const dds_extr = _dds_dev->get_extrinsics( from_stream.first, to_stream.first );
                     if( ! dds_extr )
                     {
-                        LOG_DEBUG( "missing extrinsics from " << from_stream.first << " to " << to_stream.first );
+                        //LOG_DEBUG( "missing extrinsics from " << from_stream.first << " to " << to_stream.first );
                         continue;
                     }
                     rs2_extrinsics extr = to_rs2_extrinsics( dds_extr );

src/proc/formats-converter.cpp

@@ -6,6 +6,7 @@
 #include <src/composite-frame.h>
 #include <src/core/frame-callback.h>
 
+#include <rsutils/string/from.h>
 #include <ostream>
 
 namespace librealsense
@@ -73,15 +74,18 @@ std::ostream & operator<<( std::ostream & os, const std::shared_ptr< stream_prof
 {
     if( profile )
     {
-        os << "(" << rs2_stream_to_string( profile->get_stream_type() ) << ")";
-        os << " " << rs2_format_to_string( profile->get_format() );
-        os << " " << profile->get_stream_index();
+        os << rs2_stream_to_string( profile->get_stream_type() );
+        if( auto stream_index = profile->get_stream_index() )
+            os << " " << stream_index;
         if( auto vsp = As< video_stream_profile, stream_profile_interface >( profile ) )
         {
             os << " " << vsp->get_width();
             os << "x" << vsp->get_height();
         }
-        os << " @ " << profile->get_framerate();
+        os << " " << rs2_format_to_string( profile->get_format() );
+        os << " @ " << profile->get_framerate() << " Hz";
+        if( auto bsp = std::dynamic_pointer_cast< backend_stream_profile >( profile ) )
+            bsp->to_stream( os );
     }
 
     return os;
@@ -97,7 +101,7 @@ stream_profiles formats_converter::get_all_possible_profiles( const stream_profi
 
     for( auto & raw_profile : raw_profiles )
     {
-        LOG_DEBUG( "Raw profile: " << raw_profile );
+        //LOG_DEBUG( "Raw profile: " << raw_profile );
         for( auto & pbf : _pb_factories )
         {
             const auto & sources = pbf->get_source_info();
@@ -129,7 +133,7 @@ stream_profiles formats_converter::get_all_possible_profiles( const stream_profi
                             target.resolution_transform( width, height );
                             cloned_vsp->set_dims( width, height );
                         }
-                        LOG_DEBUG( "          -> " << cloned_profile );
+                        //LOG_DEBUG( "          -> " << cloned_profile );
 
                         // Cache pbf supported profiles for efficiency in find_pbf_matching_most_profiles
                         _pbf_supported_profiles[pbf.get()].push_back( cloned_profile );
@@ -222,6 +226,7 @@ bool formats_converter::is_profile_in_list( const std::shared_ptr< stream_profil
 // Not passing const & because we modify from_profiles, would otherwise need to create a copy
 void formats_converter::prepare_to_convert( stream_profiles from_profiles )
 {
+    LOG_DEBUG( "Requested: " << from_profiles );
     clear_active_cache();
 
     // Add missing data to target profiles (was not available during get_all_possible_target_profiles)
@@ -264,10 +269,22 @@ void formats_converter::prepare_to_convert( stream_profiles from_profiles )
             }
         }
         const stream_profiles & print_current_resolved_reqs = { current_resolved_reqs.begin(), current_resolved_reqs.end() };
-        LOG_INFO( "Request: " << from_profiles_of_best_match << "\nResolved to: " << print_current_resolved_reqs );
+        LOG_DEBUG( "Resolved to: " << print_current_resolved_reqs );
     }
 }
 
+
+stream_profiles const & formats_converter::get_source_profiles_from_target(
+    std::shared_ptr< stream_profile_interface > const & target_profile ) const
+{
+    auto it = _target_profiles_to_raw_profiles.find( to_profile( target_profile.get() ) );
+    if( it == _target_profiles_to_raw_profiles.end() )
+        throw invalid_value_exception( rsutils::string::from()
+                                       << "target profile [" << target_profile << "] not found" );
+    return it->second;
+}
+
+
 void formats_converter::update_target_profiles_data( const stream_profiles & from_profiles )
 {
     for( auto & from_profile : from_profiles )

src/proc/formats-converter.h

@@ -43,6 +43,8 @@ namespace librealsense
         rs2_frame_callback_sptr get_frames_callback() const { return _converted_frames_callback; }
         void convert_frame( frame_holder & f );
 
+        stream_profiles const & get_source_profiles_from_target( std::shared_ptr< stream_profile_interface > const & target_profile ) const;
+
     protected:
         void clear_active_cache();
         void update_target_profiles_data( const stream_profiles & from_profiles );