PR #12173 from Eran: DDS hardware reset & device mgmt

src/core/options-registry.cpp

@@ -33,12 +33,16 @@ static std::mutex _mutex;
 
 inline size_t option_to_index( rs2_option by_name_option )
 {
+    // Registered rs2_option value will be negative, so we negate
+    // Indices are 0-based but negative rs2_options values are 1-based, so minus 1:
     return -by_name_option - 1;
 }
 
 
 inline rs2_option index_to_option( size_t index )
 {
+    // Index = index into the _name_by_index array -> 0-based
+    // Registered rs2_option value will be negative, and starting at -1:
     return rs2_option( -( int( index ) + 1 ) );
 }
 

src/core/options-registry.h

@@ -9,6 +9,15 @@
 namespace librealsense {
 
 
+// This registry is used to register options that have custom names. I.e., names that do not have values in
+// librealsense's built-in rs2_option enumeration.
+// 
+// If you need an option "X", it must first be registered. The name and the rs2_option value for it will then be
+// associated.
+// 
+// Such options need unique identifiers that are obviously not in rs2_option and yet must seem like they are. Since
+// rs2_option is intergral in nature, we use negative values to identify registered options.
+//
 class options_registry
 {
 public:

src/dds/rs-dds-device-info.cpp

@@ -24,7 +24,7 @@ std::string dds_device_info::get_address() const
     auto const domain_id = _dev->participant()->get()->get_domain_id();
 
     return rsutils::string::from() << "dds." << domain_id << "://"
-                                   << _dev->participant()->name_from_guid( _dev->guid() ) << "@"
+                                   << _dev->participant()->print( _dev->server_guid() ) << "@"
                                    << _dev->device_info().topic_root;
 }
 

src/dds/rs-dds-device-proxy.cpp

@@ -11,6 +11,7 @@
 #include <realdds/dds-trinsics.h>
 
 #include <realdds/topics/device-info-msg.h>
+#include <realdds/topics/flexible-msg.h>
 
 #include <src/stream.h>
 #include <src/environment.h>
@@ -477,4 +478,16 @@ void dds_device_proxy::tag_profiles( stream_profiles profiles ) const
 }
 
 
+void dds_device_proxy::hardware_reset()
+{
+    nlohmann::json control = nlohmann::json::object( { { "id", "hw-reset" } } );
+    nlohmann::json reply;
+    _dds_dev->send_control( control, &reply );
+    std::string default_status( "OK", 2 );
+    if( rsutils::json::get( reply, "status", default_status ) != default_status )
+        throw std::runtime_error( "Failed to reset: "
+                                  + rsutils::json::get( reply, "status", std::string( "unknown reason" ) ) );
+}
+
+
 }  // namespace librealsense

src/dds/rs-dds-device-proxy.h

@@ -58,6 +58,8 @@ class dds_device_proxy : public software_device
     std::shared_ptr< dds_sensor_proxy > create_sensor( std::string const & sensor_name, rs2_stream sensor_type );
 
     void tag_profiles( stream_profiles profiles ) const override;
+
+    void hardware_reset() override;
 };
 
 

third-party/realdds/include/realdds/dds-device-server.h

@@ -70,6 +70,8 @@ class dds_device_server
     dds_device_server( std::shared_ptr< dds_participant > const & participant, const std::string & topic_root );
     ~dds_device_server();
 
+    dds_guid const & guid() const;
+
     // A server is not valid until init() is called with a list of streams that we want to publish.
     // On successful return from init(), each of the streams will be alive so clients will be able
     // to subscribe.
@@ -88,14 +90,14 @@ class dds_device_server
 
     bool has_metadata_readers() const;
 
-    typedef std::function< void( const nlohmann::json & msg ) > control_callback;
-    void on_open_streams( control_callback callback ) { _open_streams_callback = std::move( callback ); }
-
     typedef std::function< void( const std::shared_ptr< realdds::dds_option > & option, float value ) > set_option_callback;
     typedef std::function< float( const std::shared_ptr< realdds::dds_option > & option ) > query_option_callback;
     void on_set_option( set_option_callback callback ) { _set_option_callback = std::move( callback ); }
     void on_query_option( query_option_callback callback ) { _query_option_callback = std::move( callback ); }
 
+    typedef std::function< bool( std::string const &, nlohmann::json const &, nlohmann::json & ) > control_callback;
+    void on_control( control_callback callback ) { _control_callback = std::move( callback ); }
+
 private:
     void on_control_message_received();
     void handle_control_message( std::string const & id,
@@ -117,9 +119,9 @@ class dds_device_server
     std::shared_ptr< dds_device_broadcaster > _broadcaster;
     dispatcher _control_dispatcher;
 
-    control_callback _open_streams_callback = nullptr;
-    set_option_callback _set_option_callback = nullptr;
-    query_option_callback _query_option_callback = nullptr;
+    set_option_callback _set_option_callback;
+    query_option_callback _query_option_callback;
+    control_callback _control_callback;
 
     extrinsics_map _extrinsics_map; // <from stream, to stream> to extrinsics
 };  // class dds_device_server

