diff --git a/third-party/realdds/include/realdds/dds-guid.h b/third-party/realdds/include/realdds/dds-guid.h index 82c90a3b32..c69f7271b8 100644 --- a/third-party/realdds/include/realdds/dds-guid.h +++ b/third-party/realdds/include/realdds/dds-guid.h @@ -5,6 +5,7 @@ #include "dds-defines.h" #include +#include namespace realdds { @@ -13,25 +14,77 @@ namespace realdds { static constexpr auto & unknown_guid = eprosima::fastrtps::rtps::c_Guid_Unknown; -// Custom GUID printer: attempts a more succinct representation -// If a base_prefix is provided, will try to minimize a common denominator (vendor, host, etc.) -- you can use your -// participant's guid if you want to shorten +// Convert a prefix to a hex string: not human-readable! +// In one of two formats: +// 223344556677.<0xPID> // eProsima +// Or: +// 001122334455667788990011 // everything else +// Used internally by next function. // -std::string print( dds_guid const & guid, - dds_guid_prefix const & base_prefix = unknown_guid.guidPrefix, - bool readable_name = true ); +struct print_raw_guid_prefix +{ + dds_guid_prefix const & _prefix; + explicit print_raw_guid_prefix( dds_guid_prefix const & prefix, + dds_guid_prefix const & /*base_prefix*/ = unknown_guid.guidPrefix ) + : _prefix( prefix ) + { + } +}; +std::ostream & operator<<( std::ostream &, print_raw_guid_prefix const & ); + -// Same as above, without a base prefix -inline std::string print( dds_guid const & guid, bool readable_name ) +// Custom GUID printer: attempts a more succinct representation: +// If the participant is known, its name will be shown instead of the raw bytes. +// . +// If a base_prefix is provided, will try to minimize a common denominator -- you can use your +// participant's guid if you want to shorten. +// +struct print_guid { - return print( guid, unknown_guid.guidPrefix, readable_name ); -} + dds_guid const & _guid; + dds_guid_prefix const & _base_prefix; + + explicit print_guid( dds_guid const & guid, dds_guid_prefix const & base_prefix = unknown_guid.guidPrefix ) + : _guid( guid ) + , _base_prefix( base_prefix ) + { + } + explicit print_guid( dds_guid const & guid, dds_guid const & base_guid ) + : print_guid( guid, base_guid.guidPrefix ) + { + } +}; +std::ostream & operator<<( std::ostream &, print_guid const & ); + -// Same as above, with a guid base for flexibility -inline std::string print( dds_guid const & guid, dds_guid const & base_guid, bool readable_name = true ) +// Same, except leaves output in raw form (bytes, not name) +// . +// +struct print_raw_guid { - return print( guid, base_guid.guidPrefix, readable_name ); -} + dds_guid const & _guid; + dds_guid_prefix const & _base_prefix; + + explicit print_raw_guid( dds_guid const & guid, dds_guid_prefix const & base_prefix = unknown_guid.guidPrefix ) + : _guid( guid ) + , _base_prefix( base_prefix ) + { + } + explicit print_raw_guid( dds_guid const & guid, dds_guid const & base_guid ) + : print_raw_guid( guid, base_guid.guidPrefix ) + { + } +}; +std::ostream & operator<<( std::ostream &, print_raw_guid const & ); + + +// The reverse: get a guid from a RAW guid string. +// Expecting one of two formats: +// 223344556677.<0xPID>.<0xEID> // eProsima +// Or: +// 001122334455667788990011.<0xEID> // everything else +// Returns unknown_guid if not parseable +dds_guid guid_from_string( std::string const & ); } // namespace realdds diff --git a/third-party/realdds/include/realdds/dds-topic-writer.h b/third-party/realdds/include/realdds/dds-topic-writer.h index 0c374756ea..262fe931e1 100644 --- a/third-party/realdds/include/realdds/dds-topic-writer.h +++ b/third-party/realdds/include/realdds/dds-topic-writer.h @@ -80,6 +80,9 @@ class dds_topic_writer : protected eprosima::fastdds::dds::DataWriterListener // The callbacks should be set before we actually create the underlying DDS objects, so the writer does not void run( qos const & = qos() ); + // Waits until all changes were acknowledged; return false on timeout + bool wait_for_acks( dds_time timeout ); + // DataWriterListener protected: // Called when the Publisher is matched (or unmatched) against an endpoint diff --git a/third-party/realdds/py/pyrealdds.cpp b/third-party/realdds/py/pyrealdds.cpp index 5ee27c002f..7dd61e4c33 100644 --- a/third-party/realdds/py/pyrealdds.cpp +++ b/third-party/realdds/py/pyrealdds.cpp @@ -29,7 +29,11 @@ #include #include +#include +#include #include +#include +#include #include #include @@ -45,12 +49,6 @@ namespace { -std::string to_string( realdds::dds_guid const & guid ) -{ - return realdds::print( guid ); -} - - py::list get_vector3( geometry_msgs::msg::Vector3 const & v ) { py::list obj( 3 ); @@ -69,6 +67,55 @@ void set_vector3( geometry_msgs::msg::Vector3 & v, std::array< double, 3 > const } +std::string script_name() +{ + // Returns the name of the python script that's currently running us + return rsutils::os::base_name( + py::module_::import( "__main__" ).attr( "__file__" ).cast< std::string >() ); +} + + +nlohmann::json load_rs_settings( nlohmann::json const & local_settings ) +{ + nlohmann::json config; + + // Load the realsense configuration file settings + std::ifstream f( rsutils::os::get_special_folder( rsutils::os::special_folder::app_data ) + "realsense-config.json" ); + if( f.good() ) + { + try + { + config = nlohmann::json::parse( f ); + } + catch( std::exception const & e ) + { + throw std::runtime_error( "failed to load configuration file: " + std::string( e.what() ) ); + } + } + + // Load "python"-specific settings + auto settings = rsutils::json::load_app_settings( config, "python", "context", "config-file" ); + + // Take the "dds" settings only + settings = rsutils::json::nested( settings, "dds" ); + + // Patch any script-specific settings + // NOTE: this is also accessed by pyrealsense2, where a "context" hierarchy is still used + auto script = script_name(); + if( auto script_settings = rsutils::json::nested( config, script, "context", "dds" ) ) + rsutils::json::patch( settings, script_settings, "config-file/" + script + "/context" ); + + // We should always have DDS enabled + if( settings.is_object() ) + settings.erase( "enabled" ); + + // Patch the given local settings into the configuration + rsutils::json::patch( settings, local_settings, "local settings" ); + + return settings; +} + + } // namespace @@ -94,14 +141,19 @@ PYBIND11_MODULE(NAME, m) { using realdds::dds_guid; py::class_< dds_guid >( m, "guid" ) .def( py::init<>() ) - .def( "__bool__", []( dds_guid const& self ) { return self != dds_guid::unknown(); } ) - .def( "__repr__", []( dds_guid const & self ) { return to_string( self ); } ) + .def_static( "from_string", + []( std::string const & raw_guid ) { return realdds::guid_from_string( raw_guid ); } ) + .def( "__bool__", []( dds_guid const & self ) { return self != realdds::unknown_guid; } ) + .def( "__str__", + []( dds_guid const & self ) { return rsutils::string::from( realdds::print_guid( (self) ) ).str(); } ) + .def( "__repr__", + []( dds_guid const & self ) { return rsutils::string::from( realdds::print_raw_guid( ( self ) ) ).str(); } ) // Following two (hash and ==) are needed if we want to be able to use guids as dictionary keys .def( "__hash__", []( dds_guid const & self ) { return std::hash< std::string >{}( - realdds::print( self, false ) ); // use hex; not the human-readable name + rsutils::string::from( realdds::print_raw_guid( self ) ) ); } ) .def( py::self == py::self ); @@ -149,12 +201,19 @@ PYBIND11_MODULE(NAME, m) { ( char const * topic_name, eprosima::fastrtps::types::DynamicType_ptr dyn_type ), callback( topic_name, dyn_type->get_name() ); ) ); + m.def( "load_rs_settings", &load_rs_settings, "local-settings"_a = nlohmann::json::object() ); + m.def( "script_name", &script_name ); + py::class_< dds_participant, std::shared_ptr< dds_participant > // handled with a shared_ptr > participant( m, "participant" ); participant.def( py::init<>() ) - .def( "init", &dds_participant::init, "domain-id"_a, "participant-name"_a, "settings"_a = nlohmann::json::object() ) + .def( "init", + []( dds_participant & self, nlohmann::json const & local_settings, realdds::dds_domain_id domain_id ) + { self.init( domain_id, script_name(), local_settings ); }, + "local-settings"_a = nlohmann::json::object(), "domain-id"_a = -1 ) + .def( "init", &dds_participant::init, "domain-id"_a, "participant-name"_a, "local-settings"_a = nlohmann::json::object() ) .def( "is_valid", &dds_participant::is_valid ) .def( "guid", &dds_participant::guid ) .def( "create_guid", &dds_participant::create_guid ) @@ -182,7 +241,7 @@ PYBIND11_MODULE(NAME, m) { eprosima::fastdds::dds::DomainParticipantQos qos; if( ReturnCode_t::RETCODE_OK == self.get()->get_qos( qos ) ) os << " \"" << qos.name() << "\""; - os << " " << to_string( self.guid() ); + os << " " << realdds::print_guid( self.guid() ); } os << ">"; return os.str(); @@ -298,8 +357,10 @@ PYBIND11_MODULE(NAME, m) { callback( self, status.current_count_change ); ) ) .def( "topic", &dds_topic_writer::topic ) .def( "run", &dds_topic_writer::run ) - .def( "qos", []() { return writer_qos(); } ) - .def( "qos", []( reliability r, durability d ) { return writer_qos( r, d ); } ); + .def( "has_readers", &dds_topic_writer::has_readers ) + .def( "wait_for_acks", &dds_topic_writer::wait_for_acks ) + .def_static( "qos", []() { return writer_qos(); } ) + .def_static( "qos", []( reliability r, durability d ) { return writer_qos( r, d ); } ); // The actual types are declared as functions and not classes: the py::init<> inheritance rules are pretty strict @@ -350,7 +411,7 @@ PYBIND11_MODULE(NAME, m) { []( SampleIdentity const & self ) { std::ostringstream os; - os << to_string( self.writer_guid() ); + os << realdds::print_guid( self.writer_guid() ); os << '.'; os << self.sequence_number(); return os.str(); diff --git a/third-party/realdds/scripts/devices.py b/third-party/realdds/scripts/devices.py index 62ab8ef337..5870a18f67 100644 --- a/third-party/realdds/scripts/devices.py +++ b/third-party/realdds/scripts/devices.py @@ -16,7 +16,7 @@ def domain_arg(x): if t <= 0 or t > 232: raise ValueError( f'--domain should be [0,232]' ) return t -args.add_argument( '--domain', metavar='<0-232>', type=domain_arg, default=0, help='DDS domain to use (default=0)' ) +args.add_argument( '--domain', metavar='<0-232>', type=domain_arg, default=-1, help='DDS domain to use (default=0)' ) args = args.parse_args() @@ -36,7 +36,7 @@ def e( *a, **kw ): dds.debug( args.debug ) participant = dds.participant() -participant.init( args.domain, 'devices' ) +participant.init( dds.load_rs_settings( settings ), args.domain ) watcher = dds.device_watcher( participant ) watcher.start() diff --git a/third-party/realdds/scripts/fps.py b/third-party/realdds/scripts/fps.py index 9f0a685a12..36e8f9577d 100644 --- a/third-party/realdds/scripts/fps.py +++ b/third-party/realdds/scripts/fps.py @@ -17,7 +17,7 @@ def domain_arg(x): if t <= 0 or t > 232: raise ValueError( f'--domain should be [0,232]' ) return t -args.add_argument( '--domain', metavar='<0-232>', type=domain_arg, default=0, help='DDS domain to use (default=0)' ) +args.add_argument( '--domain', metavar='<0-232>', type=domain_arg, default=-1, help='DDS domain to use (default=0)' ) args.add_argument( '--with-metadata', action='store_true', help='stream with metadata, if available (default off)' ) args = args.parse_args() @@ -43,7 +43,7 @@ def e( *a, **kw ): settings['device'] = { 'metadata' : False }; participant = dds.participant() -participant.init( args.domain, 'fps', settings ) +participant.init( dds.load_rs_settings( settings ), args.domain ) # Most important is the topic-root: this assumes we know it in advance and do not have to # wait for a device-info message (which would complicate the code here). diff --git a/third-party/realdds/scripts/topic-send.py b/third-party/realdds/scripts/topic-send.py index a73a92abc3..1efe5c3c83 100644 --- a/third-party/realdds/scripts/topic-send.py +++ b/third-party/realdds/scripts/topic-send.py @@ -15,12 +15,13 @@ def json_arg(x): except Exception as e: raise ArgumentError( str(e) ) args.add_argument( '--message', metavar='', type=json_arg, help='a message to send', default='{"id":"ping","message":"some message"}' ) +args.add_argument( '--ack', action='store_true', help='wait for acks' ) def domain_arg(x): t = int(x) if t <= 0 or t > 232: raise ArgumentError( f'--domain should be [0-232]' ) return t -args.add_argument( '--domain', metavar='<0-232>', type=domain_arg, default=0, help='DDS domain to use (default=0)' ) +args.add_argument( '--domain', metavar='<0-232>', type=domain_arg, default=-1, help='DDS domain to use (default=0)' ) args = args.parse_args() @@ -43,7 +44,7 @@ def e( *a, **kw ): settings = {} participant = dds.participant() -participant.init( args.domain, 'topic-send', settings ) +participant.init( dds.load_rs_settings( settings ), args.domain ) message = args.message @@ -78,7 +79,16 @@ def e( *a, **kw ): writer.run( dds.topic_writer.qos() ) # Let the client pick up on the new entity - if we send it too quickly, they won't see it before we disappear... time.sleep( 1 ) + if not writer.has_readers(): + e( 'No readers exist on topic:', topic_path ) + sys.exit( 1 ) + start = dds.now() dds.message.flexible( message ).write_to( writer ) i( f'Sent {message} on {topic_path}' ) + if args.ack: + if not writer.wait_for_acks( dds.time( 5. ) ): # seconds + e( 'Timeout waiting for ack' ) + sys.exit( 1 ) + i( f'Acknowledged ({dds.timestr( dds.now(), start )})' ) diff --git a/third-party/realdds/src/dds-device-impl.cpp b/third-party/realdds/src/dds-device-impl.cpp index b772de7af1..6a6a879eee 100644 --- a/third-party/realdds/src/dds-device-impl.cpp +++ b/third-party/realdds/src/dds-device-impl.cpp @@ -201,9 +201,9 @@ void dds_device::impl::handle_notification( nlohmann::json const & j, if( sample.size() == 2 && sample.is_array() ) { // We have to be the ones who sent the control! - auto const guid_string = rsutils::json::get< std::string >( sample, 0 ); - auto const control_guid_string = realdds::print( _control_writer->get()->guid(), false ); // raw guid - if( guid_string == control_guid_string ) + auto const reply_guid = guid_from_string( rsutils::json::get< std::string >( sample, 0 ) ); + auto const control_guid = _control_writer->get()->guid(); + if( reply_guid == control_guid ) { auto const sequence_number = rsutils::json::get< uint64_t >( sample, 1 ); std::unique_lock< std::mutex > lock( _replies_mutex ); diff --git a/third-party/realdds/src/dds-device-server.cpp b/third-party/realdds/src/dds-device-server.cpp index 1a5a7dc2cb..4749e6f6b2 100644 --- a/third-party/realdds/src/dds-device-server.cpp +++ b/third-party/realdds/src/dds-device-server.cpp @@ -282,7 +282,7 @@ void dds_device_server::on_control_message_received() { json reply; reply[sample_key] = json::array( { - realdds::print( sample.sample_identity.writer_guid(), false ), // raw guid + rsutils::string::from( realdds::print_raw_guid( sample.sample_identity.writer_guid() ) ), sample.sample_identity.sequence_number().to64long(), } ); try diff --git a/third-party/realdds/src/dds-device-watcher.cpp b/third-party/realdds/src/dds-device-watcher.cpp index 937950b34f..9fef65c9a1 100644 --- a/third-party/realdds/src/dds-device-watcher.cpp +++ b/third-party/realdds/src/dds-device-watcher.cpp @@ -110,7 +110,7 @@ void dds_device_watcher::start() init(); } LOG_DEBUG( "DDS device watcher started on '" << _participant->get()->get_qos().name() << "' " - << realdds::print( _participant->guid() ) ); + << realdds::print_guid( _participant->guid() ) ); } void dds_device_watcher::stop() diff --git a/third-party/realdds/src/dds-guid.cpp b/third-party/realdds/src/dds-guid.cpp index cbecce5af2..a4e6bd186a 100644 --- a/third-party/realdds/src/dds-guid.cpp +++ b/third-party/realdds/src/dds-guid.cpp @@ -3,12 +3,75 @@ #include #include +#include +#include #include +#include +#include +#include namespace realdds { +std::ostream & operator<<( std::ostream & os, print_raw_guid_prefix const & g ) +{ + if( eprosima::fastdds::rtps::c_VendorId_eProsima[0] == g._prefix.value[0] + && eprosima::fastdds::rtps::c_VendorId_eProsima[1] == g._prefix.value[1] ) + { + // For eProsima, use a more readable (and usually, shorter) representation or host.pid.eid + // bytes 2-7 are the host info, 8-11 the participant ID + os << rsutils::string::hexdump( g._prefix.value, g._prefix.size ).format( "{+2}{6}.{04}" ); + } + else + { + // Otherwise, just dump the 24-character hex string + os << rsutils::string::hexdump( g._prefix.value, g._prefix.size ); + } + return os; +} + + +std::ostream & operator<<( std::ostream & os, print_raw_guid const & g ) +{ + if( g._guid != unknown_guid ) + { + if( g._guid.guidPrefix != g._base_prefix ) + os << print_raw_guid_prefix( g._guid.guidPrefix, g._base_prefix ); + os << '.'; // FastDDS uses '|' + os << rsutils::string::in_hex( g._guid.entityId.to_uint32() ); + } + else + { + os << "|GUID UNKNOWN|"; // same as in FastDDS + } + return os; +} + + +std::ostream & operator<<( std::ostream & os, print_guid const & g ) +{ + if( g._guid != unknown_guid ) + { + if( g._guid.guidPrefix != g._base_prefix ) + { + std::string participant = dds_participant::name_from_guid( g._guid ); + if( ! participant.empty() ) + os << participant; + else + os << print_raw_guid_prefix( g._guid.guidPrefix, g._base_prefix ); + } + os << '.'; // FastDDS uses '|' + os << rsutils::string::in_hex( g._guid.entityId.to_uint32() ); + } + else + { + os << "|GUID UNKNOWN|"; // same as in FastDDS + } + return os; +} + + std::string print( dds_guid const & guid, dds_guid_prefix const & base_prefix, bool readable_name ) @@ -27,23 +90,10 @@ std::string print( dds_guid const & guid, } else { - output << std::hex; - char old_fill = output.fill( '0' ); - - size_t i = 0; - if( base_prefix != unknown_guid.guidPrefix ) - { - while( i < base_prefix.size && guid.guidPrefix.value[i] == base_prefix.value[i] ) - ++i; - } - for( ; i < guid.guidPrefix.size; ++i ) - output << std::setw( 2 ) << +guid.guidPrefix.value[i]; // << "."; - - output.fill( old_fill ); - output << std::dec; + output << print_raw_guid_prefix( guid.guidPrefix, base_prefix ); } - output << '.'; // between prefix and entity, DDS uses '|' } + output << '.'; // FastDDS uses '|' output << std::hex << guid.entityId.to_uint32() << std::dec; } else @@ -54,4 +104,77 @@ std::string print( dds_guid const & guid, } +static char const * hex_to_uint32( char const * pch, uint32_t * pv ) +{ + // strtoul accepts leading spaces or a minus sign + if( ! std::isxdigit( *pch ) ) + return nullptr; + + auto v = std::strtoul( pch, const_cast< char ** >( &pch ), 16 ); + if( v > std::numeric_limits< uint32_t >::max() ) + return nullptr; + + *pv = static_cast< uint32_t >( v ); + return pch; +} + + +dds_guid guid_from_string( std::string const & s ) +{ + // Expecting one of two formats: + // 223344556677.<0xPID>.<0xEID> // eProsima (first two bytes 010f) + // Or: + // 001122334455667788990011.<0xEID> // everything else + if( s.length() < 16 ) + return dds_guid(); + auto period = s.find( '.' ); + if( period == std::string::npos ) + return dds_guid(); + char const * pch = s.data(); + dds_guid guid; + uint8_t * pb = guid.guidPrefix.value; + if( period == 12 ) + { + // This format is for eProsima + *pb++ = eprosima::fastdds::rtps::c_VendorId_eProsima[0]; + *pb++ = eprosima::fastdds::rtps::c_VendorId_eProsima[1]; + } + else if( period != 24 ) + return dds_guid(); + + pb = rsutils::string::hex_to_bytes( pch, pch + period, pb ); + if( ! pb ) + return dds_guid(); + + uint32_t participant_or_entity_id; + pch = hex_to_uint32( pch + period + 1, &participant_or_entity_id ); + if( ! pch ) + return dds_guid(); + + if( ! *pch ) + { + if( period != 24 ) + return dds_guid(); + guid.entityId = participant_or_entity_id; + } + else + { + if( *pch != '.' ) + return dds_guid(); + + // little-endian; same as in GuidUtils::guid_prefix_create + *pb++ = static_cast< uint8_t >( participant_or_entity_id & 0xFF ); + *pb++ = static_cast< uint8_t >( ( participant_or_entity_id >> 8 ) & 0xFF ); + *pb++ = static_cast< uint8_t >( ( participant_or_entity_id >> 16 ) & 0xFF ); + *pb++ = static_cast< uint8_t >( ( participant_or_entity_id >> 24 ) & 0xFF ); + + pch = hex_to_uint32( pch + 1, &participant_or_entity_id ); + if( ! pch || *pch ) + return dds_guid(); + guid.entityId = participant_or_entity_id; + } + return guid; +} + + } // namespace realdds diff --git a/third-party/realdds/src/dds-participant.cpp b/third-party/realdds/src/dds-participant.cpp index 9f858f6c75..f286228787 100644 --- a/third-party/realdds/src/dds-participant.cpp +++ b/third-party/realdds/src/dds-participant.cpp @@ -103,7 +103,7 @@ struct dds_participant::listener_impl : public eprosima::fastdds::dds::DomainPar { case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DISCOVERED_PARTICIPANT: { std::string name; - std::string guid = realdds::print( info.info.m_guid ); // has to be outside the mutex + std::string guid = rsutils::string::from( realdds::print_guid( info.info.m_guid ) ); // has to be outside the mutex { std::lock_guard< std::mutex > lock( participants_mutex ); participant_info pinfo( info.info.m_participantName, info.info.m_guid.guidPrefix ); @@ -246,7 +246,7 @@ void dds_participant::init( dds_domain_id domain_id, std::string const & partici DDS_THROW( runtime_error, "provided settings are invalid" ); _settings = settings; - LOG_DEBUG( "participant '" << participant_name << "' (" << realdds::print( guid() ) << ") is up on domain " + LOG_DEBUG( "participant '" << participant_name << "' (" << realdds::print_guid( guid() ) << ") is up on domain " << domain_id << " with settings: " << _settings.dump( 4 ) ); #ifdef BUILD_EASYLOGGINGPP // DDS participant destruction happens when all contexts are done with it but, in some situations (e.g., Python), it @@ -294,7 +294,7 @@ rsutils::string::slice dds_participant::name() const std::string dds_participant::print( dds_guid const & guid_to_print ) const { - return realdds::print( guid_to_print, guid() ); + return rsutils::string::from( realdds::print_guid( guid_to_print, guid() ) ); } diff --git a/third-party/realdds/src/dds-topic-writer.cpp b/third-party/realdds/src/dds-topic-writer.cpp index e3381f4a1c..62722412bb 100644 --- a/third-party/realdds/src/dds-topic-writer.cpp +++ b/third-party/realdds/src/dds-topic-writer.cpp @@ -109,6 +109,12 @@ void dds_topic_writer::run( qos const & wqos ) } +bool dds_topic_writer::wait_for_acks( dds_time timeout ) +{ + return !! _writer->wait_for_acknowledgments( timeout ); +} + + void dds_topic_writer::on_publication_matched( eprosima::fastdds::dds::DataWriter *, eprosima::fastdds::dds::PublicationMatchedStatus const & info ) { diff --git a/third-party/rsutils/include/rsutils/os/executable-name.h b/third-party/rsutils/include/rsutils/os/executable-name.h index 1a99816154..c52617923f 100644 --- a/third-party/rsutils/include/rsutils/os/executable-name.h +++ b/third-party/rsutils/include/rsutils/os/executable-name.h @@ -12,8 +12,15 @@ namespace os { // Returns the path to the executable currently running std::string executable_path(); + +// Returns the filename component of the given path, with or without the extension +std::string base_name( std::string path, bool with_extension = false ); + // Returns the filename component of the executable currently running -std::string executable_name( bool with_extension = false ); +inline std::string executable_name( bool with_extension = false ) +{ + return base_name( executable_path(), with_extension ); +} } diff --git a/third-party/rsutils/include/rsutils/string/hexarray.h b/third-party/rsutils/include/rsutils/string/hexarray.h index d7493cf932..9a5f45dc3c 100644 --- a/third-party/rsutils/include/rsutils/string/hexarray.h +++ b/third-party/rsutils/include/rsutils/string/hexarray.h @@ -78,5 +78,12 @@ void from_json( nlohmann::json const &, hexarray & ); // See https://github.com/nlohmann/json#arbitrary-types-conversions +// Utility function for parsing hexarray strings to the corresponding bytes. +// Output in 'pb'. +// Returns the new 'pb', or null if an invalid character was encountered; +// +uint8_t * hex_to_bytes( char const * start, char const * end, uint8_t * pb ); + + } // namespace string } // namespace rsutils diff --git a/third-party/rsutils/include/rsutils/string/hexdump.h b/third-party/rsutils/include/rsutils/string/hexdump.h index c8e5fd8f1f..36fd4571f5 100644 --- a/third-party/rsutils/include/rsutils/string/hexdump.h +++ b/third-party/rsutils/include/rsutils/string/hexdump.h @@ -4,7 +4,7 @@ #pragma once #include -#include +#include #include @@ -17,6 +17,9 @@ namespace string { // E.g.: // std::ostringstream ss; // ss << hexdump( this ); // output pointer to this object as a hex value +// +// If you don't want a memory dump which may involve extra leading 0s but instead just to write a number in hex, use +// in_hex() below. // struct hexdump { @@ -138,5 +141,32 @@ std::ostream & operator<<( std::ostream & os, hexdump const & ); std::ostream & operator<<( std::ostream & os, hexdump::_format const & ); +// Write something out in hex without affecting the stream flags: +// +template< class T > +struct _in_hex +{ + T const & _t; + + _in_hex( T const & t ) + : _t( t ) + { + } +}; +template< class T > +inline std::ostream & operator<<( std::ostream & os, _in_hex< T > const & h ) +{ + auto flags = os.flags(); + os << std::hex << h._t; + os.flags( flags ); + return os; +} +template< class T > +inline _in_hex< T > in_hex( T const & t ) +{ + return _in_hex< T >( t ); +} + + } // namespace string } // namespace rsutils diff --git a/third-party/rsutils/src/executable-name.cpp b/third-party/rsutils/src/executable-name.cpp index db0d456f56..7e8cb28bb4 100644 --- a/third-party/rsutils/src/executable-name.cpp +++ b/third-party/rsutils/src/executable-name.cpp @@ -48,9 +48,8 @@ std::string executable_path() } -std::string executable_name( bool with_extension ) +std::string base_name( std::string path, bool with_extension ) { - auto path = executable_path(); auto sep = path.find_last_of( "/\\" ); if( sep != std::string::npos ) path = path.substr( sep + 1 ); diff --git a/third-party/rsutils/src/hexarray.cpp b/third-party/rsutils/src/hexarray.cpp index c0b4129b7f..85f584426f 100644 --- a/third-party/rsutils/src/hexarray.cpp +++ b/third-party/rsutils/src/hexarray.cpp @@ -54,34 +54,46 @@ void from_json( nlohmann::json const & j, hexarray & hexa ) } -hexarray hexarray::from_string( slice const & s ) +uint8_t * hex_to_bytes( char const * pch, char const * const end, uint8_t * pb ) { - if( s.length() % 2 != 0 ) - throw std::runtime_error( "odd length" ); - char const * pch = s.begin(); - hexarray hexa; - hexa._bytes.resize( s.length() / 2 ); - uint8_t * pb = hexa._bytes.data(); - while( pch < s.end() ) + while( pch < end ) { if( *pch >= '0' && *pch <= '9' ) - *pb = ( *pch - '0' ) << 4; + *pb = (*pch - '0') << 4; else if( *pch >= 'a' && *pch <= 'f' ) - *pb = ( *pch - 'a' + 10 ) << 4; + *pb = (*pch - 'a' + 10) << 4; + else if( *pch >= 'A' && *pch <= 'F' ) + *pb = (*pch - 'A' + 10) << 4; else - throw std::runtime_error( "invalid character" ); - + return nullptr; ++pch; + if( *pch >= '0' && *pch <= '9' ) - *pb += ( *pch - '0' ); + *pb += (*pch - '0'); else if( *pch >= 'a' && *pch <= 'f' ) - *pb += ( *pch - 'a' + 10 ); + *pb += (*pch - 'a' + 10); + else if( *pch >= 'A' && *pch <= 'F' ) + *pb += (*pch - 'A' + 10); else - throw std::runtime_error( "invalid character" ); + return nullptr; ++pch; ++pb; } + return pb; +} + + +hexarray hexarray::from_string( slice const & s ) +{ + if( s.length() % 2 != 0 ) + throw std::runtime_error( "odd length" ); + char const * pch = s.begin(); + hexarray hexa; + hexa._bytes.resize( s.length() / 2 ); + uint8_t * pb = hexa._bytes.data(); + if( pb && ! hex_to_bytes( pch, s.end(), pb ) ) + throw std::runtime_error( "invalid character" ); return hexa; } diff --git a/tools/dds/dds-adapter/rs-dds-adapter.cpp b/tools/dds/dds-adapter/rs-dds-adapter.cpp index c477c0bc1e..5e32b1b419 100644 --- a/tools/dds/dds-adapter/rs-dds-adapter.cpp +++ b/tools/dds/dds-adapter/rs-dds-adapter.cpp @@ -83,6 +83,10 @@ static nlohmann::json load_settings( nlohmann::json const & local_settings ) // Take the "dds" settings only config = rsutils::json::nested( config, "dds" ); + // We should always have DDS enabled + if( config.is_object() ) + config.erase( "enabled" ); + // Patch the given local settings into the configuration rsutils::json::patch( config, local_settings, "local settings" ); @@ -139,8 +143,7 @@ try // Create a DDS participant auto participant = std::make_shared< dds_participant >(); { - nlohmann::json dds_settings; - dds_settings["enabled"]; // create a 'null' json key in there to remove any enabled:false + nlohmann::json dds_settings = nlohmann::json::object(); dds_settings = load_settings( dds_settings ); participant->init( domain, "rs-dds-adapter", std::move( dds_settings ) ); } diff --git a/tools/dds/dds-sniffer/rs-dds-sniffer.cpp b/tools/dds/dds-sniffer/rs-dds-sniffer.cpp index 05b21552b3..e732eb45d1 100644 --- a/tools/dds/dds-sniffer/rs-dds-sniffer.cpp +++ b/tools/dds/dds-sniffer/rs-dds-sniffer.cpp @@ -18,15 +18,18 @@ #include #include -#include #include #include #include +#include +#include +#include +#include + using namespace TCLAP; using namespace eprosima::fastdds::dds; using namespace eprosima::fastrtps::types; -using realdds::print; // FastDDS GUID_t: (MSB first, little-endian; see GuidUtils.hpp) // 2 bytes - vendor ID @@ -46,10 +49,15 @@ constexpr uint8_t GUID_PROCESS_LOCATION = 4; static eprosima::fastrtps::rtps::GuidPrefix_t std_prefix; +static inline auto 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 = 0; + realdds::dds_domain_id domain = -1; // from settings; default to 0 uint32_t seconds = 0; CmdLine cmd( "librealsense rs-dds-sniffer tool", ' ' ); @@ -59,12 +67,14 @@ int main( int argc, char ** argv ) try 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(); @@ -99,6 +109,8 @@ int main( int argc, char ** argv ) try 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" ); @@ -181,6 +193,41 @@ dds_sniffer::~dds_sniffer() _discovered_types_datas.clear(); } + +static nlohmann::json load_settings( nlohmann::json const & local_settings ) +{ + nlohmann::json config; + + // Load the realsense configuration file settings + std::ifstream f( rsutils::os::get_special_folder( rsutils::os::special_folder::app_data ) + "realsense-config.json" ); + if( f.good() ) + { + try + { + config = nlohmann::json::parse( f ); + } + catch( std::exception const & e ) + { + throw std::runtime_error( "failed to load configuration file: " + std::string( e.what() ) ); + } + } + + config = rsutils::json::load_settings( config, "context", "config-file" ); + + // Take the "dds" settings only + config = rsutils::json::nested( config, "dds" ); + + // We should always have DDS enabled + if( config.is_object() ) + config.erase( "enabled" ); + + // Patch the given local settings into the configuration + rsutils::json::patch( config, 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 @@ -207,12 +254,27 @@ bool dds_sniffer::init( realdds::dds_domain_id domain ) on_type_discovery( topic_name, dyn_type ); } ); - auto settings = nlohmann::json::object(); - _participant.init( domain, "rs-dds-sniffer", std::move( settings ) ); + nlohmann::json settings( nlohmann::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 ) @@ -359,11 +421,11 @@ void dds_sniffer::dds_reader_listener::on_subscription_matched( DataReader * rea { if( info.current_count_change == 1 ) { - LOG_DEBUG( "Subscriber matched by reader " << print( reader->guid(), std_prefix ) ); + LOG_DEBUG( "Subscriber matched by reader " << print_guid( reader->guid() ) ); } else if( info.current_count_change == -1 ) { - LOG_DEBUG( "Subscriber unmatched by reader " << print( reader->guid(), std_prefix ) ); + LOG_DEBUG( "Subscriber unmatched by reader " << print_guid( reader->guid() ) ); } else { @@ -422,6 +484,8 @@ uint32_t dds_sniffer::calc_max_indentation() const 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 ) @@ -437,14 +501,16 @@ 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, std_prefix ) << "," << topic_name + std::cout << "DataWriter," << print_guid( guid ) << "," << topic_name << ( discovered ? ",discovered" : ",removed" ) << std::endl; } else { - std::cout << "DataWriter " << print( guid, std_prefix ) << " publishing topic '" << topic_name + std::cout << "DataWriter " << print_guid( guid ) << " publishing topic '" << topic_name << ( discovered ? "' discovered" : "' removed" ) << std::endl; } } @@ -453,14 +519,16 @@ 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, std_prefix ) << "," << topic_name + std::cout << "DataReader," << print_guid( guid ) << "," << topic_name << ( discovered ? ",discovered" : ",removed" ) << std::endl; } else { - std::cout << "DataReader " << print( guid, std_prefix ) << " reading topic '" << topic_name + std::cout << "DataReader " << print_guid( guid ) << " reading topic '" << topic_name << ( discovered ? "' discovered" : "' removed" ) << std::endl; } } @@ -471,7 +539,7 @@ void dds_sniffer::print_participant_discovered( realdds::dds_guid guid, { if( _print_machine_readable ) { - std::cout << "Participant," << print( guid, std_prefix ) << "," << participant_name + std::cout << "Participant," << print_guid( guid ) << "," << participant_name << ( discovered ? ",discovered" : ",removed" ) << std::endl; } @@ -481,7 +549,7 @@ void dds_sniffer::print_participant_discovered( realdds::dds_guid guid, //prefix_.value[5] = static_cast( ( 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, std_prefix ) << " '" << participant_name << "' (" << std::hex << pid + std::cout << "Participant " << print_guid( guid ) << " '" << participant_name << "' (" << std::hex << pid << std::dec << ") " << ( discovered ? " discovered" : " removed" ) << std::endl; } } @@ -492,6 +560,9 @@ void dds_sniffer::print_topics_machine_readable() const for( auto topic : _topics_info_by_name ) { + if( filter_topic( topic.first, _root ) ) + continue; + for( auto writer : topic.second.writers ) { std::cout << topic.first << ","; @@ -516,6 +587,8 @@ void dds_sniffer::print_topics() const 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 @@ -541,7 +614,7 @@ void dds_sniffer::print_topics() const } } - // Print reminder of string + // Print remainder of string while( std::getline( current_topic, current_topic_nested, '/' ) ) { ident( indentation ); @@ -570,7 +643,7 @@ void dds_sniffer::print_participants( bool with_guids ) const { std::cout << guid2name.second; if( with_guids ) - std::cout << ' ' << realdds::print( guid2name.first ); + std::cout << ' ' << realdds::print_raw_guid( guid2name.first ); std::cout << std::endl; } } @@ -592,18 +665,14 @@ void dds_sniffer::print_topic_writer( realdds::dds_guid guid, uint32_t indentati { if( iter->first.guidPrefix == guid.guidPrefix ) { - uint16_t tmp; - memcpy( &tmp, &iter->first.guidPrefix.value[GUID_PROCESS_LOCATION], sizeof( tmp ) ); if( _print_machine_readable ) { - std::cout << "Writer," << iter->second << "_" << std::hex << std::setw( 4 ) << std::setfill( '0' ) - << tmp << std::dec << std::endl; + std::cout << "Writer," << realdds::dds_participant::name_from_guid( iter->first ) << std::endl; } else { ident( indentation ); - std::cout << "Writer of \"" << iter->second << "_" << std::hex << std::setw( 4 ) << std::setfill( '0' ) - << tmp << std::dec << "\"" << std::endl; + std::cout << "Writer of \"" << realdds::dds_participant::name_from_guid( iter->first ) << "\"" << std::endl; } break; } @@ -615,6 +684,7 @@ void dds_sniffer::print_topic_writer( realdds::dds_guid guid, uint32_t indentati } } + void dds_sniffer::print_topic_reader( realdds::dds_guid guid, uint32_t indentation ) const { auto iter = _discovered_participants.begin(); @@ -622,18 +692,14 @@ void dds_sniffer::print_topic_reader( realdds::dds_guid guid, uint32_t indentati { if( iter->first.guidPrefix == guid.guidPrefix ) { - uint16_t tmp; - memcpy( &tmp, &iter->first.guidPrefix.value[GUID_PROCESS_LOCATION], sizeof( tmp ) ); if( _print_machine_readable ) { - std::cout << "Reader," << iter->second << "_" << std::hex << std::setw( 4 ) << std::setfill( '0' ) - << tmp << std::dec << std::endl; + std::cout << "Reader," << realdds::dds_participant::name_from_guid( iter->first ) << std::endl; } else { ident( indentation ); - std::cout << "Reader of \"" << iter->second << "_" << std::hex << std::setw( 4 ) << std::setfill( '0' ) - << tmp << std::dec << "\"" << std::endl; + std::cout << "Reader of \"" << realdds::dds_participant::name_from_guid( iter->first ) << "\"" << std::endl; } break; } diff --git a/tools/dds/dds-sniffer/rs-dds-sniffer.h b/tools/dds/dds-sniffer/rs-dds-sniffer.h index 4e3474737f..0638a71630 100644 --- a/tools/dds/dds-sniffer/rs-dds-sniffer.h +++ b/tools/dds/dds-sniffer/rs-dds-sniffer.h @@ -25,6 +25,8 @@ class dds_sniffer void print_machine_readable( bool enable ) { _print_machine_readable = enable; } void print_topic_samples( bool enable ) { _print_topic_samples = enable; } + void set_root( std::string const & root ) { _root = root; } + bool init( realdds::dds_domain_id domain = 0 ); realdds::dds_participant const & get_participant() const { return _participant; } @@ -51,6 +53,8 @@ class dds_sniffer bool _print_machine_readable = false; bool _print_topic_samples = false; + std::string _root; + mutable std::mutex _dds_entities_lock; // For sniffing topics (can sniff only topics that send TypeObject during discovery) diff --git a/unit-tests/dds/test-guid.py b/unit-tests/dds/test-guid.py new file mode 100644 index 0000000000..5d9ddb3a9e --- /dev/null +++ b/unit-tests/dds/test-guid.py @@ -0,0 +1,36 @@ +# License: Apache 2.0. See LICENSE file in root directory. +# Copyright(c) 2023 Intel Corporation. All Rights Reserved. + +#test:donotrun:!dds + +from rspy import log, test +import pyrealdds as dds + + +with test.closure( 'default ctor' ): + test.check_false( dds.guid() ) + +with test.closure( 'from string: eProsima format' ): + test.check_equal( repr( dds.guid.from_string( '112233445566.5.100' )), '112233445566.5.100' ) + test.check_false( dds.guid.from_string( '1122334455.5.100' ) ) + test.check_false( dds.guid.from_string( '00112233445566778899001122.5.100' ) ) + test.check_equal( repr( dds.guid.from_string( 'aabbccddeeff.5.100' ) ), 'aabbccddeeff.5.100' ) # hex digits + test.check_equal( repr( dds.guid.from_string( '112233445566.b.100' ) ), '112233445566.b.100' ) + test.check_equal( repr( dds.guid.from_string( '112233445566.5.10a' ) ), '112233445566.5.10a' ) + test.check_equal( repr( dds.guid.from_string( 'aaBBccDDeeFF.5.100' ) ), 'aabbccddeeff.5.100' ) # uppercase OK, but output is lowercase + test.check_equal( repr( dds.guid.from_string( '112233445566.CDE.100' ) ), '112233445566.cde.100' ) + test.check_equal( repr( dds.guid.from_string( '112233445566.5.1A0' ) ), '112233445566.5.1a0' ) + test.check_false( dds.guid.from_string( '112233g45566.5.100' ) ) # invalid + test.check_false( dds.guid.from_string( '112233445566.U.100' ) ) + test.check_false( dds.guid.from_string( '112233445566.5.1X0' ) ) + test.check_false( dds.guid.from_string( '112233445566.-1.100' ) ) + test.check_false( dds.guid.from_string( '112233445566.0.100-' ) ) + +with test.closure( 'from string: generic format' ): + test.check_false( dds.guid.from_string( '' ) ) + test.check_equal( repr( dds.guid.from_string( '001122334455667788990011.100' ) ), '001122334455667788990011.100' ) + test.check_equal( repr( dds.guid.from_string( '010f22334455667788990000.100' ) ), '223344556677.9988.100' ) # eProsima vendor ID + + + +test.print_results_and_exit() diff --git a/unit-tests/py/rspy/test.py b/unit-tests/py/rspy/test.py index d18fd4b848..f9e188d41a 100644 --- a/unit-tests/py/rspy/test.py +++ b/unit-tests/py/rspy/test.py @@ -203,11 +203,13 @@ def check_passed(): return True -def check_failed( on_fail=LOG ): +def check_failed( on_fail=LOG, description=None ): """ Function for when a check fails :return: always False (so you can 'return check_failed()' """ + if description: + log.out( f' {description}' ) _count_check() global n_failed_assertions, test_failed n_failed_assertions += 1 @@ -235,11 +237,7 @@ def check( exp, description=None, on_fail=LOG ): """ if not exp: print_stack() - if description: - log.out( f' {description}' ) - else: - log.out( f' check failed; received {exp}' ) - return check_failed( on_fail ) + return check_failed( on_fail, description=( description or f'Got \'{exp}\'' )) return check_passed() @@ -247,7 +245,10 @@ def check_false( exp, description=None, on_fail=LOG ): """ Opposite of check() """ - return check( not exp, description, on_fail ) + if exp: + print_stack() + return check_failed( on_fail, description=( description or f'Expecting False; got \'{exp}\'' )) + return check_passed() def check_equal( result, expected, on_fail=LOG ): @@ -315,8 +316,7 @@ def _unexpected_exception( type, e, tb ): print_stack_( traceback.format_list( traceback.extract_tb( tb ))) for line in traceback.format_exception_only( type, e ): log.out( line[:-1], line_prefix = ' ' ) - log.out( ' Unexpected exception!' ) - check_failed() + check_failed( description='Unexpected exception!' ) def unexpected_exception(): diff --git a/unit-tests/rsutils/string/test-hexarray.cpp b/unit-tests/rsutils/string/test-hexarray.cpp index 62cdec7438..2c84a00317 100644 --- a/unit-tests/rsutils/string/test-hexarray.cpp +++ b/unit-tests/rsutils/string/test-hexarray.cpp @@ -369,9 +369,16 @@ TEST_CASE( "hexarray", "[hexarray]" ) { CHECK( hexarray::from_string( ba_string ).get_bytes() == ba ); } + SECTION( "case insensitive" ) + { + CHECK( hexarray::from_string( { "0A", 2 } ).to_string() == "0a" ); + } + SECTION( "empty string is an empty array" ) + { + CHECK( hexarray::from_string( { "abc", size_t(0) } ).to_string() == "" ); + } SECTION( "throws on invalid chars" ) { - CHECK_NOTHROW( hexarray::from_string( std::string( "" ) ) ); CHECK_THROWS( hexarray::from_string( std::string( "1" ) ) ); // invalid length CHECK_NOTHROW( hexarray::from_string( std::string( "01" ) ) ); CHECK_NOTHROW( hexarray::from_string( std::string( "02" ) ) ); @@ -389,7 +396,6 @@ TEST_CASE( "hexarray", "[hexarray]" ) CHECK_NOTHROW( hexarray::from_string( std::string( "0e" ) ) ); CHECK_NOTHROW( hexarray::from_string( std::string( "0f" ) ) ); CHECK_THROWS( hexarray::from_string( std::string( "0g" ) ) ); // all the rest should throw - CHECK_THROWS( hexarray::from_string( std::string( "0A" ) ) ); // upper-case isn't accepted } SECTION( "to json" ) {