Skip to content

Commit

Permalink
WIP
Browse files Browse the repository at this point in the history
  • Loading branch information
@maloel
maloel committed Mar 19, 2024
1 parent 864a85e commit 87b70dc
Show file tree
Hide file tree
Showing 10 changed files with 172 additions and 146 deletions.
38 changes: 32 additions & 6 deletions src/dds/rs-dds-device-proxy.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,7 @@ rs2_intrinsics to_rs2_intrinsics( const realdds::video_intrinsics & intrinsics )
static rs2_video_stream to_rs2_video_stream( rs2_stream const stream_type,
sid_index const & sidx,
std::shared_ptr< realdds::dds_video_stream_profile > const & profile,
const realdds::video_intrinsics & intrinsics )
const std::set< realdds::video_intrinsics > & intrinsics )
{
rs2_video_stream prof;
prof.type = stream_type;
Expand All @@ -89,7 +89,17 @@ static rs2_video_stream to_rs2_video_stream( rs2_stream const stream_type,
prof.height = profile->height();
prof.fps = profile->frequency();
prof.fmt = static_cast< rs2_format >( profile->encoding().to_rs2() );
prof.intrinsics = to_rs2_intrinsics( intrinsics.scaled_to( profile->width(), profile->height() ) );

// Handle intrinsics
auto intr = std::find_if( intrinsics.begin(),
intrinsics.end(),
[profile]( const realdds::video_intrinsics & intr )
{ return profile->width() == intr.width && profile->height() == intr.height; } );
if( intr != intrinsics.end() ) // Some profiles don't have intrinsics
{
prof.intrinsics = to_rs2_intrinsics( *intr );
}

return prof;
}

Expand Down Expand Up @@ -412,11 +422,27 @@ void dds_device_proxy::set_video_profile_intrinsics( std::shared_ptr< stream_pro
auto vsp = std::dynamic_pointer_cast< video_stream_profile >( profile );
int const w = vsp->get_width();
int const h = vsp->get_height();
vsp->set_intrinsics(
[stream_intrinsics = stream->get_intrinsics(), w, h]()

auto & stream_intrinsics = stream->get_intrinsics();
if( 1 == stream_intrinsics.size() )
{
// A single set of intrinsics will get scaled to any profile resolution
vsp->set_intrinsics( [intrinsics = *stream_intrinsics.begin(), w, h]()
{ return to_rs2_intrinsics( intrinsics.scaled_to( w, h ) ); } );
}
else
{
// When we have multiple sets of intrinsics (one per resolution), we're limited to these
auto it = std::find_if( stream_intrinsics.begin(),
stream_intrinsics.end(),
[w, h]( const realdds::video_intrinsics & intr )
{ return intr.width == w && intr.height == h; } );
if( it != stream_intrinsics.end() ) // Some profiles don't have intrinsics
{
return to_rs2_intrinsics( stream_intrinsics.scaled_to( w, h ) );
} );
vsp->set_intrinsics( [intr = to_rs2_intrinsics( *it )]() { return intr; } );
}
}

}


Expand Down
6 changes: 3 additions & 3 deletions third-party/realdds/include/realdds/dds-stream-server.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,8 +85,8 @@ class dds_video_stream_server : public dds_stream_server

void open( std::string const & topic_name, std::shared_ptr< dds_publisher > const & ) override;

void set_intrinsics( const video_intrinsics & intrinsics ) { _intrinsics = intrinsics; }
video_intrinsics const & get_intrinsics() const { return _intrinsics; }
void set_intrinsics( const std::set< video_intrinsics > & intrinsics ) { _intrinsics = intrinsics; }
const std::set< video_intrinsics > & get_intrinsics() const { return _intrinsics; }

void start_streaming( const image_header & );
void stop_streaming() override;
Expand All @@ -97,7 +97,7 @@ class dds_video_stream_server : public dds_stream_server
private:
void check_profile( std::shared_ptr< dds_stream_profile > const & ) const override;