third-party/realdds/include/realdds/dds-device.h

@@ -23,26 +23,22 @@ class device_info;
 class dds_participant;
 
 // Represents a device via the DDS system. Such a device exists as of its identification by the device-watcher, and
-// always contains a device-info and GUID of the remote DataWriter to which it belongs.
+// always contains a device-info.
 // 
 // The device may not be ready for use (will not contain sensors, profiles, etc.) until it received all handshake
 // notifications from the server, but it will start receiving notifications and be able to send controls right away.
 //
 class dds_device
 {
 public:
-    static std::shared_ptr< dds_device > find( dds_guid const & guid );
-
-    static std::shared_ptr< dds_device > create( std::shared_ptr< dds_participant > const & participant,
-                                                 dds_guid const & guid,
-                                                 topics::device_info const & info );
+    dds_device( std::shared_ptr< dds_participant > const &, topics::device_info const & );
 
     std::shared_ptr< dds_participant > const & participant() const;
     std::shared_ptr< dds_subscriber > const & subscriber() const;
     topics::device_info const & device_info() const;
 
-    // The device GUID is that of the DataWriter which declares it!
-    dds_guid const & guid() const;
+    dds_guid const & server_guid() const;  // server notification writer
+    dds_guid const & guid() const;         // client control writer (and notification samples)
 
     // A device is ready for use after it's gone through handshake and can start streaming
     bool is_ready() const;
@@ -77,17 +73,13 @@ class dds_device
         on_device_log_callback;
     void on_device_log( on_device_log_callback cb );
 
+    typedef std::function< bool( std::string const & id, nlohmann::json const & ) > on_notification_callback;
+    void on_notification( on_notification_callback );
+
 private:
     class impl;
     std::shared_ptr< impl > _impl;
-
-    // Ctor is private: use find() or create() instead. Same for dtor -- it should be automatic
-    dds_device( std::shared_ptr< impl > );
-
-    //Called internally by other functions, mutex should be locked prior to calling this function
-    //Solves possible race conditions when serching for an item and creating if does not exist.
-    static std::shared_ptr< dds_device > find_internal( dds_guid const & guid );
-};  // class dds_device
+};
 
 
 }  // namespace realdds

third-party/realdds/include/realdds/dds-guid.h

@@ -10,18 +10,21 @@
 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
 //
 std::string print( dds_guid const & guid,
-                   dds_guid_prefix const & base_prefix = eprosima::fastrtps::rtps::c_Guid_Unknown.guidPrefix,
+                   dds_guid_prefix const & base_prefix = unknown_guid.guidPrefix,
                    bool readable_name = true );
 
 // Same as above, without a base prefix
 inline std::string print( dds_guid const & guid, bool readable_name )
 {
-    return print( guid, eprosima::fastrtps::rtps::c_Guid_Unknown.guidPrefix, readable_name );
+    return print( guid, unknown_guid.guidPrefix, readable_name );
 }
 
 // Same as above, with a guid base for flexibility

third-party/realdds/include/realdds/dds-notification-server.h

@@ -30,6 +30,8 @@ class dds_notification_server
     dds_notification_server( std::shared_ptr< dds_publisher > const & publisher, const std::string & topic_name );
     ~dds_notification_server();
 
+    dds_guid const & guid() const;
+
     // By default we're not running, to avoid on-discovery before all discovery messages have been collected
     void run();
     bool is_running() const { return _active; }

third-party/realdds/include/realdds/dds-topic-writer.h

@@ -5,6 +5,7 @@
 
 #include <fastdds/dds/publisher/DataWriterListener.hpp>
 #include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
+#include "dds-defines.h"
 
 #include <memory>
 
@@ -52,6 +53,7 @@ class dds_topic_writer : protected eprosima::fastdds::dds::DataWriterListener
 
     std::shared_ptr< dds_topic > const & topic() const { return _topic; }
     std::shared_ptr< dds_publisher > const & publisher() const { return _publisher; }
+    dds_guid const & guid() const;
 
     typedef std::function< void( eprosima::fastdds::dds::PublicationMatchedStatus const & ) >
         on_publication_matched_callback;

third-party/realdds/include/realdds/topics/device-info-msg.h

@@ -17,7 +17,7 @@ class device_info
     std::string serial;
     std::string product_line;
     std::string topic_root;
-    bool locked = false;
+    bool locked = true;
 
     nlohmann::json to_json() const;
     static device_info from_json( nlohmann::json const & j );

third-party/realdds/py/pyrealdds.cpp

@@ -95,7 +95,15 @@ PYBIND11_MODULE(NAME, m) {
     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( "__repr__", []( dds_guid const & self ) { return to_string( self ); } )
+        // 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
+              } )
+        .def( py::self == py::self );
 
     using realdds::dds_participant;
     using eprosima::fastrtps::types::ReturnCode_t;
