PR IntelRealSense#12127 from Eran: add dds-device::send-control() and…

… topic-send script

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

@@ -62,6 +62,8 @@ class dds_device
     void set_option_value( const std::shared_ptr< dds_option > & option, float new_value );
     float query_option_value( const std::shared_ptr< dds_option > & option );
 
+    void send_control( topics::flexible_msg &&, nlohmann::json * reply = nullptr );
+
     bool has_extrinsics() const;
     std::shared_ptr< extrinsics > get_extrinsics( std::string const & from, std::string const & to ) const;
 

third-party/realdds/py/pyrealdds.cpp

@@ -816,6 +816,16 @@ PYBIND11_MODULE(NAME, m) {
               } )
         .def( "set_option_value", &dds_device::set_option_value )
         .def( "query_option_value", &dds_device::query_option_value )
+        .def(
+            "send_control",
+            []( dds_device & self, nlohmann::json const & j, bool wait_for_reply )
+            {
+                nlohmann::json reply;
+                self.send_control( j, wait_for_reply ? &reply : nullptr );
+                return reply;
+            },
+            py::arg( "json" ), py::arg( "wait-for-reply" ) = false,
+            py::call_guard< py::gil_scoped_release >() )
         .def( "__repr__", []( dds_device const & self ) {
             std::ostringstream os;
             os << "<" SNAME ".device ";

third-party/realdds/scripts/topic-send.py

@@ -0,0 +1,76 @@
+# License: Apache 2.0. See LICENSE file in root directory.
+# Copyright(c) 2023 Intel Corporation. All Rights Reserved.
+
+from argparse import ArgumentParser
+args = ArgumentParser()
+args.add_argument( '--debug', action='store_true', help='enable debug mode' )
+args.add_argument( '--quiet', action='store_true', help='No output; just the minimum FPS as a number' )
+args.add_argument( '--device', metavar='<path>', help='the topic root for the device' )
+args.add_argument( '--topic', metavar='<path>', help='the topic on which to send flexible message, if --device is not supplied' )
+args.add_argument( '--message', metavar='<string>', help='a message to send', default='some message' )
+def domain_arg(x):
+    t = int(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 = args.parse_args()
+
+
+if args.quiet:
+    def i( *a, **kw ):
+        pass
+else:
+    def i( *a, **kw ):
+        print( '-I-', *a, **kw )
+def e( *a, **kw ):
+    print( '-E-', *a, **kw )
+
+
+import pyrealdds as dds
+import time
+import sys
+
+dds.debug( args.debug )
+
+settings = {}
+
+participant = dds.participant()
+participant.init( args.domain, 'topic-send', settings )
+
+message = { 'id': 'ping', 'message': args.message }
+
+if args.device:
+    info = dds.message.device_info()
+    info.name = 'Dummy Device'
+    info.topic_root = args.device
+    device = dds.device( participant, participant.create_guid(), info )
+    try:
+        i( 'Looking for device at', info.topic_root, '...' )
+        device.wait_until_ready()  # If unavailable before timeout, this throws
+    except:
+        e( 'Cannot find device' )
+        sys.exit( 1 )
+
+    wait_for_reply = True
+    reply = device.send_control( message, wait_for_reply )
+    i( f'Sent {message} on {info.topic_root}; got back {reply}' )
+
+    if args.debug or not wait_for_reply:
+        # Sleep a bit, to allow us to catch and display any replies
+        time.sleep( 2 )
+
+elif not args.topic:
+    e( 'Either --device or --topic is required' )
+    sys.exit( 1 )
+
+else:
+    topic_path = args.topic
+    writer = dds.topic_writer( dds.message.flexible.create_topic( participant, topic_path ))
+    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 )
+    dds.message.flexible( message ).write_to( writer )
+    i( f'Sent {message} on {topic_path}' )
+
+

third-party/realdds/src/dds-device-impl.cpp

@@ -145,10 +145,8 @@ void dds_device::impl::handle_notification( nlohmann::json const & j )
     {
         auto id = rsutils::json::get< std::string >( j, id_key );
         auto it = _notification_handlers.find( id );
-        if( it == _notification_handlers.end() )
-            throw std::runtime_error( "unknown id" );
-
-        ( this->*( it->second ) )( j );
+        if( it != _notification_handlers.end() )
+            ( this->*( it->second ) )( j );
 
         // Check if this is a reply - maybe someone's waiting on it...
         auto sampleit = j.find( sample_key );
@@ -173,6 +171,9 @@ void dds_device::impl::handle_notification( nlohmann::json const & j )
                 }
             }
         }
+
+        if( it == _notification_handlers.end() )
+            throw std::runtime_error( "unknown id" );
     }
     catch( std::exception const & e )
     {
@@ -391,6 +392,7 @@ void dds_device::impl::write_control_message( topics::flexible_msg && msg, nlohm
         {
             throw std::runtime_error( "timeout waiting for reply #" + std::to_string( this_sequence_number ) );
         }
+        LOG_DEBUG( "got reply: " << actual_reply );
         *reply = std::move( actual_reply );
         _replies.erase( this_sequence_number );
     }

third-party/realdds/src/dds-device.cpp

@@ -144,6 +144,11 @@ float dds_device::query_option_value( const std::shared_ptr< dds_option > & opti
     return _impl->query_option_value( option );
 }
 
+void dds_device::send_control( topics::flexible_msg && msg, nlohmann::json * reply )
+{
+    _impl->write_control_message( std::move( msg ), reply );
+}
+
 bool dds_device::has_extrinsics() const
 {
     return ! _impl->_extrinsics_map.empty();