PR #13386 from Eran: DDS changes and fixes

Author: maloel

src/backend-device-factory.cpp

@@ -145,14 +145,14 @@ backend_device_factory::backend_device_factory( std::shared_ptr< context > const
               for( auto & device_removed : subtract_sets( old_list, new_list ) )
               {
                   devices_removed.push_back( device_removed );
-                  LOG_DEBUG( "Device disconnected: " << device_removed->get_address() );
+                  LOG_INFO( "Device disconnected: " << device_removed->get_address() );
               }
 
               std::vector< std::shared_ptr< device_info > > devices_added;
               for( auto & device_added : subtract_sets( new_list, old_list ) )
               {
                   devices_added.push_back( device_added );
-                  LOG_DEBUG( "Device connected: " << device_added->get_address() );
+                  LOG_INFO( "Device connected: " << device_added->get_address() );
               }
 
               if( devices_removed.size() + devices_added.size() )

src/context.cpp

@@ -116,20 +116,20 @@ namespace librealsense {
         {
             for( auto & dev_info : factory->query_devices( requested_mask ) )
             {
-                LOG_INFO( "... " << dev_info->get_address() );
+                LOG_DEBUG( "... " << dev_info->get_address() );
                 list.push_back( dev_info );
             }
         }
         for( auto & item : _user_devices )
         {
             if( auto dev_info = item.second.lock() )
             {
-                LOG_INFO( "... " << dev_info->get_address() );
+                LOG_DEBUG( "... " << dev_info->get_address() );
                 list.push_back( dev_info );
             }
         }
-        LOG_INFO( "Found " << list.size() << " RealSense devices (0x" << std::hex << requested_mask << " requested & 0x"
-                           << get_device_mask() << " from device-mask in settings)" << std::dec );
+        LOG_DEBUG( "Found " << list.size() << " RealSense devices (0x" << std::hex << requested_mask
+                            << " requested & 0x" << get_device_mask() << " from device-mask in settings)" << std::dec );
         return list;
     }
 

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

@@ -149,7 +149,7 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const &
     , auto_calibrated_proxy()
     , _dds_dev( dev )
 {
-    LOG_DEBUG( "=====> dds-device-proxy " << this << " created on top of dds-device " << _dds_dev.get() );
+    //LOG_DEBUG( "=====> dds-device-proxy " << this << " created on top of dds-device " << _dds_dev.get() );
     register_info( RS2_CAMERA_INFO_NAME, dev->device_info().name() );
     register_info( RS2_CAMERA_INFO_PHYSICAL_PORT, dev->device_info().topic_root() );
     register_info( RS2_CAMERA_INFO_PRODUCT_ID, "DDS" );
@@ -236,8 +236,8 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const &
             sensor_info.proxy->add_dds_stream( sidx, stream );
             _stream_name_to_owning_sensor[stream->name()] = sensor_info.proxy;
             type_and_index_to_dds_stream_sidx.insert( { type_and_index, sidx }  );
-            LOG_DEBUG( sidx.to_string() << " " << get_string( sensor_info.type ) << " '" << stream->sensor_name()
-                                        << "' : '" << stream->name() << "' " << get_string( stream_type ) );
+            //LOG_DEBUG( sidx.to_string() << " " << get_string( sensor_info.type ) << " '" << stream->sensor_name()
+            //                            << "' : '" << stream->name() << "' " << get_string( stream_type ) );
 
             software_sensor & sensor = get_software_sensor( sensor_info.sensor_index );
             auto video_stream = std::dynamic_pointer_cast< realdds::dds_video_stream >( stream );
@@ -281,7 +281,7 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const &
 
     for( auto & sensor_info : sensor_name_to_info )
     {
-        LOG_DEBUG( sensor_info.first );
+        //LOG_DEBUG( sensor_info.first );
 
         // Set profile's ID based on the dds_stream's ID (index already set). Connect the profile to the extrinsics graph.
         for( auto & profile : sensor_info.second.proxy->get_stream_profiles() )
@@ -348,7 +348,7 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const &
                     auto const dds_extr = _dds_dev->get_extrinsics( from_stream.first, to_stream.first );
                     if( ! dds_extr )
                     {
-                        LOG_DEBUG( "missing extrinsics from " << from_stream.first << " to " << to_stream.first );
+                        //LOG_DEBUG( "missing extrinsics from " << from_stream.first << " to " << to_stream.first );
                         continue;
                     }
                     rs2_extrinsics extr = to_rs2_extrinsics( dds_extr );

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

@@ -306,30 +306,29 @@ void dds_sensor_proxy::handle_video_data( realdds::topics::image_msg && dds_fram
     data.backend_timestamp                        // time when the underlying backend (DDS) received it
         = static_cast<rs2_time_t>(realdds::time_to_double( dds_sample.reception_timestamp ) * 1e3);
     data.timestamp               // in ms
-        = static_cast< rs2_time_t >( realdds::time_to_double( dds_frame.timestamp ) * 1e3 );
+        = static_cast< rs2_time_t >( realdds::time_to_double( dds_frame.timestamp() ) * 1e3 );
     data.last_timestamp = streaming.last_timestamp.exchange( data.timestamp );
     data.timestamp_domain;  // from metadata, or leave default (hardware domain)
     data.depth_units;       // from metadata
     data.frame_number;      // filled in only once metadata is known
-    data.raw_size = static_cast< uint32_t >( dds_frame.raw_data.size() );
+    data.raw_size = static_cast< uint32_t >( dds_frame.raw().data().size() );
 
     auto vid_profile = dynamic_cast< video_stream_profile_interface * >( profile.get() );
     if( ! vid_profile )
         throw invalid_value_exception( "non-video profile provided to on_video_frame" );
 
-    auto stride = static_cast< int >( dds_frame.height > 0 ? dds_frame.raw_data.size() / dds_frame.height
-                                                           : dds_frame.raw_data.size() );
-    auto bpp = dds_frame.width > 0 ? stride / dds_frame.width : stride;
+    auto stride = static_cast< int >( dds_frame.height() > 0 ? data.raw_size / dds_frame.height() : data.raw_size );
+    auto bpp = dds_frame.width() > 0 ? stride / dds_frame.width() : stride;
     auto new_frame_interface = allocate_new_video_frame( vid_profile, stride, bpp, std::move( data ) );
     if( ! new_frame_interface )
         return;
 
     auto new_frame = static_cast< frame * >( new_frame_interface );
-    new_frame->data = std::move( dds_frame.raw_data );
+    new_frame->data = std::move( dds_frame.raw().data() );
 
     if( _md_enabled )
     {
-        streaming.syncer.enqueue_frame( dds_frame.timestamp.to_ns(), streaming.syncer.hold( new_frame ) );
+        streaming.syncer.enqueue_frame( dds_frame.timestamp().to_ns(), streaming.syncer.hold( new_frame ) );
     }
     else
     {
@@ -610,7 +609,7 @@ void dds_sensor_proxy::add_option( std::shared_ptr< realdds::dds_option > option
     if( get_option_handler( option_id ) )
         throw std::runtime_error( "option '" + option->get_name() + "' already exists in sensor" );
 
-    LOG_DEBUG( "... option -> " << option->get_name() );
+    //LOG_DEBUG( "... option -> " << option->get_name() );
     auto opt = std::make_shared< rs_dds_option >(
         option,
         [=]( json value )

src/dds/rsdds-device-factory.cpp

@@ -157,9 +157,15 @@ rsdds_device_factory::rsdds_device_factory( std::shared_ptr< context > const & c
                 std::vector< std::shared_ptr< device_info > > infos_removed;
                 auto dev_info = std::make_shared< dds_device_info >( ctx, dev );
                 if( added )
+                {
                     infos_added.push_back( dev_info );
+                    LOG_INFO( "Device connected: " << dev_info->get_address() );
+                }
                 else
+                {
                     infos_removed.push_back( dev_info );
+                    LOG_INFO( "Device disconnected: " << dev_info->get_address() );
+                }
                 cb( infos_removed, infos_added );
             } );
     }

src/environment.cpp

@@ -133,7 +133,7 @@ namespace librealsense
         }
 
         if (!invalid_ids.empty())
-            LOG_INFO("Found " << invalid_ids.size() << " unreachable streams, " << std::dec << counter << " extrinsics deleted");
+            LOG_DEBUG("Found " << invalid_ids.size() << " unreachable streams, " << std::dec << counter << " extrinsics deleted");
     }
 
     int extrinsics_graph::find_stream_profile(const stream_interface& p, bool add_if_not_there)

src/proc/formats-converter.cpp

@@ -97,7 +97,7 @@ stream_profiles formats_converter::get_all_possible_profiles( const stream_profi
 
     for( auto & raw_profile : raw_profiles )
     {
-        LOG_DEBUG( "Raw profile: " << raw_profile );
+        //LOG_DEBUG( "Raw profile: " << raw_profile );
         for( auto & pbf : _pb_factories )
         {
             const auto & sources = pbf->get_source_info();
@@ -129,7 +129,7 @@ stream_profiles formats_converter::get_all_possible_profiles( const stream_profi
                             target.resolution_transform( width, height );
                             cloned_vsp->set_dims( width, height );
                         }
-                        LOG_DEBUG( "          -> " << cloned_profile );
+                        //LOG_DEBUG( "          -> " << cloned_profile );
 
                         // Cache pbf supported profiles for efficiency in find_pbf_matching_most_profiles
                         _pbf_supported_profiles[pbf.get()].push_back( cloned_profile );

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

@@ -92,7 +92,7 @@ class dds_video_stream_server : public dds_stream_server
     void stop_streaming() override;
     image_header const & get_image_header() const { return _image_header; }
 
-    virtual void publish_image( topics::image_msg && );
+    virtual void publish_image( topics::image_msg & );
 
 private:
     void check_profile( std::shared_ptr< dds_stream_profile > const & ) const override;

third-party/realdds/include/realdds/topics/image-msg.h

@@ -5,6 +5,8 @@
 #include <realdds/dds-defines.h>
 #include <fastdds/rtps/common/Time_t.h>
 
+#include <realdds/topics/ros2/ros2image.h>
+
 #include <string>
 #include <memory>
 #include <vector>
@@ -13,7 +15,6 @@
 namespace sensor_msgs {
 namespace msg {
 class ImagePubSubType;
-class Image;
 }  // namespace msg
 }  // namespace sensor_msgs
 
@@ -31,6 +32,8 @@ namespace topics {
 
 class image_msg
 {
+    sensor_msgs::msg::Image _raw;
+
 public:
     using type = sensor_msgs::msg::ImagePubSubType;
 
@@ -46,8 +49,34 @@ class image_msg
     image_msg & operator=( image_msg && ) = default;
     image_msg & operator=( sensor_msgs::msg::Image && );
 
-    bool is_valid() const { return width != -1 && height != -1; }
-    void invalidate() { width = -1; }
+    bool is_valid() const { return ! _raw.data().empty(); }
+    void invalidate() { _raw.data().clear(); }
+
+    sensor_msgs::msg::Image & raw() { return _raw; }
+    sensor_msgs::msg::Image const & raw() const { return _raw; }
+
+    auto width() const { return _raw.width(); }
+    void set_width( uint32_t w ) { _raw.width( w ); }
+    auto height() const { return _raw.height(); }
+    void set_height( uint32_t h ) { _raw.height( h ); }
+    auto step() const { return _raw.step(); }
+    void set_step( uint32_t step ) { _raw.step( step ); }
+
+    std::string const & frame_id() const { return _raw.header().frame_id(); }
+    void set_frame_id( std::string new_id ) { _raw.header().frame_id( std::move( new_id ) ); }
+
+    std::string const & encoding() const { return _raw.encoding(); }
+    void set_encoding( std::string new_encoding ) { _raw.encoding( std::move( new_encoding ) ); }
+
+    bool is_bigendian() const { return _raw.is_bigendian(); }
+
+    void set_timestamp( dds_time const & t )
+    {
+        _raw.header().stamp().sec( t.seconds );
+        _raw.header().stamp().nanosec( t.nanosec );
+    }
+    dds_time timestamp() const { return { _raw.header().stamp().sec(), _raw.header().stamp().nanosec() }; }
+
 
     static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant,
                                                       char const * topic_name );
@@ -63,11 +92,6 @@ class image_msg
     static bool take_next( dds_topic_reader &,
                            image_msg * output,
                            dds_sample * optional_sample = nullptr );
-
-    std::vector< uint8_t > raw_data;
-    int width = -1;
-    int height = -1;
-    dds_time timestamp;
 };
 
 

third-party/realdds/include/realdds/topics/ros2/ros2header.h

@@ -13,7 +13,7 @@
 
 #include "ros2time.h"
 
-#include <fastrtps/utils/fixed_size_string.hpp>
+//#include <fastrtps/utils/fixed_size_string.hpp>
 
 #include <stdint.h>
 #include <array>