video_intrinsics _intrinsics;
std::set< video_intrinsics > _intrinsics;
image_header _image_header;
};

Expand Down
6 changes: 3 additions & 3 deletions third-party/realdds/include/realdds/dds-stream.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,14 +68,14 @@ class dds_video_stream : public dds_stream
typedef std::function< void( topics::image_msg && f ) > on_data_available_callback;
void on_data_available( on_data_available_callback cb ) { _on_data_available = cb; }

void set_intrinsics( const video_intrinsics & intrinsics ) { _intrinsics = intrinsics; }
const video_intrinsics & get_intrinsics() const { return _intrinsics; }
void set_intrinsics( const std::set< video_intrinsics > & intrinsics ) { _intrinsics = intrinsics; }
const std::set< video_intrinsics > & get_intrinsics() const { return _intrinsics; }

protected:
void handle_data() override;
bool can_start_streaming() const override { return _on_data_available != nullptr; }

video_intrinsics _intrinsics;
std::set< video_intrinsics > _intrinsics;
on_data_available_callback _on_data_available = nullptr;
};

Expand Down
2 changes: 2 additions & 0 deletions third-party/realdds/include/realdds/dds-trinsics.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,8 +53,10 @@ struct video_intrinsics
distortion_parameters distortion = { distortion_model::none, { 0 } };

bool is_valid() const { return focal_length.x > 0 && focal_length.y > 0; }

bool operator<( const video_intrinsics & rhs ) const
{
// Arrange the intrinsics in order of increasing resolution (width then height)
return width < rhs.width || ( width == rhs.width && height < rhs.height );
}

