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);
 
+/**
+* retrieve list of debug stream profiles that given subdevice can provide
+* \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_stream_sensor : public sensor
+    {
+    public:
+        debug_stream_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-stream-sensor.h"
 )

src/core/streaming.h

@@ -31,9 +31,10 @@ namespace librealsense
 
     typedef enum profile_tag
     {
-        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_ANY = 4,      // does not include PROFILE_TAG_DEBUG
+        PROFILE_TAG_DEBUG = 8,    // tag for debug formats
     } profile_tag;
 
     struct tagged_profile

src/debug-stream-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_stream_sensor
+    {
+    public:
+        virtual stream_profiles get_debug_stream_profiles() const= 0;
+    };
+    MAP_EXTENSION( RS2_EXTENSION_DEBUG_STREAM_SENSOR, debug_stream_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;
     }
 
@@ -379,6 +380,11 @@ namespace librealsense
        return l500::max_usable_range(noise_estimation);
     }
 
+    stream_profiles l500_depth_sensor::get_debug_stream_profiles() const
+    {
+       return get_stream_profiles( PROFILE_TAG_DEBUG );
+    }
+
     // 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-stream-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_stream_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() const 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,6 @@ namespace librealsense
         bool is_max_range_preset() const;
 
     private:
-
         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_stream_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_stream_sensor ) != nullptr;
 
 
     default:

src/sensor.cpp

@@ -190,17 +190,19 @@ namespace librealsense
         _source_owner = owner;
     }
 
-    stream_profiles sensor_base::get_stream_profiles(int tag) const
+    stream_profiles sensor_base::get_stream_profiles( int tag ) const
     {
-        if (tag == profile_tag::PROFILE_TAG_ANY)
-            return *_profiles;
-
         stream_profiles results;
-        for (auto p : *_profiles)
+
+        for( auto p : *_profiles )
         {
             auto curr_tag = p->get_tag();
-            if (curr_tag & tag)
-                results.push_back(p);
+            if( curr_tag & tag
+                || tag == profile_tag::PROFILE_TAG_ANY
+                       && ! ( curr_tag & profile_tag::PROFILE_TAG_DEBUG ) )
+            {
+                results.push_back( p );
+            }
         }
 
         return results;
@@ -548,7 +550,6 @@ namespace librealsense
         std::unordered_set<std::shared_ptr<video_stream_profile>> profiles;
         power on(std::dynamic_pointer_cast<uvc_sensor>(shared_from_this()));
 
-        if (_uvc_profiles.empty()) {}
         _uvc_profiles = _device->get_profiles();
 
         for (auto&& p : _uvc_profiles)
@@ -1210,7 +1211,7 @@ namespace librealsense
     stream_profiles synthetic_sensor::init_stream_profiles()
     {
         stream_profiles result_profiles;
-        auto profiles = _raw_sensor->get_stream_profiles();
+        auto profiles = _raw_sensor->get_stream_profiles( PROFILE_TAG_ANY | PROFILE_TAG_DEBUG );
 
         for (auto&& pbf : _pb_factories)
         {

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

unit-tests/func/fg/test-fg.py

@@ -0,0 +1,67 @@
+import pyrealsense2 as rs, test, ac
+
+dev = test.find_devices_by_product_line_or_exit(8)
+depth_sensor = dev[0].first_depth_sensor()
+
+debug_sensor = rs.debug_stream_sensor(depth_sensor)
+debug_profiles = debug_sensor.get_debug_stream_profiles()
+
+
+#############################################################################################
+test.start("FG doesn't exposed by get_stream_profiles ")
+
+matches = list(p for p in depth_sensor.profiles if p.format() == rs.format.fg)
+test.check(len(matches) == 0 )
+test.finish()
+
+#############################################################################################
+test.start("FG exposed by debug_stream_sensor ")
+
+matches = list(p for p in debug_profiles if p.format() == rs.format.fg)
+test.check(len(matches) > 0 )
+test.finish()
+
+#############################################################################################
+test.start("streaming FG 800x600")
+
+dp = next(p for p in debug_profiles if p.fps() == 30
+                        and p.stream_type() == rs.stream.depth
+                        and p.format() == rs.format.fg
+                        and p.as_video_stream_profile().width() == 800
+                        and p.as_video_stream_profile().height() == 600)
+depth_sensor.open( dp )
+lrs_queue = rs.frame_queue(capacity=10, keep_frames=False)
+depth_sensor.start( lrs_queue )
+
+try:
+    lrs_frame = lrs_queue.wait_for_frame(150000)
+    debug_sensor.stop()
+    debug_sensor.close()
+    test.check_equal(lrs_frame.profile.format(), rs.format.fg)
+except:
+    test.unexpected_exception()
+test.finish()
+
+#############################################################################################
+test.start("streaming FG 1280x720")
+
+dp = next(p for p in debug_profiles if p.fps() == 30
+                        and p.stream_type() == rs.stream.depth
+                        and p.format() == rs.format.fg
+                        and p.as_video_stream_profile().width() == 1280
+                        and p.as_video_stream_profile().height() == 720)
+depth_sensor.open( dp )
+lrs_queue = rs.frame_queue(capacity=10, keep_frames=False)
+depth_sensor.start( lrs_queue )
+
+try:
+    lrs_frame = lrs_queue.wait_for_frame(5000)
+    debug_sensor.stop()
+    debug_sensor.close()
+    test.check_equal(lrs_frame.profile.format(), rs.format.fg)
+except:
+    test.unexpected_exception()
+test.finish()
+#############################################################################################
+
+test.print_results_and_exit()

unit-tests/py/test.py

@@ -39,6 +39,17 @@ def find_first_device_or_exit():
         exit(0)
     return c.devices[0]
 
+# Returns a list of devices of specific product line that was found, if no device is found the test is skipped.
+# That way we can still run the unit-tests when no device is connected
+# and not fail the tests that check a connected device
+def find_devices_by_product_line_or_exit(product):
+    c = rs.context()
+    devices_list = c.query_devices(product)
+    if devices_list.size() == 0:
+        print("No device of the " , product ," product line was found; skipping test")
+        exit(0)
+    return devices_list
+
 # Function for printing the current call stack. Used when an assertion fails
 def print_stack():
     for line in traceback.format_stack():

wrappers/nodejs/src/addon.cpp

@@ -4905,6 +4905,7 @@ void InitModule(v8::Local<v8::Object> exports) {
   _FORCE_SET_ENUM(RS2_EXTENSION_HDR_MERGE);
   _FORCE_SET_ENUM(RS2_EXTENSION_SEQUENCE_ID_FILTER);
   _FORCE_SET_ENUM(RS2_EXTENSION_MAX_USABLE_RANGE_SENSOR);
+  _FORCE_SET_ENUM(RS2_EXTENSION_DEBUG_STREAM_SENSOR);
   _FORCE_SET_ENUM(RS2_EXTENSION_COUNT);
 
 

wrappers/python/pyrs_context.cpp

@@ -20,6 +20,8 @@ void init_context(py::module &m) {
     context.def(py::init<>())
         .def("query_devices", (rs2::device_list(rs2::context::*)() const) &rs2::context::query_devices, "Create a static"
              " snapshot of all connected devices at the time of the call.")
+        .def( "query_devices", ( rs2::device_list( rs2::context::* )(int) const ) & rs2::context::query_devices, "Create a static"
+              " snapshot of all connected devices of specific product line at the time of the call." )
         .def_property_readonly("devices", (rs2::device_list(rs2::context::*)() const) &rs2::context::query_devices,
                                "A static snapshot of all connected devices at time of access. Identical to calling query_devices.")
         .def("query_all_sensors", &rs2::context::query_all_sensors, "Generate a flat list of "

wrappers/python/pyrs_sensor.cpp

@@ -76,8 +76,8 @@ void init_sensor(py::module &m) {
         .def(BIND_DOWNCAST(sensor, pose_sensor))
         .def(BIND_DOWNCAST(sensor, calibrated_sensor))
         .def(BIND_DOWNCAST(sensor, wheel_odometer))
-        .def(BIND_DOWNCAST(sensor, max_usable_range_sensor));
-
+        .def(BIND_DOWNCAST(sensor, max_usable_range_sensor))
+        .def(BIND_DOWNCAST(sensor, debug_stream_sensor));
     // rs2::sensor_from_frame [frame.def("get_sensor", ...)?
     // rs2::sensor==sensor?
 
@@ -133,6 +133,12 @@ void init_sensor(py::module &m) {
             &rs2::max_usable_range_sensor::get_max_usable_depth_range,
             py::call_guard< py::gil_scoped_release >());
 
+    py::class_< rs2::debug_stream_sensor, rs2::sensor > ds_sensor( m, "debug_stream_sensor" );
+    ds_sensor.def( py::init< rs2::sensor >(), "sensor"_a )
+        .def( "get_debug_stream_profiles",
+               &rs2::debug_stream_sensor::get_debug_stream_profiles,
+              py::call_guard< py::gil_scoped_release >() );
+
     // rs2::depth_stereo_sensor
     py::class_<rs2::depth_stereo_sensor, rs2::depth_sensor> depth_stereo_sensor(m, "depth_stereo_sensor"); // No docstring in C++
     depth_stereo_sensor.def(py::init<rs2::sensor>())

wrappers/python/python.hpp

@@ -186,6 +186,7 @@ template<template<rs2_format> class F>
     case RS2_FORMAT_INZI: return F<RS2_FORMAT_INZI>::func();
     case RS2_FORMAT_INVI: return F<RS2_FORMAT_INVI>::func();
     case RS2_FORMAT_W10: return F<RS2_FORMAT_W10>::func();
+    case RS2_FORMAT_FG: return F<RS2_FORMAT_FG>::func();
     // c++11 standard doesn't allow throw in constexpr function switch case
     case RS2_FORMAT_COUNT: throw std::runtime_error("format.count is not a valid value for arguments of type format!");
     default: return F<RS2_FORMAT_ANY>::func();