@@ -282,6 +290,7 @@ PYBIND11_MODULE(NAME, m) {
     using realdds::dds_topic_writer;
     py::class_< dds_topic_writer, std::shared_ptr< dds_topic_writer > >( m, "topic_writer" )
         .def( py::init< std::shared_ptr< dds_topic > const & >() )
+        .def( "guid", &dds_topic_writer::guid )
         .def( FN_FWD( dds_topic_writer,
                       on_publication_matched,
                       (dds_topic_writer &, int),
@@ -712,10 +721,22 @@ PYBIND11_MODULE(NAME, m) {
         .def( "set_accel_intrinsics", &dds_motion_stream_server::set_accel_intrinsics )
         .def( "start_streaming", &dds_motion_stream_server::start_streaming );
 
+    // To have the python code be able to modify json objects in callbacks, we need to somehow refer to the original
+    // json object as changes to translated dict will not automatically get picked up!
+    // We do this thru the [] operator:
+    //     def callback( json ):
+    //         print( json['value'] )            # calls __getitem__
+    //         json['value'] = { 'more': True }  # calls __setitem__
+    struct json_ref { nlohmann::json & j; };
+    py::class_< json_ref, std::shared_ptr< json_ref > >( m, "json_ref" )
+        .def( "__getitem__", []( json_ref const & jr, std::string const & key ) { return jr.j.at( key ); } )
+        .def( "__setitem__", []( json_ref & jr, std::string const & key, nlohmann::json const & value ) { jr.j[key] = value; } );
+
     using realdds::dds_device_server;
     py::class_< dds_device_server, std::shared_ptr< dds_device_server > >( m, "device_server" )
         .def( py::init< std::shared_ptr< dds_participant > const&, std::string const& >() )
         .def( "init", &dds_device_server::init )
+        .def( "guid", &dds_device_server::guid )
         .def( "streams",
               []( dds_device_server const & self )
               {
@@ -729,7 +750,12 @@ PYBIND11_MODULE(NAME, m) {
             []( dds_device_server & self, nlohmann::json const & j ) { self.publish_notification( j ); },
             py::call_guard< py::gil_scoped_release >() )
         .def( "publish_metadata", &dds_device_server::publish_metadata, py::call_guard< py::gil_scoped_release >() )
-        .def( "broadcast", &dds_device_server::broadcast );
+        .def( "broadcast", &dds_device_server::broadcast )
+        .def( FN_FWD_R( dds_device_server, on_control,
+                        false,
+                        (dds_device_server &, std::string const &, py::object &&, json_ref &&),
+                        ( std::string const & id, nlohmann::json const & control, nlohmann::json & reply ),
+                        return callback( self, id, json_to_py( control ), json_ref{ reply } ); ) );
 
     using realdds::dds_stream;
     py::class_< dds_stream, std::shared_ptr< dds_stream > > stream_client_base( m, "stream", stream_base );
@@ -785,9 +811,10 @@ PYBIND11_MODULE(NAME, m) {
     py::class_< dds_device,
         std::shared_ptr< dds_device >  // handled with a shared_ptr
     >( m, "device" )
-        .def( py::init( &dds_device::create ) )
+        .def( py::init< std::shared_ptr< dds_participant > const &, device_info const & >() )
         .def( "device_info", &dds_device::device_info )
         .def( "participant", &dds_device::participant )
+        .def( "server_guid", &dds_device::server_guid )
         .def( "guid", &dds_device::guid )
         .def( "is_ready", &dds_device::is_ready )
         .def( "wait_until_ready",
@@ -804,6 +831,12 @@ PYBIND11_MODULE(NAME, m) {
                       (dds_device &, dds_time const &, char, std::string const &, py::object && ),
                       (dds_time const & timestamp, char type, std::string const & text, nlohmann::json const & data),
                       callback( self, timestamp, type, text, json_to_py( data ) ); ) )
+        .def( FN_FWD_R( dds_device,
+                        on_notification,
+                        false,
+                        (dds_device &, std::string const &, py::object &&),
+                        ( std::string const & id, nlohmann::json const & data ),
+                        return callback( self, id, json_to_py( data ) ); ) )
         .def( "n_streams", &dds_device::number_of_streams )
         .def( "streams",
               []( dds_device const & self ) {
@@ -832,12 +865,11 @@ PYBIND11_MODULE(NAME, m) {
             py::call_guard< py::gil_scoped_release >() )
         .def( "__repr__", []( dds_device const & self ) {
             std::ostringstream os;
-            os << "<" SNAME ".device ";
-            os << self.participant()->print( self.guid() );
+            os << "<" SNAME ".device";
+            os << " " << self.participant()->print( self.guid() );
             if( ! self.device_info().name.empty() )
                 os << " \"" << self.device_info().name << "\"";
-            if( ! self.device_info().serial.empty() )
-                os << " s/n \"" << self.device_info().serial << "\"";
+            os << " @ " << self.device_info().debug_name();
             os << ">";
             return os.str();
         } );