Expand Down
18 changes: 5 additions & 13 deletions third-party/realdds/src/dds-device-impl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -693,25 +693,17 @@ void dds_device::impl::on_stream_options( json const & j, eprosima::fastdds::dds
{
if( auto video_stream = std::dynamic_pointer_cast< dds_video_stream >( stream ) )
{
video_intrinsics intrinsics;
std::set< video_intrinsics > intrinsics;
if( j_int.is_array() )
{
// Old-style intrinsics; multiple resolutions are provided, but we pick only one
// TODO remove
LOG_WARNING( "[" << debug_name() << "] stream '" << stream->name() << "' is using old-style intrinsics format" );
auto default_profile = std::dynamic_pointer_cast< dds_video_stream_profile >( video_stream->default_profile() );
if( ! default_profile )
DDS_THROW( runtime_error, "no default profile available to choose proper intrinsics" );
// Multiple resolutions are provided, likely from legacy devices from the adapter
for( auto & intr : j_int )
{
intrinsics = video_intrinsics::from_json( intr );
if( intrinsics.width == default_profile->width() && intrinsics.height == default_profile->height() )
break;
}
intrinsics.insert( video_intrinsics::from_json( intr ) );
}
else
{
intrinsics = video_intrinsics::from_json( j_int );
// Single intrinsics that will get scaled
intrinsics.insert( video_intrinsics::from_json( j_int ) );
}
video_stream->set_intrinsics( intrinsics );
}
Expand Down
61 changes: 38 additions & 23 deletions third-party/realdds/src/dds-device-server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,26 +80,22 @@ static void on_discovery_device_header( size_t const n_streams,
for( auto & ex : extr )
extrinsics_json.push_back( json::array( { ex.first.first, ex.first.second, ex.second->to_json() } ) );

topics::flexible_msg device_header( json{
json j_device_header{
{ topics::notification::key::id, topics::notification::device_header::id },
{ topics::notification::device_header::key::n_streams, n_streams },
{ topics::notification::device_header::key::extrinsics, std::move( extrinsics_json ) }
} );
auto json_string = slice( device_header.custom_data< char const >(), device_header._data.size() );
LOG_DEBUG( "-----> JSON = " << shorten_json_string( json_string, 300 ) << " size " << json_string.length() );
//LOG_DEBUG( "-----> CBOR size = " << json::to_cbor( device_header.json_data() ).size() );
};
topics::flexible_msg device_header( j_device_header );
LOG_DEBUG( "device-header " << std::setw( 4 ) << j_device_header << " size " << device_header._data.size() );
notifications.add_discovery_notification( std::move( device_header ) );

auto device_options = json::array();
for( auto & opt : options )
device_options.push_back( std::move( opt->to_json() ) );
topics::flexible_msg device_options_message( json {
{ topics::notification::key::id, topics::notification::device_options::id },
{ topics::notification::device_options::key::options, std::move( device_options ) }
} );
json_string = slice( device_options_message.custom_data< char const >(), device_options_message._data.size() );
LOG_DEBUG( "-----> JSON = " << shorten_json_string( json_string, 300 ) << " size " << json_string.length() );
//LOG_DEBUG( "-----> CBOR size = " << json::to_cbor( device_options_message.json_data() ).size() );
json j_device_options{ { topics::notification::key::id, topics::notification::device_options::id },
{ topics::notification::device_options::key::options, std::move( device_options ) } };
topics::flexible_msg device_options_message( j_device_options );
LOG_DEBUG( "device-options " << std::setw( 4 ) << j_device_options << " size " << device_options_message._data.size() );
notifications.add_discovery_notification( std::move( device_options_message ) );
}

Expand All @@ -110,18 +106,18 @@ static void on_discovery_stream_header( std::shared_ptr< dds_stream_server > con
auto profiles = json::array();
for( auto & sp : stream->profiles() )
profiles.push_back( std::move( sp->to_json() ) );
topics::flexible_msg stream_header_message( json{
json j_stream_header{
{ topics::notification::key::id, topics::notification::stream_header::id },
{ topics::notification::stream_header::key::type, stream->type_string() },
{ topics::notification::stream_header::key::name, stream->name() },
{ topics::notification::stream_header::key::sensor_name, stream->sensor_name() },
{ topics::notification::stream_header::key::profiles, std::move( profiles ) },
{ topics::notification::stream_header::key::default_profile_index, stream->default_profile_index() },
{ topics::notification::stream_header::key::metadata_enabled, stream->metadata_enabled() },
} );
auto json_string = slice( stream_header_message.custom_data< char const >(), stream_header_message._data.size() );
LOG_DEBUG( "-----> JSON = " << shorten_json_string( json_string, 300 ) << " size " << json_string.length() );
//LOG_DEBUG( "-----> CBOR size = " << json::to_cbor( stream_header_message.json_data() ).size() );
};
topics::flexible_msg stream_header_message( j_stream_header );
LOG_DEBUG( stream->name() << " stream-header " << std::setw( 4 ) << j_stream_header << " size "
<< stream_header_message._data.size() );
notifications.add_discovery_notification( std::move( stream_header_message ) );

json j_stream_options = json::object( {
Expand All @@ -133,13 +129,33 @@ static void on_discovery_stream_header( std::shared_ptr< dds_stream_server > con
for( auto & opt : stream->options() )
j_options.push_back( std::move( opt->to_json() ) );

auto & j_intrinsics = j_stream_options[topics::notification::stream_options::key::intrinsics];
if( auto video_stream = std::dynamic_pointer_cast< dds_video_stream_server >( stream ) )
{
j_intrinsics = video_stream->get_intrinsics().to_json();
auto & intrinsics = video_stream->get_intrinsics();
if( intrinsics.empty() )
{
// No intrinsics...
}
else
{
auto & j_intrinsics = j_stream_options[topics::notification::stream_options::key::intrinsics];
if( 1 == intrinsics.size() )
{
// Use an object with a single intrinsic
j_intrinsics = intrinsics.begin()->to_json();
}
else
{
// Multiple intrinsics are available
j_intrinsics = json::array();
for( auto & i : intrinsics )
j_intrinsics.push_back( i.to_json() );
}
}
}
else if( auto motion_stream = std::dynamic_pointer_cast< dds_motion_stream_server >( stream ) )
{
auto & j_intrinsics = j_stream_options[topics::notification::stream_options::key::intrinsics];
j_intrinsics = json::object( {
{ topics::notification::stream_options::intrinsics::key::accel,
motion_stream->get_accel_intrinsics().to_json() },
Expand All @@ -152,10 +168,9 @@ static void on_discovery_stream_header( std::shared_ptr< dds_stream_server > con
for( auto & filter : stream->recommended_filters() )
j_filters.push_back( filter );

topics::flexible_msg stream_options_message( std::move( j_stream_options ) );
json_string = slice( stream_options_message.custom_data< char const >(), stream_options_message._data.size() );
LOG_DEBUG( "-----> JSON = " << shorten_json_string( json_string, 300 ) << " size " << json_string.length() );
//LOG_DEBUG( "-----> CBOR size = " << json::to_cbor( stream_options_message.json_data() ).size() );
topics::flexible_msg stream_options_message( j_stream_options );
LOG_DEBUG( stream->name() << " stream-options " << std::setw( 4 ) << j_stream_options << " size "
<< stream_options_message._data.size() );
notifications.add_discovery_notification( std::move( stream_options_message ) );
}

Expand Down
2 changes: 2 additions & 0 deletions third-party/realdds/src/dds-trinsics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -169,6 +169,8 @@ json video_intrinsics::to_json() const
break;

case distortion_model::brown:
case distortion_model::inverse_brown:
case distortion_model::modified_brown:
if( ! coeffs_j.is_array() )
DDS_THROW( runtime_error, "invalid distortion coefficients: " << coeffs_j );
if( coeffs_j.size() != 5 )
Expand Down
26 changes: 6 additions & 20 deletions tools/dds/dds-adapter/lrs-device-controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -152,7 +152,7 @@ std::vector< std::shared_ptr< realdds::dds_stream_server > > lrs_device_controll
{
std::map< std::string, realdds::dds_stream_profiles > stream_name_to_profiles;
std::map< std::string, size_t > stream_name_to_default_profile;
std::map< std::string, realdds::video_intrinsics > stream_name_to_video_intrinsics;
std::map< std::string, std::set< realdds::video_intrinsics > > stream_name_to_video_intrinsics;

// Iterate over all profiles of all sensors and build appropriate dds_stream_servers
for( auto & sensor : _rs_dev.query_sensors() )
Expand Down Expand Up @@ -194,25 +194,12 @@ std::vector< std::shared_ptr< realdds::dds_stream_server > > lrs_device_controll
realdds::dds_video_encoding::from_rs2( vsp.format() ),
static_cast< uint16_t >( vsp.width() ),
static_cast< int16_t >( vsp.height() ) );
if( vsp.is_default() )
try
{
try
{
auto intr = to_realdds( vsp.get_intrinsics() );
LOG_DEBUG( "=============> " << profiles.size() << " default " << intr.to_json().dump( 4 ) );
stream_name_to_video_intrinsics[stream_name] = intr;
}
catch( ... ) {} //Some profiles don't have intrinsics
}
else
{
try
{
auto intr = to_realdds( vsp.get_intrinsics() );
LOG_DEBUG( "-------------> " << profiles.size() << " " << intr.to_json().dump( 4 ) );
}
catch( ... ) {} //Some profiles don't have intrinsics
auto intr = to_realdds( vsp.get_intrinsics() );
stream_name_to_video_intrinsics[stream_name].insert( intr );
}
catch( ... ) {} //Some profiles don't have intrinsics
}
else if( auto const msp = rs2::motion_stream_profile( sp ) )
{
Expand All @@ -237,9 +224,8 @@ std::vector< std::shared_ptr< realdds::dds_stream_server > > lrs_device_controll
{
if( sp.is_default() )
stream_name_to_default_profile[stream_name] = profiles.size();
LOG_DEBUG( stream_name << " " << profiles.size() << ": " << profile->to_string()
<< ( sp.is_default() ? " default" : "" ) );
profiles.push_back( profile );
LOG_DEBUG( stream_name << ": " << profile->to_string() );
}
} );
}
Expand Down
Loading

0 comments on commit 87b70dc

Please sign in to comment.