L500 support for FG with debug_stream_sensor

include/librealsense2/h/rs_sensor.h

@@ -325,6 +325,14 @@ const char* rs2_get_notification_serialized_data(rs2_notification* notification,
 */
 rs2_stream_profile_list* rs2_get_stream_profiles(rs2_sensor* sensor, rs2_error** error);
 
+/**
+* check if physical subdevice is supported
+* \param[in] sensor  input RealSense subdevice
+* \param[out] error  if non-null, receives any error that occurs during this call, otherwise, errors are ignored
+* \return            list of debug stream profiles that given subdevice can provide, should be released by rs2_delete_profiles_list
+*/
+rs2_stream_profile_list * rs2_get_debug_stream_profiles( rs2_sensor * sensor, rs2_error ** error );
+
 /**
 * check how subdevice is streaming
 * \param[in] sensor  input RealSense subdevice

include/librealsense2/h/rs_types.h

@@ -213,6 +213,7 @@ typedef enum rs2_extension
     RS2_EXTENSION_HDR_MERGE,
     RS2_EXTENSION_SEQUENCE_ID_FILTER,
     RS2_EXTENSION_MAX_USABLE_RANGE_SENSOR,
+    RS2_EXTENSION_DEBUG_STREAM_SENSOR,
     RS2_EXTENSION_COUNT
 } rs2_extension;
 const char* rs2_extension_type_to_string(rs2_extension type);

include/librealsense2/hpp/rs_sensor.hpp

@@ -752,5 +752,49 @@ namespace rs2
             return res;
         }
     };
-}
+
+    class debug_streaming_sensor : public sensor
+    {
+    public:
+        debug_streaming_sensor( sensor s )
+            : sensor( s.get() )
+        {
+            rs2_error * e = nullptr;
+            if( rs2_is_sensor_extendable_to( _sensor.get(), RS2_EXTENSION_DEBUG_STREAM_SENSOR, &e ) == 0 && ! e )
+            {
+                _sensor.reset();
+            }
+            error::handle( e );
+        }
+
+        operator bool() const { return _sensor.get() != nullptr; }
+
+        /**
+        * Retrieves the list of debug stream profiles supported by the sensor.
+        * \return   list of debug stream profiles that given sensor can provide
+        */
+        std::vector< stream_profile > get_debug_stream_profiles() const
+        {
+            std::vector< stream_profile > results{};
+
+            rs2_error * e = nullptr;
+            std::shared_ptr< rs2_stream_profile_list > list(
+                rs2_get_debug_stream_profiles( _sensor.get(), &e ),
+                rs2_delete_stream_profiles_list );
+            error::handle( e );
+
+            auto size = rs2_get_stream_profiles_count( list.get(), &e );
+            error::handle( e );
+
+            for( auto i = 0; i < size; i++ )
+            {
+                stream_profile profile( rs2_get_stream_profile( list.get(), i, &e ) );
+                error::handle( e );
+                results.push_back( profile );
+            }
+
+            return results;
+        }
+    };
+    }
 #endif // LIBREALSENSE_RS2_SENSOR_HPP

src/CMakeLists.txt

@@ -133,4 +133,5 @@ target_sources(${LRS_TARGET}
         "${CMAKE_CURRENT_LIST_DIR}/serializable-interface.h"
         "${CMAKE_CURRENT_LIST_DIR}/depth-to-rgb-calibration.h"
         "${CMAKE_CURRENT_LIST_DIR}/max-usable-range-sensor.h"
+        "${CMAKE_CURRENT_LIST_DIR}/debug-streams-sensor.h"
 )

src/core/streaming.h

@@ -34,6 +34,7 @@ namespace librealsense
         PROFILE_TAG_ANY = 0,
         PROFILE_TAG_SUPERSET = 1, // to be included in enable_all
         PROFILE_TAG_DEFAULT = 2,  // to be included in default pipeline start
+        PROFILE_TAG_DEBUG = 4,    // tag for debug formats
     } profile_tag;
 
     struct tagged_profile

src/debug-streams-sensor.h

