Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

dds stream type derivations; rs-enumerate-devices works #11075

Merged
merged 9 commits into from
Nov 8, 2022
Merged

dds stream type derivations; rs-enumerate-devices works #11075

Show file tree
Hide file tree
Changes from 7 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
584acc9
complain if device.open() or .close() with no profiles/streams
maloel Nov 7, 2022
1140d42
reverse frequency & format in profile ctors to fit order in json
maloel Nov 7, 2022
41f8c67
add basic d435i test
maloel Nov 7, 2022
fcbfd00
add RS2_PRODUCT_LINE_SW_ONLY to find only software devices in context
maloel Nov 7, 2022
b98943a
add --debug, --sw-only, --dds-domain to rs-enumerate-devices
maloel Nov 7, 2022
2a0fb31
fix context creation on pyrealsense2 load
maloel Nov 7, 2022
32505ad
stream type derivations; change rs2 (sid.index) calc
maloel Nov 8, 2022
2fd9227
add librs test for d435i
maloel Nov 8, 2022
9ff3df6
CR fixes
maloel Nov 8, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions include/librealsense2/h/rs_context.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,6 +107,7 @@ rs2_device_list* rs2_query_devices(const rs2_context* context, rs2_error** error
#define RS2_PRODUCT_LINE_SR300 0x04
#define RS2_PRODUCT_LINE_L500 0x08
#define RS2_PRODUCT_LINE_T200 0x10
#define RS2_PRODUCT_LINE_SW_ONLY 0x100 // enable to return only SW devices, including playback
#define RS2_PRODUCT_LINE_DEPTH (RS2_PRODUCT_LINE_L500 | RS2_PRODUCT_LINE_SR300 | RS2_PRODUCT_LINE_D400)
#define RS2_PRODUCT_LINE_TRACKING RS2_PRODUCT_LINE_T200

Expand Down
139 changes: 92 additions & 47 deletions src/context.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -379,6 +379,27 @@ namespace librealsense
}

#ifdef BUILD_WITH_DDS
struct sid_index
{
int16_t sid; // Stream ID; assigned based on the stream TYPE, really
int8_t index; // Used to distinguish similar streams like IR L / R, 0 otherwise

sid_index( int sid_, int index_ )
: sid( static_cast< int16_t >( sid_ ) )
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why cast and not save as int? If it is received from LibRS as int then there is no point casting it. If need to cast when sending over DDS cast there, If the message interface will change, no need to update here

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's a copy from dds_stream_uid that was removed. You're right, but I don't think it's important right now?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I prefer that we will fix this now, we have to many things we commented on as "for later" and we'll forget to update them all

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

fixed

, index( static_cast< int8_t >( index_ ) )
{
}

sid_index() = default;
sid_index( sid_index const& ) = default;
sid_index( sid_index&& ) = default;

std::string to_string() const { return '(' + std::to_string( sid ) + '.' + std::to_string( index ) + ')'; }

inline bool operator<( sid_index const & r ) const { return this->sid < r.sid || this->index < r.index; }
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think this is a well defined "smaller than" operation. If a = { 5, 0 } and b = { 3, 1 } then a < b is true and also b < a is true;

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It doesn't matter, as long as it's consistent. This is simply a requirement for ordering items in a map.
That said, I think it's logical to break by sid and then index -- which is what I did, no?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It is logical to break sid and then index, but I think it means:
this->sid < r.sid || ( this->sid == r.sid && this->index < r.index )
No case when this->sid > r.sid will return true

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Right :)

};


