frame-vlidator added to l500 sensor

src/CMakeLists.txt

@@ -89,6 +89,7 @@ target_sources(${LRS_TARGET}
         "${CMAKE_CURRENT_LIST_DIR}/sync.cpp"
         "${CMAKE_CURRENT_LIST_DIR}/types.cpp"
         "${CMAKE_CURRENT_LIST_DIR}/verify.c"
+        "${CMAKE_CURRENT_LIST_DIR}/frame-validator.cpp"
 
         "${CMAKE_CURRENT_LIST_DIR}/algo.h"
         "${CMAKE_CURRENT_LIST_DIR}/api.h"
@@ -116,4 +117,5 @@ target_sources(${LRS_TARGET}
         "${CMAKE_CURRENT_LIST_DIR}/sync.h"
         "${CMAKE_CURRENT_LIST_DIR}/types.h"
         "${CMAKE_CURRENT_LIST_DIR}/command_transfer.h"
+        "${CMAKE_CURRENT_LIST_DIR}/frame-validator.h"
 )

src/frame-validator.cpp

@@ -0,0 +1,89 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright(c) 2018 Intel Corporation. All Rights Reserved.
+
+#include "frame-validator.h"
+
+namespace librealsense
+{
+    void frame_validator::on_frame(rs2_frame * f)
+    {
+        if (propagate((frame_interface*)f))
+        {
+            _user_callback->on_frame(f);
+            _sensor->set_frames_callback(_user_callback);
+        }
+    }
+
+    void frame_validator::release()
+    {
+
+    }
+
+    frame_validator::frame_validator(std::shared_ptr<sensor_interface> sensor, frame_callback_ptr user_callback, stream_profiles current_requests) :
+        _sensor(sensor), 
+        _user_callback(user_callback), 
+        _current_requests(current_requests)
+    {
+
+    }
+
+    frame_validator::~frame_validator()
+    {
+
+    }
+
+    bool frame_validator::propagate(frame_interface* frame)
+    {
+        auto vf = dynamic_cast<video_frame*>(frame);
+        if (vf == nullptr) {
+            return true;
+        }
+        auto stream = vf->get_stream();
+        if (stream->get_stream_type() != RS2_STREAM_DEPTH)
+            return false;
+
+        auto w = vf->get_width();
+        auto h = vf->get_height();
+        auto bpp = vf->get_bpp() / 8;
+        auto data = reinterpret_cast<const uint16_t*>(vf->get_frame_data());
+
+        bool non_empty_pixel_found = false;
+
+        int y_begin = h * 0.5 - h * 0.05;
+        int y_end = h * 0.5 + h * 0.05;
+        int x_begin = w * 0.5 - w * 0.05;
+        int x_end = w * 0.5 + w * 0.05;
+        for (int y = y_begin; y < y_end; y++)
+        {
+            for (int x = x_begin; x < x_end; x++)
+            {
+                auto index = x + y * w;
+                if (data[index] != 0)
+                {
+                    non_empty_pixel_found = true;
+                    break;
+                }
+            }
+        }
+
+        if (non_empty_pixel_found)
+            return true;
+        else
+        {
+            LOG_ERROR("frame_source recieved an empty depth frame, restarting the sensor...");
+            auto s = _sensor;
+            auto cr = _current_requests;
+            auto uc = _user_callback;
+            _reset_thread = std::thread([s, cr, uc]()
+            {
+                s->stop();
+                s->close();
+                s->open(cr);
+                s->start(uc);
+            });
+            _reset_thread.detach();
+            //std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+        }
+        return false;
+    }
+}

src/frame-validator.h

@@ -0,0 +1,26 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright(c) 2015 Intel Corporation. All Rights Reserved.
+
+#pragma once
+
+#include "sensor.h"
+
+namespace librealsense
+{
+    class frame_validator : public rs2_frame_callback
+    {
+    public:
+        explicit frame_validator(std::shared_ptr<sensor_interface> sensor, frame_callback_ptr user_callback, stream_profiles current_requests);
+        virtual ~frame_validator();
+
+        void on_frame(rs2_frame * f) override;
+        void release() override;
+    private:
+        bool propagate(frame_interface* frame);
+
+        std::thread _reset_thread;
+        frame_callback_ptr _user_callback;
+        stream_profiles _current_requests;
+        std::shared_ptr<sensor_interface> _sensor;
+    };
+}

src/l500/l500-depth.cpp

@@ -15,6 +15,7 @@
 #include "proc/zero-order.h"
 #include <cstddef>
 #include "metadata-parser.h"
+#include "frame-validator.h"
 
 #define MM_TO_METER 1/1000
 #define MIN_ALGO_VERSION 115
@@ -240,4 +241,15 @@ namespace librealsense
         return res;
     }
 
+    void l500_depth_sensor::start(frame_callback_ptr callback)
+    {
+        uvc_sensor::start(std::make_shared<frame_validator>(shared_from_this(), callback, _current_requests));
+    }
+
+    void l500_depth_sensor::open(const stream_profiles& requests)
+    {
+        _current_requests = requests;
+        uvc_sensor::open(requests);
+    }
+
 }

src/l500/l500-depth.h

@@ -150,8 +150,11 @@ namespace librealsense
         float read_baseline();
         float read_znorm();
 
+        void start(frame_callback_ptr callback) override;
+        void open(const stream_profiles& requests) override;
     private:
         const l500_device* _owner;
         float _depth_units;
+        stream_profiles _current_requests;
     };
 }