@@ -0,0 +1,16 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright(c) 2020 Intel Corporation. All Rights Reserved.
+
+#pragma once
+
+#include "core/streaming.h"
+
+namespace librealsense
+{
+    class debug_streaming_sensor
+    {
+    public:
+        virtual stream_profiles get_debug_stream_profiles() = 0;
+    };
+    MAP_EXTENSION( RS2_EXTENSION_DEBUG_STREAM_SENSOR, debug_streaming_sensor );
+    }

src/l500/l500-depth.cpp

@@ -174,7 +174,8 @@ namespace librealsense
         tags.push_back({ RS2_STREAM_DEPTH, -1, width, height, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
         tags.push_back({ RS2_STREAM_INFRARED, -1, width, height, RS2_FORMAT_Y8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
         tags.push_back({ RS2_STREAM_CONFIDENCE, -1, width, height, RS2_FORMAT_RAW8, 30, profile_tag::PROFILE_TAG_SUPERSET });
-
+        tags.push_back({ RS2_STREAM_DEPTH, -1, 800, 600, RS2_FORMAT_FG, 30, profile_tag::PROFILE_TAG_DEBUG } );
+        tags.push_back({ RS2_STREAM_DEPTH, -1, 1280, 720, RS2_FORMAT_FG, 30, profile_tag::PROFILE_TAG_DEBUG } );
         return tags;
     }
 
@@ -270,6 +271,11 @@ namespace librealsense
             l500_depth_fourcc_to_rs2_format_map,
             l500_depth_fourcc_to_rs2_stream_map )
         , _owner( owner )
+        , _debug_profiles( [&, uvc_sensor]() {
+            auto profiles = init_debug_stream_profiles( uvc_sensor );
+            _owner->tag_profiles( profiles );
+            return profiles;
+        } )
     {
 
         register_option( RS2_OPTION_DEPTH_UNITS, std::make_shared<const_value_option>( "Number of meters represented by a single depth unit",
@@ -282,6 +288,79 @@ namespace librealsense
 
     }
 
+    stream_profiles
+    l500_depth_sensor::init_debug_stream_profiles( std::shared_ptr< uvc_sensor > uvc_sensor )
+    {
+        stream_profiles result_profiles;
+        auto profiles = uvc_sensor->init_stream_profiles();
+        _owner->tag_profiles( profiles );
+
+        for( auto && pbf : synthetic_sensor::_pb_factories )
+        {
+            const auto && sources = pbf->get_source_info();
+            const auto && targets = pbf->get_target_info();
+
+            for( auto && source : sources )
+            {
+                // add profiles that are supported by the device
+                for( auto & profile : profiles )
+                {
+                    if( profile->get_tag() == PROFILE_TAG_DEBUG
+                        && profile->get_format() == source.format
+                        && ( source.stream == profile->get_stream_type()
+                             || source.stream == RS2_STREAM_ANY ) )
+                    {
+                        for( auto target : targets )
+                        {
+                            target.fps = profile->get_framerate();
+
+                            auto && cloned_profile = clone_profile( profile );
+                            cloned_profile->set_format( target.format );
+                            cloned_profile->set_stream_index( target.index );
+                            cloned_profile->set_stream_type( target.stream );
+
+                            auto && cloned_vsp
+                                = As< video_stream_profile, stream_profile_interface >(
+                                    cloned_profile );
+                            if( cloned_vsp )
+                            {
+                                const auto && res = target.stream_resolution(
+                                    { cloned_vsp->get_width(), cloned_vsp->get_height() } );
+                                target.height = res.height;
+                                target.width = res.width;
+                                cloned_vsp->set_dims( target.width, target.height );
+                            }
+
+                            // Add the cloned profile to the supported profiles by this processing
+                            // block factory, for later processing validation in resolving the
+                            // request.
+                            _pbf_supported_profiles[pbf.get()].push_back( cloned_profile );
+
+                            // cache the source to target mapping
+                            _source_to_target_profiles_map[profile].push_back( cloned_profile );
+                            // cache each target profile to its source profiles which were generated
+                            // from.
+                            _target_to_source_profiles_map[target].push_back( profile );
+
+                            // disregard duplicated from profiles list
+                            if( is_duplicated_profile( cloned_profile, result_profiles ) )
+                                continue;
+
+                            // Only injective cloning in many to one mapping.
+                            // One to many is not affected.
+                            if( sources.size() > 1 && target.format != source.format )
+                                continue;
+
+                            result_profiles.push_back( cloned_profile );
+                        }
+                    }
+                }
+            }
+        }
+        sort_profiles( &result_profiles );
+        return result_profiles;
+    }
+
     l500_depth_sensor::~l500_depth_sensor()
     {
         _owner->stop_temperatures_reader();
@@ -379,6 +458,19 @@ namespace librealsense
        return l500::max_usable_range(noise_estimation);
     }
 
+    stream_profiles l500_depth_sensor::get_debug_stream_profiles()
+    {
+        stream_profiles results;
+
+        for( auto p : *_debug_profiles )
+        {
+            auto curr_tag = p->get_tag();
+            if(  curr_tag & PROFILE_TAG_DEBUG )
+                results.push_back( p );
+        }
+        return results;
+    }
+
     // We want to disable max-usable-range when not in a particular preset:
     bool l500_depth_sensor::is_max_range_preset() const
     {

src/l500/l500-depth.h

@@ -19,6 +19,7 @@
 #include "l500-options.h"
 #include "calibrated-sensor.h"
 #include "max-usable-range-sensor.h"
+#include "debug-streams-sensor.h"
 
 namespace librealsense
 {
@@ -93,6 +94,7 @@ namespace librealsense
         , public virtual l500_depth_sensor_interface
         , public calibrated_sensor
         , public max_usable_range_sensor
+        , public debug_streaming_sensor
     {
     public:
         explicit l500_depth_sensor(
@@ -221,6 +223,8 @@ namespace librealsense
 
         float get_max_usable_depth_range() const override;
 
+        stream_profiles get_debug_stream_profiles() override;
+
         void create_snapshot(std::shared_ptr<depth_sensor>& snapshot) const override
         {
             snapshot = std::make_shared<depth_sensor_snapshot>(get_depth_scale());
@@ -260,7 +264,9 @@ namespace librealsense
         bool is_max_range_preset() const;
 
     private:
+        stream_profiles init_debug_stream_profiles( std::shared_ptr< uvc_sensor > uvc_sensor );
 
+        lazy< stream_profiles > _debug_profiles;
         action_delayer _action_delayer;
         l500_device * const _owner;
         float _depth_units;

src/realsense.def

@@ -398,5 +398,6 @@ EXPORTS
     rs2_terminal_parse_response
 
     rs2_get_max_usable_depth_range
+    rs2_get_debug_stream_profiles
 
 

src/rs.cpp

@@ -331,6 +331,16 @@ rs2_stream_profile_list* rs2_get_stream_profiles(rs2_sensor* sensor, rs2_error**
 }
 HANDLE_EXCEPTIONS_AND_RETURN(nullptr, sensor)
 
+rs2_stream_profile_list * rs2_get_debug_stream_profiles( rs2_sensor * sensor,
+                                                   rs2_error ** error ) BEGIN_API_CALL
+{
+    VALIDATE_NOT_NULL( sensor );
+    auto debug_streaming
+        = VALIDATE_INTERFACE( sensor->sensor, librealsense::debug_streaming_sensor );
+    return new rs2_stream_profile_list{ debug_streaming->get_debug_stream_profiles() };
+}
+HANDLE_EXCEPTIONS_AND_RETURN( nullptr, sensor )
+
 rs2_stream_profile_list* rs2_get_active_streams(rs2_sensor* sensor, rs2_error** error) BEGIN_API_CALL
 {
     VALIDATE_NOT_NULL(sensor);
@@ -1390,6 +1400,7 @@ int rs2_is_sensor_extendable_to(const rs2_sensor* sensor, rs2_extension extensio
     case RS2_EXTENSION_FISHEYE_SENSOR          : return VALIDATE_INTERFACE_NO_THROW(sensor->sensor, librealsense::fisheye_sensor)         != nullptr;
     case RS2_EXTENSION_CALIBRATED_SENSOR       : return VALIDATE_INTERFACE_NO_THROW(sensor->sensor, librealsense::calibrated_sensor)      != nullptr;
     case RS2_EXTENSION_MAX_USABLE_RANGE_SENSOR : return VALIDATE_INTERFACE_NO_THROW(sensor->sensor, librealsense::max_usable_range_sensor) != nullptr;
+    case RS2_EXTENSION_DEBUG_STREAM_SENSOR     : return VALIDATE_INTERFACE_NO_THROW( sensor->sensor, librealsense::debug_streaming_sensor ) != nullptr;
 
 
     default:

src/sensor.cpp

@@ -192,10 +192,18 @@ namespace librealsense
 
     stream_profiles sensor_base::get_stream_profiles(int tag) const
     {
+        stream_profiles results;
+
         if (tag == profile_tag::PROFILE_TAG_ANY)
-            return *_profiles;
+        {
+            for( auto p : *_profiles )
+            {
+                auto curr_tag = p->get_tag();
+                if( !(curr_tag & PROFILE_TAG_DEBUG) )
+                    results.push_back( p );
+            }
+        }
 
-        stream_profiles results;
         for (auto p : *_profiles)
         {
             auto curr_tag = p->get_tag();

src/sensor.h

@@ -227,27 +227,33 @@ namespace librealsense
 
     protected:
         void add_source_profiles_missing_data();
+        std::vector< std::shared_ptr< processing_block_factory > > _pb_factories;
+        std::shared_ptr< stream_profile_interface >
+        clone_profile( const std::shared_ptr< stream_profile_interface > & profile );
+        std::unordered_map< processing_block_factory *, stream_profiles > _pbf_supported_profiles;
+        std::unordered_map< std::shared_ptr< stream_profile_interface >, stream_profiles >
+            _source_to_target_profiles_map;
+        std::unordered_map< stream_profile, stream_profiles > _target_to_source_profiles_map;
+        bool is_duplicated_profile( const std::shared_ptr< stream_profile_interface > & duplicate,
+                                    const stream_profiles & profiles );
+        void sort_profiles( stream_profiles * profiles );
 
     private:
         stream_profiles resolve_requests(const stream_profiles& requests);
         std::shared_ptr<stream_profile_interface> filter_frame_by_requests(const frame_interface* f);
-        void sort_profiles(stream_profiles * profiles);
+
         std::pair<std::shared_ptr<processing_block_factory>, stream_profiles> find_requests_best_pb_match(const stream_profiles& sp);
         void add_source_profile_missing_data(std::shared_ptr<stream_profile_interface>& source_profile);
-        bool is_duplicated_profile(const std::shared_ptr<stream_profile_interface>& duplicate, const stream_profiles& profiles);
-        std::shared_ptr<stream_profile_interface> clone_profile(const std::shared_ptr<stream_profile_interface>& profile);
+
         void register_processing_block_options(const processing_block& pb);
         void unregister_processing_block_options(const processing_block& pb);
 
         std::mutex _synthetic_configure_lock;
 
         frame_callback_ptr _post_process_callback;
         std::shared_ptr<sensor_base> _raw_sensor;
-        std::vector<std::shared_ptr<processing_block_factory>> _pb_factories;
-        std::unordered_map<processing_block_factory*, stream_profiles> _pbf_supported_profiles;
+
         std::unordered_map<std::shared_ptr<stream_profile_interface>, std::unordered_set<std::shared_ptr<processing_block>>> _profiles_to_processing_block;
-        std::unordered_map<std::shared_ptr<stream_profile_interface>, stream_profiles> _source_to_target_profiles_map;
-        std::unordered_map<stream_profile, stream_profiles> _target_to_source_profiles_map;
         std::unordered_map<rs2_format, stream_profiles> _cached_requests;
         std::vector<rs2_option> _cached_processing_blocks_options;
     };
@@ -346,9 +352,9 @@ namespace librealsense
             power on(std::dynamic_pointer_cast<uvc_sensor>(shared_from_this()));
             return action(*_device);
         }
+        stream_profiles init_stream_profiles() override;
 
     protected:
-        stream_profiles init_stream_profiles() override;
         rs2_extension stream_to_frame_types(rs2_stream stream) const;
 
     private:

src/types.cpp

@@ -324,6 +324,7 @@ namespace librealsense
             CASE(SEQUENCE_ID_FILTER)
             CASE(HDR_MERGE)
             CASE(MAX_USABLE_RANGE_SENSOR)
+            CASE(DEBUG_STREAM_SENSOR)
         default: assert(!is_valid(value)); return UNKNOWN_VALUE;
         }
 #undef CASE