class dds_sensor_proxy : public software_sensor
{
public:
Expand All @@ -391,23 +412,29 @@ namespace librealsense
{
}

int add_dds_stream( std::shared_ptr< realdds::dds_stream > const & stream )
void add_dds_stream( sid_index sidx, std::shared_ptr< realdds::dds_stream > const & stream )
{
int stream_index = static_cast< int >( _streams.size() );
_streams.push_back( stream );
return stream_index;
auto & s = _streams[sidx];
if( s )
{
LOG_ERROR( "stream at " << sidx.to_string() << " already exists for sensor '" << get_name() << "'" );
return;
}
s = stream;
}

std::shared_ptr< realdds::dds_video_stream_profile >
find_profile( int stream_index, realdds::dds_video_stream_profile const & profile )
find_profile( sid_index sidx, realdds::dds_video_stream_profile const & profile ) const
{
if( stream_index < 0 || stream_index >= _streams.size() )
auto it = _streams.find( sidx );
if( it == _streams.end() )
{
LOG_ERROR( "Invalid stream index " << stream_index << " in rs2 profile for sensor " << get_name() );
LOG_ERROR( "Invalid stream index " << sidx.to_string() << " in rs2 profile for sensor '" << get_name() << "'" );
}
else
{
for( auto & sp : _streams[stream_index]->profiles() )
auto& stream = it->second;
for( auto & sp : stream->profiles() )
{
auto vsp = std::static_pointer_cast< realdds::dds_video_stream_profile >( sp );
if( profile.width() == vsp->width() && profile.height() == vsp->height()
Expand All @@ -421,15 +448,17 @@ namespace librealsense
}

std::shared_ptr< realdds::dds_motion_stream_profile >
find_profile( int stream_index, realdds::dds_motion_stream_profile const & profile )
find_profile( sid_index sidx, realdds::dds_motion_stream_profile const & profile ) const
{
if( stream_index < 0 || stream_index >= _streams.size() )
auto it = _streams.find( sidx );
if( it == _streams.end() )
{
LOG_ERROR( "Invalid stream index " << stream_index << " in rs2 profile for sensor " << get_name() );
LOG_ERROR( "Invalid stream index " << sidx.to_string() << " in rs2 profile for sensor '" << get_name() << "'" );
}
else
{
for( auto & sp : _streams[stream_index]->profiles() )
auto & stream = it->second;
for( auto & sp : stream->profiles() )
{
auto msp = std::static_pointer_cast< realdds::dds_motion_stream_profile >( sp );
if( profile.format() == msp->format() && profile.frequency() == msp->frequency() )
Expand All @@ -446,34 +475,38 @@ namespace librealsense
realdds::dds_stream_profiles realdds_profiles;
for( size_t i = 0; i < profiles.size(); ++i )
{
if( Is< video_stream_profile >( profiles[i] ) )
auto & sp = profiles[i];
sid_index sidx( sp->get_unique_id(), sp->get_stream_index() );
if( Is< video_stream_profile >( sp ) )
{
const auto && vsp = As< video_stream_profile >( profiles[i] );
auto stream_index = vsp->get_stream_index();
auto video_profile
= find_profile( stream_index,
= find_profile( sidx,
realdds::dds_video_stream_profile(
realdds::dds_stream_format::from_rs2( profiles[i]->get_format() ),
profiles[i]->get_framerate(),
sp->get_framerate(),
realdds::dds_stream_format::from_rs2( sp->get_format() ),
vsp->get_width(),
vsp->get_height() ) );
if( video_profile )
realdds_profiles.push_back( video_profile );
else
LOG_ERROR( "no profile found in stream for rs2 profile " << vsp );
}
else if( Is< motion_stream_profile >( profiles[i] ) )
else if( Is< motion_stream_profile >( sp ) )
{
auto stream_index = profiles[i]->get_stream_index();
auto motion_profile
= find_profile( stream_index,
= find_profile( sidx,
realdds::dds_motion_stream_profile(
realdds::dds_stream_format::from_rs2( profiles[i]->get_format() ),
profiles[i]->get_framerate() ) );
profiles[i]->get_framerate(),
realdds::dds_stream_format::from_rs2( sp->get_format() ) ) );
if( motion_profile )
realdds_profiles.push_back( motion_profile );
else
LOG_ERROR( "no profile found in stream for rs2 profile " << profiles[i] );
LOG_ERROR( "no profile found in stream for rs2 profile " << sp );
}
else
{
LOG_ERROR( "unknown stream profile type for rs2 profile " << sp );
}
}

Expand All @@ -489,9 +522,10 @@ namespace librealsense
{
//dds_device::close expects <stream_uid, stream_index> pairs
realdds::dds_streams streams_to_close;
for (auto profile : sensor_base::get_active_streams())
for( auto profile : sensor_base::get_active_streams() )
{
streams_to_close.push_back( _streams[profile->get_stream_index()] );
streams_to_close.push_back(
_streams[sid_index( profile->get_unique_id(), profile->get_stream_index() )] );
}
_dev->close( streams_to_close );
software_sensor::close();
Expand All @@ -500,12 +534,12 @@ namespace librealsense
//float get_option( rs2_option option ) const override;
//void set_option( rs2_option option, float value ) const override;

const std::string & get_name() { return _name; }
const std::string & get_name() const { return _name; }

private:
std::shared_ptr< realdds::dds_device > const & _dev;
std::string _name;
realdds::dds_streams _streams;
std::map< sid_index, std::shared_ptr< realdds::dds_stream > > _streams;
};

// This is the rs2 device; it proxies to an actual DDS device that does all the actual
Expand All @@ -531,8 +565,6 @@ namespace librealsense
{ "gpio", RS2_STREAM_GPIO },
{ "pose", RS2_STREAM_POSE },
{ "confidence", RS2_STREAM_CONFIDENCE },
{ "video", RS2_STREAM_COLOR },
{ "motion", RS2_STREAM_GYRO },
};
auto it = type_to_rs2.find( type_string );
if( it == type_to_rs2.end() )
Expand All @@ -545,14 +577,13 @@ namespace librealsense

static rs2_video_stream
to_rs2_video_stream( rs2_stream const stream_type,
size_t const sensor_index,
int const stream_index,
sid_index const & sidx,
std::shared_ptr< realdds::dds_video_stream_profile > const & profile )
{
rs2_video_stream prof;
prof.type = stream_type;
prof.index = stream_index;
prof.uid = static_cast< int >( sensor_index );
prof.index = sidx.index;
prof.uid = sidx.sid;
prof.width = profile->width();
prof.height = profile->height();
prof.fps = profile->frequency();
Expand All @@ -564,14 +595,13 @@ namespace librealsense

static rs2_motion_stream
to_rs2_motion_stream( rs2_stream const stream_type,
size_t const sensor_index,
int const stream_index,
sid_index const & sidx,
std::shared_ptr< realdds::dds_motion_stream_profile > const & profile )
{
rs2_motion_stream prof;
prof.type = stream_type;
prof.index = stream_index;
prof.uid = static_cast< int >( sensor_index );
prof.index = sidx.index;
prof.uid = sidx.sid;
prof.fps = profile->frequency();
prof.fmt = static_cast< rs2_format >( profile->format().to_rs2() );

Expand All @@ -598,9 +628,19 @@ namespace librealsense
int sensor_index;
};
std::map< std::string, sensor_info > sensor_name_to_info;
// We assign (sid,index) based on the stream type:
// LibRS assigns streams names based on the type followed by an index if it's not 0.
// I.e., sid is based on the type, and index is 0 unless there's more than 1 in which case it's 1-based.
int counts[RS2_STREAM_COUNT] = { 0 };
// Count how many streams per type
_dds_dev->foreach_stream( [&]( std::shared_ptr< realdds::dds_stream > const & stream ) {
++counts[to_rs2_stream_type( stream->type_string() )];
} );
// Anything that's more than 1 stream starts the count at 1, otherwise 0
for( int & count : counts )
count = ( count > 1 ) ? 1 : 0;
// Now we can finally assign (sid,index):
_dds_dev->foreach_stream( [&]( std::shared_ptr< realdds::dds_stream > const & stream ) {
// We assign (sid,index) based on the sensor: multiple streams can come from the same
// sensor, in which case they'd get the same sid but a different index...
auto & info = sensor_name_to_info[stream->sensor_name()];
if( ! info.proxy )
{
Expand All @@ -610,13 +650,14 @@ namespace librealsense
assert( info.sensor_index == _software_sensors.size() );
_software_sensors.push_back( info.proxy );
}
auto stream_index = info.proxy->add_dds_stream( stream );
LOG_DEBUG( info.sensor_index << " " << stream->sensor_name() << " : " << stream->name() << " " << stream_index );
auto stream_type = to_rs2_stream_type( stream->type_string() );
sid_index sidx( stream_type, counts[stream_type]++ );
info.proxy->add_dds_stream( sidx, stream );
LOG_DEBUG( sidx.to_string() << " " << stream->sensor_name() << " : " << stream->name() );

software_sensor & sensor = get_software_sensor( info.sensor_index );
auto video_stream = std::dynamic_pointer_cast< realdds::dds_video_stream >( stream );
auto motion_stream = std::dynamic_pointer_cast< realdds::dds_motion_stream >( stream );
auto stream_type = to_rs2_stream_type( stream->type_string() );
auto & profiles = stream->profiles();
auto const & default_profile = profiles[stream->default_profile_index()];
for( auto & profile : profiles )
Expand All @@ -626,7 +667,7 @@ namespace librealsense
sensor.add_video_stream(
to_rs2_video_stream(
stream_type,
info.sensor_index, stream_index,
sidx,
std::static_pointer_cast< realdds::dds_video_stream_profile >( profile ) ),
profile == default_profile );
}
Expand All @@ -635,7 +676,7 @@ namespace librealsense
sensor.add_motion_stream(
to_rs2_motion_stream(
stream_type,
info.sensor_index, stream_index,
sidx,
std::static_pointer_cast< realdds::dds_motion_stream_profile >( profile ) ),
profile == default_profile );
}
Expand Down Expand Up @@ -682,9 +723,13 @@ namespace librealsense

std::vector<std::shared_ptr<device_info>> context::query_devices(int mask) const
{
platform::backend_device_group devices( _backend->query_uvc_devices(),
_backend->query_usb_devices(),
_backend->query_hid_devices() );
platform::backend_device_group devices;
if( ! ( mask & RS2_PRODUCT_LINE_SW_ONLY ) )
{
devices.uvc_devices = _backend->query_uvc_devices();
devices.usb_devices = _backend->query_usb_devices();
devices.hid_devices = _backend->query_hid_devices();
}
return create_devices( devices, _playback_devices, mask );
}

Expand Down
13 changes: 5 additions & 8 deletions third-party/realdds/include/realdds/dds-stream-profile.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,12 +57,11 @@ class dds_stream_profile
virtual ~dds_stream_profile() {}

protected:
dds_stream_profile( dds_stream_format format, int16_t frequency )
dds_stream_profile( int16_t frequency, dds_stream_format format )
: _format( format )
, _frequency( frequency )
{
}
dds_stream_profile( dds_stream_profile && ) = default;
dds_stream_profile( nlohmann::json const &, int & index );

public:
Expand Down Expand Up @@ -108,14 +107,13 @@ class dds_video_stream_profile : public dds_stream_profile
uint16_t _height; // Resolution height [pixels]

public:
dds_video_stream_profile( dds_stream_format format, int16_t frequency, uint16_t width, uint16_t height )
: super( format, frequency )
dds_video_stream_profile( int16_t frequency, dds_stream_format format, uint16_t width, uint16_t height )
: super( frequency, format )
, _width( width )
, _height( height )
{
}
dds_video_stream_profile( nlohmann::json const &, int & index );
dds_video_stream_profile( dds_video_stream_profile && ) = default;

uint16_t width() const { return _width; }
uint16_t height() const { return _height; }
Expand All @@ -131,15 +129,14 @@ class dds_motion_stream_profile : public dds_stream_profile
typedef dds_stream_profile super;

public:
dds_motion_stream_profile( dds_stream_format format, int16_t frequency )
: super( format, frequency )
dds_motion_stream_profile( int16_t frequency, dds_stream_format format )
: super( frequency, format )
{
}
dds_motion_stream_profile( nlohmann::json const & j, int & index )
: super( j, index )
{
}
dds_motion_stream_profile( dds_motion_stream_profile && ) = default;
};


Expand Down
Loading