Implement DDS close-streams using open-streams control

Author: OhadMeir

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

@@ -261,39 +261,28 @@ dds_sensor_proxy::find_profile( sid_index sidx, realdds::dds_motion_stream_profi
 }
 
 
-void dds_sensor_proxy::open( const stream_profiles & profiles )
+realdds::dds_stream_profiles dds_sensor_proxy::find_dds_profiles( const librealsense::stream_profiles & source_profiles ) const
 {
-    _formats_converter.prepare_to_convert( profiles );
-    _active_converted_profiles = profiles;
-    const auto & source_profiles = _formats_converter.get_active_source_profiles();
-    // TODO - register processing block options?
-
     realdds::dds_stream_profiles realdds_profiles;
     for( size_t i = 0; i < source_profiles.size(); ++i )
     {
         auto & sp = source_profiles[i];
         sid_index sidx( sp->get_unique_id(), sp->get_stream_index() );
         if( auto const vsp = As< video_stream_profile >( sp ) )
         {
-            auto video_profile = find_profile(
-                sidx,
-                realdds::dds_video_stream_profile( sp->get_framerate(),
-                                                   realdds::dds_video_encoding::from_rs2( sp->get_format() ),
-                                                   vsp->get_width(),
-                                                   vsp->get_height() ) );
+            auto video_profile = find_profile( sidx, 
+                                               realdds::dds_video_stream_profile( sp->get_framerate(),
+                                                                                  realdds::dds_video_encoding::from_rs2( sp->get_format() ),
+                                                                                  vsp->get_width(),
+                                                                                  vsp->get_height() ) );
             if( video_profile )
-            {
                 realdds_profiles.push_back( video_profile );
-                calculate_bandwidth( vsp );
-            }
             else
                 LOG_ERROR( "no profile found in stream for rs2 profile " << vsp );
         }
         else if( Is< motion_stream_profile >( sp ) )
         {
-            auto motion_profile = find_profile(
-                sidx,
-                realdds::dds_motion_stream_profile( source_profiles[i]->get_framerate() ) );
+            auto motion_profile = find_profile( sidx, realdds::dds_motion_stream_profile( source_profiles[i]->get_framerate() ) );
             if( motion_profile )
                 realdds_profiles.push_back( motion_profile );
             else
@@ -305,23 +294,38 @@ void dds_sensor_proxy::open( const stream_profiles & profiles )
         }
     }
 
+    return realdds_profiles;
+}
+
+void dds_sensor_proxy::open( const stream_profiles & profiles )
+{
+    _formats_converter.prepare_to_convert( profiles );
+    _active_converted_profiles = profiles;
+    const auto & source_profiles = _formats_converter.get_active_source_profiles();
+    // TODO - register processing block options?
+
+    for( size_t i = 0; i < source_profiles.size(); ++i )
+        if( auto const vsp = As< video_stream_profile >( source_profiles[i] ) )
+            log_bandwidth( vsp );
+
     try
     {
+        software_sensor::open( source_profiles ); // Call before send to device to check SDK conditions (not open/streaming/etc...)
         if( source_profiles.size() > 0 )
         {
+            realdds::dds_stream_profiles realdds_profiles = find_dds_profiles( source_profiles );
             _dev->open( realdds_profiles );
         }
-
-        software_sensor::open( source_profiles );
     }
     catch( realdds::dds_runtime_error const & e )
     {
+        software_sensor::close();
         throw invalid_value_exception( e.what() );
     }
 }
 
 
