From eeb70a499f01bab7a17708f42518936d7b42fbd0 Mon Sep 17 00:00:00 2001 From: "Katz, Matan" Date: Tue, 20 Aug 2019 13:04:36 +0300 Subject: [PATCH] frame-vlidator added to l500 sensor --- src/CMakeLists.txt | 2 + src/frame-validator.cpp | 89 +++++++++++++++++++++++++++++++++++++++++ src/frame-validator.h | 26 ++++++++++++ src/l500/l500-depth.cpp | 12 ++++++ src/l500/l500-depth.h | 3 ++ 5 files changed, 132 insertions(+) create mode 100644 src/frame-validator.cpp create mode 100644 src/frame-validator.h diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 4ba6b561a4..c75c67f78f 100644 --- a/src/CMakeLists.txt +++ b/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" ) diff --git a/src/frame-validator.cpp b/src/frame-validator.cpp new file mode 100644 index 0000000000..02d25c62ab --- /dev/null +++ b/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, 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(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(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; + } +} diff --git a/src/frame-validator.h b/src/frame-validator.h new file mode 100644 index 0000000000..b8b168540c --- /dev/null +++ b/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, 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; + }; +} diff --git a/src/l500/l500-depth.cpp b/src/l500/l500-depth.cpp index 67e88739dc..57e34c4458 100644 --- a/src/l500/l500-depth.cpp +++ b/src/l500/l500-depth.cpp @@ -15,6 +15,7 @@ #include "proc/zero-order.h" #include #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(shared_from_this(), callback, _current_requests)); + } + + void l500_depth_sensor::open(const stream_profiles& requests) + { + _current_requests = requests; + uvc_sensor::open(requests); + } + } diff --git a/src/l500/l500-depth.h b/src/l500/l500-depth.h index 88f997f8e7..441bfc08c4 100644 --- a/src/l500/l500-depth.h +++ b/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; }; }