You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

703 lines
24 KiB

// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2022 Intel Corporation. All Rights Reserved.
#include "rs-dds-sniffer.h"
#include <thread>
#include <memory>
#include <fastdds/dds/domain/DomainParticipant.hpp>
#include <fastdds/dds/subscriber/DataReader.hpp>
#include <fastdds/dds/subscriber/qos/DataReaderQos.hpp>
#include <fastdds/dds/subscriber/DataReaderListener.hpp>
#include <fastdds/dds/log/Log.hpp>
#include <fastrtps/types/DynamicDataHelper.hpp>
#include <fastrtps/types/DynamicDataFactory.h>
#include <tclap/CmdLine.h>
#include <tclap/ValueArg.h>
#include <tclap/SwitchArg.h>
#include <realdds/dds-utilities.h>
#include <realdds/dds-guid.h>
#include <realdds/dds-log-consumer.h>
#include <rsutils/os/special-folder.h>
#include <rsutils/os/executable-name.h>
#include <rsutils/easylogging/easyloggingpp.h>
#include <rsutils/json.h>
#include <rsutils/json-config.h>
using namespace TCLAP;
using namespace eprosima::fastdds::dds;
using namespace eprosima::fastrtps::types;
// FastDDS GUID_t: (MSB first, little-endian; see GuidUtils.hpp)
// 2 bytes - vendor ID
// 2 bytes - host
// 4 bytes - process (2 pid, 2 random)
// 4 bytes - participant
// 4 bytes - entity ID (reader/writer)
// For example:
// Participant 1 - 01.0f.be.05.f0.09.86.b6.01.00.00.00|0.0.1.c1
// Writer under participant 1 - 01.0f.be.05.f0.09.86.b6.01.00.00.00|0.0.1.2
// Participant 2 of same process - 01.0f.be.05.f0.09.86.b6.02.00.00.00|0.0.1.c1
// Reader under participant 2 - 01.0f.be.05.f0.09.86.b6.02.00.00.00|0.0.1.7
// Participant 3 other process - 01.0f.be.05.88.50.ea.4a.01.00.00.00|0.0.1.c1
// Note same host for all, participant and entity IDs may be repeat for different processes
// To differentiate entities of different participant with same name we append process GUID values to the name
constexpr uint8_t GUID_PROCESS_LOCATION = 4;
static eprosima::fastrtps::rtps::GuidPrefix_t std_prefix;
static inline realdds::print_guid print_guid( realdds::dds_guid const & guid )
{
return realdds::print_guid( guid, std_prefix );
}
int main( int argc, char ** argv ) try
{
realdds::dds_domain_id domain = -1; // from settings; default to 0
uint32_t seconds = 0;
CmdLine cmd( "librealsense rs-dds-sniffer tool", ' ' );
SwitchArg snapshot_arg( "s", "snapshot", "run momentarily taking a snapshot of the domain" );
SwitchArg machine_readable_arg( "m", "machine-readable", "output entities in a way more suitable for automatic parsing" );
SwitchArg topic_samples_arg( "t", "topic-samples", "register to topics that send TypeObject and print their samples" );
SwitchArg debug_arg( "", "debug", "Enable debug logging", false );
SwitchArg participants_arg( "", "participants", "Show participants and quit; implies --snapshot", false );
ValueArg< realdds::dds_domain_id > domain_arg( "d", "domain", "select domain ID to listen on", false, 0, "0-232" );
ValueArg< std::string > root_arg( "r", "root", "filter anything not inside this root path", false, "", "" );
cmd.add( snapshot_arg );
cmd.add( machine_readable_arg );
cmd.add( topic_samples_arg );
cmd.add( domain_arg );
cmd.add( debug_arg );
cmd.add( participants_arg );
cmd.add( root_arg );
cmd.parse( argc, argv );
bool participants = participants_arg.isSet();
bool machine_readable = machine_readable_arg.isSet();
bool topic_samples = topic_samples_arg.isSet();
bool snapshot = snapshot_arg.isSet() || participants;
// Intercept DDS messages and redirect them to our own logging mechanism
rsutils::configure_elpp_logger( debug_arg.isSet() );
eprosima::fastdds::dds::Log::ClearConsumers();
eprosima::fastdds::dds::Log::RegisterConsumer( realdds::log_consumer::create() );
if( snapshot )
{
seconds = participants ? 1 : 3; // don't need as much time to detect participants
}
if( domain_arg.isSet() )
{
domain = domain_arg.getValue();
if( domain > 232 )
{
LOG_ERROR( "Invalid domain value, enter a value in the range [0, 232]" );
return EXIT_FAILURE;
}
}
dds_sniffer sniffer;
sniffer.print_discoveries( ! snapshot );
sniffer.print_by_topics( snapshot && ! participants );
sniffer.print_machine_readable( machine_readable );
sniffer.print_topic_samples( topic_samples && ! snapshot );
sniffer.set_root( root_arg.getValue() );
if( ! sniffer.init( domain ) )
{
LOG_ERROR( "Initialization failure" );
return EXIT_FAILURE;
}
std_prefix = sniffer.get_participant().guid().guidPrefix;
if( ! machine_readable && ! snapshot )
{
std::cout << "rs-dds-sniffer listening on domain " << domain;
std::cout << " (press Ctrl+C to stop)";
std::cout << std::endl;
}
if( ! snapshot )
{
// Wait until user presses Ctrl+C
std::cin.ignore( std::numeric_limits< std::streamsize >::max() );
}
else
{
// We need to allow enough time to pick up all participants, writers, etc. -- it takes time
std::this_thread::sleep_for( std::chrono::seconds( seconds ) );
}
if( participants )
{
sniffer.print_participants();
}
else if( snapshot )
{
if( machine_readable )
sniffer.print_topics_machine_readable();
else
sniffer.print_topics();
}
return EXIT_SUCCESS;
}
catch( const TCLAP::ExitException & )
{
LOG_ERROR( "Undefined exception while parsing command line arguments" );
return EXIT_FAILURE;
}
catch( const TCLAP::ArgException & e )
{
LOG_ERROR( e.what() );
return EXIT_FAILURE;
}
catch( const std::exception & e )
{
LOG_ERROR( e.what() );
return EXIT_FAILURE;
}
dds_sniffer::dds_sniffer()
: _participant()
, _reader_listener( _discovered_types_datas )
{
}
dds_sniffer::~dds_sniffer()
{
for( const auto & it : _discovered_types_readers )
{
DDS_API_CALL_NO_THROW( _discovered_types_subscriber->delete_datareader( it.first ) ); // If not empty than _discovered_types_subscriber != nullptr
DDS_API_CALL_NO_THROW( _participant.get()->delete_topic( it.second ) );
}
if( _discovered_types_subscriber != nullptr )
{
DDS_API_CALL_NO_THROW( _participant.get()->delete_subscriber( _discovered_types_subscriber ) );
}
_discovered_types_readers.clear();
_discovered_types_datas.clear();
}
static rsutils::json load_settings( rsutils::json const & local_settings )
{
// Load the realsense configuration file settings
std::string const filename = rsutils::os::get_special_folder( rsutils::os::special_folder::app_data ) + "realsense-config.json";
auto config = rsutils::json_config::load_from_file( filename );
// Take just the 'context' part
config = rsutils::json_config::load_settings( config, "context", "config-file" );
// Take the "dds" settings only
config = config.nested( "dds" );
// We should always have DDS enabled
if( config.is_object() )
config.erase( "enabled" );
// Patch the given local settings into the configuration
config.override( local_settings, "local settings" );
return config;
}
bool dds_sniffer::init( realdds::dds_domain_id domain )
{
// Set callbacks before calling _participant.init(), or some events, specifically on_participant_added, might get lost
_participant.create_listener( &_listener )
->on_writer_added( [this]( realdds::dds_guid guid, char const * topic_name ) {
on_writer_added( guid, topic_name );
} )
->on_writer_removed( [this]( realdds::dds_guid guid, char const * topic_name ) {
on_writer_removed( guid, topic_name );
} )
->on_reader_added( [this]( realdds::dds_guid guid, char const * topic_name ) {
on_reader_added( guid, topic_name );
} )
->on_reader_removed( [this]( realdds::dds_guid guid, char const * topic_name ) {
on_reader_removed( guid, topic_name );
} )
->on_participant_added( [this]( realdds::dds_guid guid, char const * participant_name ) {
on_participant_added( guid, participant_name );
} )
->on_participant_removed( [this]( realdds::dds_guid guid, char const * participant_name ) {
on_participant_removed( guid, participant_name );
} )
->on_type_discovery( [this]( char const * topic_name, DynamicType_ptr dyn_type ) {
on_type_discovery( topic_name, dyn_type );
} );
rsutils::json settings( rsutils::json::object() );
settings = load_settings( settings );
_participant.init( domain, rsutils::os::executable_name(), std::move( settings ) );
return _participant.is_valid();
}
static bool filter_topic( std::string const & topic, std::string const & root )
{
if( root.empty() )
return false;
if( 0 == strncmp( topic.data(), root.data(), root.length() ) )
return false;
if( 0 == strncmp( topic.data(), "rt/", 3 )
&& 0 == strncmp( topic.data() + 3, root.data(), root.length() ) )
return false;
return true;
}
void dds_sniffer::on_writer_added( realdds::dds_guid guid, const char * topic_name )
{
if( _print_discoveries )
{
print_writer_discovered( guid, topic_name, true );
}
save_topic_writer( guid, topic_name );
}
void dds_sniffer::on_writer_removed( realdds::dds_guid guid, const char * topic_name )
{
if( _print_discoveries )
{
print_writer_discovered( guid, topic_name, false );
}
remove_topic_writer( guid, topic_name );
}
void dds_sniffer::on_reader_added( realdds::dds_guid guid, const char * topic_name )
{
if( _print_discoveries )
{
print_reader_discovered( guid, topic_name, true );
}
save_topic_reader( guid, topic_name );
}
void dds_sniffer::on_reader_removed( realdds::dds_guid guid, const char * topic_name )
{
if( _print_discoveries )
{
print_reader_discovered( guid, topic_name, false );
}
remove_topic_reader( guid, topic_name );
}
void dds_sniffer::on_participant_added( realdds::dds_guid guid, const char * participant_name )
{
if( _print_discoveries )
{
print_participant_discovered( guid, participant_name, true );
}
std::lock_guard< std::mutex > lock( _dds_entities_lock );
_discovered_participants[guid] = participant_name;
}
void dds_sniffer::on_participant_removed( realdds::dds_guid guid, const char * participant_name )
{
if( _print_discoveries )
{
print_participant_discovered( guid, participant_name, false );
}
std::lock_guard< std::mutex > lock( _dds_entities_lock );
_discovered_participants.erase( guid );
}
void dds_sniffer::on_type_discovery( char const * topic_name, DynamicType_ptr dyn_type )
{
if( ! _print_by_topics )
{
// Register type with participant
TypeSupport type_support( DDS_API_CALL( new DynamicPubSubType( dyn_type ) ) );
DDS_API_CALL( type_support.register_type( _participant.get() ) );
std::cout << "Discovered topic '" << topic_name << "' of type '" << type_support->getName() << "'" << std::endl;
if( _print_topic_samples )
{
// Create subscriber, topic and reader to receive instances of this topic
if( _discovered_types_subscriber == nullptr )
{
_discovered_types_subscriber = DDS_API_CALL( _participant.get()->create_subscriber( SUBSCRIBER_QOS_DEFAULT,
nullptr ) );
if( _discovered_types_subscriber == nullptr )
{
LOG_ERROR( "Cannot create subscriber for discovered type '" << topic_name );
return;
}
}
Topic * topic = DDS_API_CALL( _participant.get()->create_topic( topic_name, type_support->getName(),
TOPIC_QOS_DEFAULT ) );
if( topic == nullptr )
{
LOG_ERROR( "Cannot create topic for discovered type '" << topic_name );
return;
}
StatusMask sub_mask = StatusMask::subscription_matched() << StatusMask::data_available();
DataReaderQos rqos = DATAREADER_QOS_DEFAULT;
rqos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
DataReader * reader = DDS_API_CALL( _discovered_types_subscriber->create_datareader( topic,
rqos,
&_reader_listener,
sub_mask ) );
if( reader == nullptr )
{
LOG_ERROR( "Cannot create reader for discovered type '" << topic_name );
DDS_API_CALL( _participant.get()->delete_topic( topic ) );
return;
}
_discovered_types_readers[reader] = topic;
DynamicData_ptr data( DDS_API_CALL( DynamicDataFactory::get_instance()->create_data( dyn_type ) ) );
_discovered_types_datas[reader] = data;
}
}
}
dds_sniffer::dds_reader_listener::dds_reader_listener( std::map< DataReader *, DynamicData_ptr > & datas )
: _datas( datas )
{
}
void dds_sniffer::dds_reader_listener::on_data_available( DataReader * reader )
{
const TopicDescription * topic_desc = DDS_API_CALL( reader->get_topicdescription() );
std::cout << "Received topic " << topic_desc->get_name() << " of type "
<< topic_desc->get_type_name() << std::endl;
auto dit = _datas.find( reader );
if( dit != _datas.end() )
{
DynamicData_ptr data = dit->second;
SampleInfo info;
if( DDS_API_CALL( reader->take_next_sample( data.get(), &info ) ) == ReturnCode_t::RETCODE_OK )
{
if( info.valid_data )
{
DynamicDataHelper::print( data );
}
}
}
}
void dds_sniffer::dds_reader_listener::on_subscription_matched( DataReader * reader, const SubscriptionMatchedStatus & info )
{
if( info.current_count_change == 1 )
{
LOG_DEBUG( "Subscriber matched by reader " << print_guid( reader->guid() ) );
}
else if( info.current_count_change == -1 )
{
LOG_DEBUG( "Subscriber unmatched by reader " << print_guid( reader->guid() ) );
}
else
{
LOG_ERROR( info.current_count_change << " is not a valid value for SubscriptionMatchedStatus current count change" );
}
}
void dds_sniffer::save_topic_writer( realdds::dds_guid guid, const char * topic_name )
{
std::lock_guard< std::mutex > lock( _dds_entities_lock );
_topics_info_by_name[topic_name].writers.insert( guid );
}
void dds_sniffer::remove_topic_writer( realdds::dds_guid guid, const char * topic_name )
{
std::lock_guard< std::mutex > lock( _dds_entities_lock );
auto topic_entry = _topics_info_by_name.find( topic_name );
if( topic_entry != _topics_info_by_name.end() )
{
topic_entry->second.writers.erase( guid );
if( topic_entry->second.writers.empty() && topic_entry->second.readers.empty() )
{
_topics_info_by_name.erase( topic_entry );
}
}
}
void dds_sniffer::save_topic_reader( realdds::dds_guid guid, const char * topic_name )
{
std::lock_guard< std::mutex > lock( _dds_entities_lock );
_topics_info_by_name[topic_name].readers.insert( guid );
}
void dds_sniffer::remove_topic_reader( realdds::dds_guid guid, const char * topic_name )
{
std::lock_guard< std::mutex > lock( _dds_entities_lock );
auto topic_entry = _topics_info_by_name.find( topic_name );
if( topic_entry != _topics_info_by_name.end() )
{
topic_entry->second.readers.erase( guid );
if( topic_entry->second.writers.empty() && topic_entry->second.readers.empty() )
{
_topics_info_by_name.erase( topic_entry );
}
}
}
uint32_t dds_sniffer::calc_max_indentation() const
{
uint32_t indentation = 0;
uint32_t max_indentation = 0;
for( auto & topic : _topics_info_by_name ) //_dds_entities_lock locked by print_topics()
{
if( filter_topic( topic.first, _root ) )
continue;
// Use / as delimiter for nested topic names
indentation = static_cast< uint32_t >( std::count( topic.first.begin(), topic.first.end(), '/' ) );
if( indentation >= max_indentation )
{
max_indentation = indentation + 1; //+1 for Reader/Writer indentation
}
}
return max_indentation;
}
void dds_sniffer::print_writer_discovered( realdds::dds_guid guid,
const char * topic_name,
bool discovered ) const
{
if( filter_topic( topic_name, _root ) )
return;
if( _print_machine_readable )
{
std::cout << "DataWriter," << print_guid( guid ) << "," << topic_name
<< ( discovered ? ",discovered" : ",removed" ) << std::endl;
}
else
{
std::cout << "DataWriter " << print_guid( guid ) << " publishing topic '" << topic_name
<< ( discovered ? "' discovered" : "' removed" ) << std::endl;
}
}
void dds_sniffer::print_reader_discovered( realdds::dds_guid guid,
const char * topic_name,
bool discovered ) const
{
if( filter_topic( topic_name, _root ) )
return;
if( _print_machine_readable )
{
std::cout << "DataReader," << print_guid( guid ) << "," << topic_name
<< ( discovered ? ",discovered" : ",removed" ) << std::endl;
}
else
{
std::cout << "DataReader " << print_guid( guid ) << " reading topic '" << topic_name
<< ( discovered ? "' discovered" : "' removed" ) << std::endl;
}
}
void dds_sniffer::print_participant_discovered( realdds::dds_guid guid,
const char * participant_name,
bool discovered ) const
{
if( _print_machine_readable )
{
std::cout << "Participant," << print_guid( guid ) << "," << participant_name
<< ( discovered ? ",discovered" : ",removed" )
<< std::endl;
}
else
{
//prefix_.value[4] = static_cast<octet>( pid & 0xFF );
//prefix_.value[5] = static_cast<octet>( ( pid >> 8 ) & 0xFF );
uint16_t pid
= guid.guidPrefix.value[GUID_PROCESS_LOCATION] + ( guid.guidPrefix.value[GUID_PROCESS_LOCATION + 1] << 8 );
std::cout << "Participant " << print_guid( guid ) << " '" << participant_name << "' (" << std::hex << pid
<< std::dec << ") " << ( discovered ? " discovered" : " removed" ) << std::endl;
}
}
void dds_sniffer::print_topics_machine_readable() const
{
std::lock_guard< std::mutex > lock( _dds_entities_lock );
for( auto & topic : _topics_info_by_name )
{
if( filter_topic( topic.first, _root ) )
continue;
for( auto & writer : topic.second.writers )
{
std::cout << topic.first << ",";
print_topic_writer( writer );
}
for( auto & reader : topic.second.readers )
{
std::cout << topic.first << ",";
print_topic_reader( reader );
}
}
}
void dds_sniffer::print_topics() const
{
std::istringstream last_topic( "" );
std::string last_topic_nested;
std::lock_guard< std::mutex > lock( _dds_entities_lock );
uint32_t max_indentation( calc_max_indentation() );
for( auto & topic : _topics_info_by_name )
{
if( filter_topic( topic.first, _root ) )
continue;
std::cout << std::endl;
std::istringstream current_topic( topic.first ); // Get topic name
std::string current_topic_nested;
uint32_t indentation = 0;
// Compare to previous topic
while( std::getline( last_topic, last_topic_nested, '/' ) ) // Use / as delimiter for nested topic names
{
if( std::getline( current_topic, current_topic_nested, '/' ) )
{
if( current_topic_nested.compare( last_topic_nested ) == 0 )
{
++indentation; // Skip parts that are same as previous topic
}
else
{
ident( indentation );
std::cout << current_topic_nested << std::endl;
++indentation;
break;
}
}
}
// Print remainder of string
while( std::getline( current_topic, current_topic_nested, '/' ) )
{
ident( indentation );
std::cout << current_topic_nested << std::endl;
++indentation;
}
for( auto & writer : topic.second.writers )
{
print_topic_writer( writer, max_indentation );
}
for( auto & reader : topic.second.readers )
{
print_topic_reader( reader, max_indentation );
}
last_topic.clear();
last_topic.str( topic.first ); // Save topic name for next iteration
last_topic.seekg( 0, last_topic.beg );
}
}
void dds_sniffer::print_participants( bool with_guids ) const
{
for( auto & guid2name : _discovered_participants )
{
std::cout << guid2name.second;
if( with_guids )
std::cout << ' ' << realdds::print_raw_guid( guid2name.first );
std::cout << std::endl;
}
}
void dds_sniffer::ident( uint32_t indentation ) const
{
while( indentation > 0 )
{
std::cout << " ";
--indentation;
}
std::cout << "- ";
}
void dds_sniffer::print_topic_writer( realdds::dds_guid guid, uint32_t indentation ) const
{
auto iter = _discovered_participants.begin();
for( ; iter != _discovered_participants.end(); ++iter ) //_dds_entities_lock locked by caller
{
if( iter->first.guidPrefix == guid.guidPrefix )
{
if( _print_machine_readable )
{
std::cout << "Writer," << realdds::dds_participant::name_from_guid( iter->first ) << std::endl;
}
else
{
ident( indentation );
std::cout << "Writer of \"" << realdds::dds_participant::name_from_guid( iter->first ) << "\"" << std::endl;
}
break;
}
}
if( iter == _discovered_participants.end() )
{
ident( indentation );
std::cout << "Writer of unknown participant" << std::endl;
}
}
void dds_sniffer::print_topic_reader( realdds::dds_guid guid, uint32_t indentation ) const
{
auto iter = _discovered_participants.begin();
for( ; iter != _discovered_participants.end(); ++iter ) //_dds_entities_lock locked by caller
{
if( iter->first.guidPrefix == guid.guidPrefix )
{
if( _print_machine_readable )
{
std::cout << "Reader," << realdds::dds_participant::name_from_guid( iter->first ) << std::endl;
}
else
{
ident( indentation );
std::cout << "Reader of \"" << realdds::dds_participant::name_from_guid( iter->first ) << "\"" << std::endl;
}
break;
}
}
if( iter == _discovered_participants.end() )
{
ident( indentation );
std::cout << "Reader of unknown participant" << std::endl;
}
}