Skip to content

Commit

Permalink
fix problems with type-index mapping
Browse files Browse the repository at this point in the history
  • Loading branch information
@maloel
maloel committed Oct 8, 2024
1 parent 54c3b1d commit d014218
Show file tree
Hide file tree
Showing 4 changed files with 61 additions and 45 deletions.
14 changes: 4 additions & 10 deletions src/core/stream-profile-interface.h
View file
Original file line number Diff line number Diff line change
@@ -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"
Expand All @@ -9,7 +9,7 @@
#include <functional>
#include <memory>
#include <vector>
#include <ostream>
#include <iosfwd>


namespace librealsense {
Expand Down Expand Up @@ -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
63 changes: 33 additions & 30 deletions src/dds/rs-dds-device-proxy.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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
Expand All @@ -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 )
{
Expand All @@ -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 );
Expand Down Expand Up @@ -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 )
{
auto & sensor_name = name_info.first;
auto & sensor_info = name_info.second;
auto & sensor_proxy = sensor_info.proxy;
//LOG_DEBUG( sensor_info.first );

// 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.
Expand Down
27 changes: 22 additions & 5 deletions src/proc/formats-converter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
#include <src/composite-frame.h>
#include <src/core/frame-callback.h>

#include <rsutils/string/from.h>
#include <ostream>

namespace librealsense
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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 )
Expand Down
2 changes: 2 additions & 0 deletions src/proc/formats-converter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 );
Expand Down

0 comments on commit d014218

Please sign in to comment.