-void dds_sensor_proxy::calculate_bandwidth( const std::shared_ptr< video_stream_profile > & vsp )
+void dds_sensor_proxy::log_bandwidth( const std::shared_ptr< video_stream_profile > & vsp ) const
 {
     size_t width = vsp->get_width();
     size_t height = vsp->get_height();
@@ -632,8 +636,19 @@ void dds_sensor_proxy::stop()
 
 void dds_sensor_proxy::close()
 {
-    software_sensor::close();
-    _active_converted_profiles.clear();
+    const auto & source_profiles = _formats_converter.get_active_source_profiles();
+    realdds::dds_stream_profiles realdds_profiles = find_dds_profiles( source_profiles );
+
+    try
+    {
+        software_sensor::close();
+        _dev->close( realdds_profiles );
+        _active_converted_profiles.clear();
+    }
+    catch( realdds::dds_runtime_error const & e )
+    {
+        throw invalid_value_exception( e.what() );
+    }
 }
 
 

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

@@ -11,6 +11,7 @@
 #include <realdds/dds-defines.h>
 #include <realdds/dds-metadata-syncer.h>
 #include <realdds/dds-embedded-filter.h>
+#include <realdds/dds-stream-profile.h>
 
 #include <rsutils/json-fwd.h>
 #include <memory>
@@ -98,14 +99,16 @@ class dds_sensor_proxy : public software_sensor
 protected:
     void register_converters();
     stream_profiles init_stream_profiles() override;
-    void calculate_bandwidth( const std::shared_ptr< librealsense::video_stream_profile > & vsp );
+    void log_bandwidth( const std::shared_ptr< librealsense::video_stream_profile > & vsp ) const;
 
     std::shared_ptr< realdds::dds_video_stream_profile >
     find_profile( sid_index sidx, realdds::dds_video_stream_profile const & profile ) const;
 
     std::shared_ptr< realdds::dds_motion_stream_profile >
     find_profile( sid_index sidx, realdds::dds_motion_stream_profile const & profile ) const;
 
+    realdds::dds_stream_profiles find_dds_profiles( const librealsense::stream_profiles & source_profiles ) const;
+    
     void handle_video_data( std::vector< uint8_t > &&,
                             realdds::dds_time &&,
                             realdds::dds_sample &&,

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

@@ -78,6 +78,7 @@ class dds_device : public dds_discovery_sink
     size_t foreach_option( std::function< void( std::shared_ptr< dds_option > option ) > fn ) const;
 
     void open( const dds_stream_profiles & profiles );
+    void close( const dds_stream_profiles & profiles );
 
     void set_option_value( const std::shared_ptr< dds_option > & option, rsutils::json new_value );
     rsutils::json query_option_value( const std::shared_ptr< dds_option > & option );

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

@@ -489,32 +489,68 @@ void dds_device::impl::on_log( json const & j, dds_sample const & )
 }
 
 
-void dds_device::impl::open( const dds_stream_profiles & profiles )
+void dds_device::impl::add_profiles_to_json( const realdds::dds_stream_profiles & profiles, rsutils::json & profiles_as_json ) const
 {
-    if( profiles.empty() )
-        DDS_THROW( runtime_error, "must provide at least one profile" );
-
-    json stream_profiles;
     for( auto & profile : profiles )
     {
         auto stream = profile->stream();
         if( ! stream )
             DDS_THROW( runtime_error, "profile '" << profile->to_string() << "' is not part of any stream" );
-        if( stream_profiles.nested( stream->name() ) )
+        if( profiles_as_json.nested( stream->name() ) )
             DDS_THROW( runtime_error, "more than one profile found for stream '" << stream->name() << "'" );
 
-        stream_profiles[stream->name()] = profile->to_json();
+        profiles_as_json[stream->name()] = profile->to_json();
     }
+}
+
+void dds_device::impl::open( const dds_stream_profiles & profiles )
+{
+    if( profiles.empty() )
+        DDS_THROW( runtime_error, "must provide at least one profile" );
+
+    json profiles_to_open;
+    add_profiles_to_json( profiles, profiles_to_open );
+    // Not needed, already open streams are kept open by FW
+    // add_profiles_to_json( _open_profiles_list, profiles_to_open ); // Add already open profiles to the list
 
     json j = {
         { topics::control::key::id, topics::control::open_streams::id },
-        { topics::control::open_streams::key::stream_profiles, std::move( stream_profiles ) },
+        // D555 initial FW treats reset field as implicitly true, so we explicitly mention it here
+        { topics::control::open_streams::key::reset, false }
     };
+    if( ! profiles_to_open.empty() )
+        j[topics::control::open_streams::key::stream_profiles] = std::move( profiles_to_open );
 
     json reply;
     write_control_message( j, &reply );
+
+    // If no exception writing to the device then save profiles in open profiles list
+    _open_profiles_list.insert( _open_profiles_list.end(), profiles.begin(), profiles.end() );
 }
 
+void dds_device::impl::close( const dds_stream_profiles & profiles )
+{
+    // Remove profiles from open profiles list. Not using erase-remove idiom but for a small number of profiles it does not really matter...
+    for( auto & profile : profiles )
+    {
+        auto it = find( _open_profiles_list.begin(), _open_profiles_list.end(), profile );
+        if( it != _open_profiles_list.end() )
+            _open_profiles_list.erase( it );
+    }
+
+    json keep_open_profiles;
+    add_profiles_to_json( _open_profiles_list, keep_open_profiles );
+
+    json j = {
+        { topics::control::key::id, topics::control::open_streams::id },
+        { topics::control::open_streams::key::reset, true }
+    };
+    if( ! keep_open_profiles.empty() )
+        j[topics::control::open_streams::key::stream_profiles] = std::move( keep_open_profiles );
+
+    json reply;
+    write_control_message( j, &reply );
+}
 
 void dds_device::impl::set_option_value( const std::shared_ptr< dds_option > & option, json new_value )
 {

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

@@ -80,6 +80,7 @@ class dds_device::impl
     bool is_ready() const { return state_t::READY == _state; }
 
     void open( const dds_stream_profiles & profiles );
+    void close( const dds_stream_profiles & profiles );
 
     void write_control_message( rsutils::json const &, rsutils::json * reply = nullptr );
 
@@ -140,10 +141,13 @@ class dds_device::impl
     void on_set_filter(rsutils::json const&, dds_sample const&);
     void on_query_filter(rsutils::json const&, dds_sample const&);
 
+    void add_profiles_to_json( const realdds::dds_stream_profiles & profiles, rsutils::json & profiles_as_json ) const;
+
     on_metadata_available_signal _on_metadata_available;
     on_device_log_signal _on_device_log;
     on_notification_signal _on_notification;
     on_calibration_changed_signal _on_calibration_changed;
+    dds_stream_profiles _open_profiles_list;
 };
 
 

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

@@ -192,6 +192,12 @@ void dds_device::open( const dds_stream_profiles & profiles )
     _impl->open( profiles );
 }
 
+void dds_device::close( const dds_stream_profiles & profiles )
+{
+    wait_until_ready( 0 );  // throw if not
+    _impl->close( profiles );
+}
+
 void dds_device::set_option_value( const std::shared_ptr< dds_option > & option, json new_value )
 {
     wait_until_ready( 0 );  // throw if not