From ab391c9e81f45175c13ec862ce7030211892960f Mon Sep 17 00:00:00 2001 From: Eran Date: Fri, 3 Nov 2023 07:21:28 +0200 Subject: [PATCH] software-device, -sensor --- src/CMakeLists.txt | 4 + src/dds/rs-dds-device-info.cpp | 2 + src/dds/rs-dds-sensor-proxy.cpp | 2 + src/dds/rs-dds-sensor-proxy.h | 2 +- src/proc/spatial-filter.cpp | 7 +- src/rs.cpp | 13 +- src/software-device-info.cpp | 44 ++++ src/software-device-info.h | 35 +++ src/software-device.cpp | 324 +-------------------------- src/software-device.h | 199 ++++------------- src/software-sensor.cpp | 375 ++++++++++++++++++++++++++++++++ src/software-sensor.h | 99 +++++++++ src/types.h | 1 - 13 files changed, 614 insertions(+), 493 deletions(-) create mode 100644 src/software-device-info.cpp create mode 100644 src/software-device-info.h create mode 100644 src/software-sensor.cpp create mode 100644 src/software-sensor.h diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index eddfd8065b..d0e69e1f0d 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -89,6 +89,8 @@ target_sources(${LRS_TARGET} "${CMAKE_CURRENT_LIST_DIR}/hid-sensor.cpp" "${CMAKE_CURRENT_LIST_DIR}/uvc-sensor.cpp" "${CMAKE_CURRENT_LIST_DIR}/software-device.cpp" + "${CMAKE_CURRENT_LIST_DIR}/software-device-info.cpp" + "${CMAKE_CURRENT_LIST_DIR}/software-sensor.cpp" "${CMAKE_CURRENT_LIST_DIR}/source.cpp" "${CMAKE_CURRENT_LIST_DIR}/stream.cpp" "${CMAKE_CURRENT_LIST_DIR}/sync.cpp" @@ -140,6 +142,8 @@ target_sources(${LRS_TARGET} "${CMAKE_CURRENT_LIST_DIR}/hid-sensor.h" "${CMAKE_CURRENT_LIST_DIR}/uvc-sensor.h" "${CMAKE_CURRENT_LIST_DIR}/software-device.h" + "${CMAKE_CURRENT_LIST_DIR}/software-device-info.h" + "${CMAKE_CURRENT_LIST_DIR}/software-sensor.h" "${CMAKE_CURRENT_LIST_DIR}/source.h" "${CMAKE_CURRENT_LIST_DIR}/stream.h" "${CMAKE_CURRENT_LIST_DIR}/sync.h" diff --git a/src/dds/rs-dds-device-info.cpp b/src/dds/rs-dds-device-info.cpp index f94edf51a9..f871227143 100644 --- a/src/dds/rs-dds-device-info.cpp +++ b/src/dds/rs-dds-device-info.cpp @@ -9,6 +9,8 @@ #include #include +#include + namespace librealsense { diff --git a/src/dds/rs-dds-sensor-proxy.cpp b/src/dds/rs-dds-sensor-proxy.cpp index 0eb9bd5545..bf5a505726 100644 --- a/src/dds/rs-dds-sensor-proxy.cpp +++ b/src/dds/rs-dds-sensor-proxy.cpp @@ -4,6 +4,8 @@ #include "rs-dds-sensor-proxy.h" #include "rs-dds-option.h" +#include + #include #include diff --git a/src/dds/rs-dds-sensor-proxy.h b/src/dds/rs-dds-sensor-proxy.h index 184fbdc85a..8d504c40bf 100644 --- a/src/dds/rs-dds-sensor-proxy.h +++ b/src/dds/rs-dds-sensor-proxy.h @@ -5,7 +5,7 @@ #include "sid_index.h" -#include +#include #include #include diff --git a/src/proc/spatial-filter.cpp b/src/proc/spatial-filter.cpp index d2b90f4157..a3a9735928 100644 --- a/src/proc/spatial-filter.cpp +++ b/src/proc/spatial-filter.cpp @@ -1,15 +1,16 @@ // License: Apache 2.0. See LICENSE file in root directory. // Copyright(c) 2017 Intel Corporation. All Rights Reserved. -#include -#include #include "option.h" #include "environment.h" -#include "software-device.h" +#include "software-sensor.h" #include "proc/synthetic-stream.h" #include "proc/hole-filling-filter.h" #include "proc/spatial-filter.h" +#include +#include + #include diff --git a/src/rs.cpp b/src/rs.cpp index c546c9a966..bc0dc12008 100644 --- a/src/rs.cpp +++ b/src/rs.cpp @@ -44,6 +44,8 @@ #include "environment.h" #include "proc/temporal-filter.h" #include "software-device.h" +#include "software-device-info.h" +#include "software-sensor.h" #include "global_timestamp_reader.h" #include "auto-calibrated-device.h" #include "terminal-parser.h" @@ -804,7 +806,7 @@ void rs2_software_device_set_destruction_callback(const rs2_device* dev, rs2_sof VALIDATE_NOT_NULL(dev); auto swdev = VALIDATE_INTERFACE(dev->device, librealsense::software_device); VALIDATE_NOT_NULL(on_destruction); - librealsense::software_device_destruction_callback_ptr callback( + librealsense::software_device::destruction_callback_ptr callback( new librealsense::software_device_destruction_callback(on_destruction, user), [](rs2_software_device_destruction_callback* p) { delete p; }); swdev->register_destruction_callback(std::move(callback)); @@ -868,10 +870,11 @@ void rs2_software_device_set_destruction_callback_cpp(const rs2_device* dev, rs2 // Take ownership of the callback ASAP or else memory leaks could result if we throw! (the caller usually does a // 'new' when calling us) VALIDATE_NOT_NULL( callback ); - software_device_destruction_callback_ptr callback_ptr{ callback, - []( rs2_software_device_destruction_callback * p ) { - p->release(); - } }; + software_device::destruction_callback_ptr callback_ptr{ callback, + []( rs2_software_device_destruction_callback * p ) + { + p->release(); + } }; VALIDATE_NOT_NULL(dev); auto swdev = VALIDATE_INTERFACE(dev->device, librealsense::software_device); diff --git a/src/software-device-info.cpp b/src/software-device-info.cpp new file mode 100644 index 0000000000..b862aba584 --- /dev/null +++ b/src/software-device-info.cpp @@ -0,0 +1,44 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. + +#include "software-device-info.h" +#include "software-device.h" +#include "librealsense-exception.h" + +#include + + +namespace librealsense { + + +software_device_info::software_device_info( std::shared_ptr< context > const & ctx ) + : device_info( ctx ) + , _address() // leave empty until set_device() +{ +} + + +void software_device_info::set_device( std::shared_ptr< software_device > const & dev ) +{ + if( ! _address.empty() ) + throw wrong_api_call_sequence_exception( "software_device_info already initialized" ); + _dev = dev; + _address = rsutils::string::from() << "software-device://" << (unsigned long long)dev.get(); +} + + +bool software_device_info::is_same_as( std::shared_ptr< const device_info > const & other ) const +{ + if( auto rhs = std::dynamic_pointer_cast< const software_device_info >( other ) ) + return _address == rhs->_address; + return false; +} + + +std::shared_ptr< device_interface > software_device_info::create_device() +{ + return _dev.lock(); +} + + +} // namespace librealsense diff --git a/src/software-device-info.h b/src/software-device-info.h new file mode 100644 index 0000000000..aec6728129 --- /dev/null +++ b/src/software-device-info.h @@ -0,0 +1,35 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. +#pragma once + +#include "device-info.h" + + +namespace librealsense { + + +class software_device; + + +class software_device_info : public device_info +{ + std::weak_ptr< software_device > _dev; + std::string _address; + +public: + explicit software_device_info( std::shared_ptr< context > const & ctx ); + + // The usage is dictated by the rs2 APIs: rather than creating the info and then using create_device() to create the + // device, it's the other way around (see rs2_context_add_software_device). + // + void set_device( std::shared_ptr< software_device > const & dev ); + + std::string get_address() const override { return _address; } + + bool is_same_as( std::shared_ptr< const device_info > const & other ) const override; + + std::shared_ptr< device_interface > create_device() override; +}; + + +} // namespace librealsense diff --git a/src/software-device.cpp b/src/software-device.cpp index cb68e1ad2b..75310c8da5 100644 --- a/src/software-device.cpp +++ b/src/software-device.cpp @@ -2,17 +2,10 @@ // Copyright(c) 2018 Intel Corporation. All Rights Reserved. #include "software-device.h" -#include "stream.h" -#include "option.h" -#include "core/video-frame.h" +#include "software-sensor.h" +#include "environment.h" #include "core/matcher-factory.h" -#include -#include -#include - -using rsutils::deferred; - namespace librealsense { @@ -52,7 +45,7 @@ namespace librealsense register_stream_to_extrinsic_group(stream, max_idx+1); } - void software_device::register_destruction_callback(software_device_destruction_callback_ptr callback) + void software_device::register_destruction_callback( destruction_callback_ptr callback ) { _user_destruction_callback = std::move(callback); } @@ -71,30 +64,6 @@ namespace librealsense _matcher = matcher; } - static std::shared_ptr< metadata_parser_map > create_software_metadata_parser_map() - { - auto md_parser_map = std::make_shared< metadata_parser_map >(); - for( int i = 0; i < static_cast< int >( rs2_frame_metadata_value::RS2_FRAME_METADATA_COUNT ); ++i ) - { - auto key = static_cast< rs2_frame_metadata_value >( i ); - md_parser_map->emplace( key, std::make_shared< md_array_parser >( key ) ); - } - return md_parser_map; - } - - software_sensor::software_sensor( std::string const & name, software_device * owner ) - : sensor_base( name, owner, &_pbs ) - , _stereo_extension( [this]() { return stereo_extension( this ); } ) - , _metadata_map{} // to all 0's - { - // At this time (and therefore for backwards compatibility) no register_metadata is required for SW sensors, - // and metadata persists between frames (!!!!!!!). All SW sensors support ALL metadata. We can therefore - // also share their parsers: - static auto software_metadata_parser_map = create_software_metadata_parser_map(); - _metadata_parsers = software_metadata_parser_map; - _unique_id = unique_id::generate_id(); - } - std::shared_ptr software_device::create_matcher(const frame_holder& frame) const { std::vector profiles; @@ -106,293 +75,6 @@ namespace librealsense return matcher_factory::create(_matcher, profiles); } - std::shared_ptr software_sensor::add_video_stream(rs2_video_stream video_stream, bool is_default) - { - auto profile = std::make_shared( - platform::stream_profile{ (uint32_t)video_stream.width, (uint32_t)video_stream.height, (uint32_t)video_stream.fps, 0 }); - profile->set_dims(video_stream.width, video_stream.height); - profile->set_format(video_stream.fmt); - profile->set_framerate(video_stream.fps); - profile->set_stream_index(video_stream.index); - profile->set_stream_type(video_stream.type); - profile->set_unique_id(video_stream.uid); - profile->set_intrinsics([=]() {return video_stream.intrinsics; }); - if (is_default) profile->tag_profile(profile_tag::PROFILE_TAG_DEFAULT); - _sw_profiles.push_back(profile); - - return profile; - } - - std::shared_ptr software_sensor::add_motion_stream(rs2_motion_stream motion_stream, bool is_default) - { - auto profile = std::make_shared( - platform::stream_profile{ 0, 0, (uint32_t)motion_stream.fps, 0 }); - profile->set_format(motion_stream.fmt); - profile->set_framerate(motion_stream.fps); - profile->set_stream_index(motion_stream.index); - profile->set_stream_type(motion_stream.type); - profile->set_unique_id(motion_stream.uid); - profile->set_intrinsics([=]() {return motion_stream.intrinsics; }); - if (is_default) profile->tag_profile(profile_tag::PROFILE_TAG_DEFAULT); - _sw_profiles.push_back(profile); - - return std::move(profile); - } - - std::shared_ptr software_sensor::add_pose_stream(rs2_pose_stream pose_stream, bool is_default) - { - auto profile = std::make_shared( - platform::stream_profile{ 0, 0, (uint32_t)pose_stream.fps, 0 }); - if (!profile) - throw librealsense::invalid_value_exception("null pointer passed for argument \"profile\"."); - - profile->set_format(pose_stream.fmt); - profile->set_framerate(pose_stream.fps); - profile->set_stream_index(pose_stream.index); - profile->set_stream_type(pose_stream.type); - profile->set_unique_id(pose_stream.uid); - if (is_default) profile->tag_profile(profile_tag::PROFILE_TAG_DEFAULT); - _sw_profiles.push_back(profile); - - return std::move(profile); - } - - - bool software_sensor::extend_to(rs2_extension extension_type, void ** ptr) - { - if (extension_type == RS2_EXTENSION_DEPTH_SENSOR) - { - if (supports_option(RS2_OPTION_DEPTH_UNITS)) - { - auto & ext = *_stereo_extension; - *ptr = static_cast< depth_sensor * >( &ext ); - return true; - } - } - else if (extension_type == RS2_EXTENSION_DEPTH_STEREO_SENSOR) - { - if (supports_option(RS2_OPTION_DEPTH_UNITS) && - supports_option(RS2_OPTION_STEREO_BASELINE)) - { - *ptr = &(*_stereo_extension); - return true; - } - } - return false; - } - - stream_profiles software_sensor::init_stream_profiles() - { - // By default the raw profiles we're given is what we output - return _sw_profiles; - } - - void software_sensor::open(const stream_profiles& requests) - { - if (_is_streaming) - throw wrong_api_call_sequence_exception("open(...) failed. Software device is streaming!"); - else if (_is_opened) - throw wrong_api_call_sequence_exception("open(...) failed. Software device is already opened!"); - _is_opened = true; - set_active_streams(requests); - } - - void software_sensor::close() - { - if (_is_streaming) - throw wrong_api_call_sequence_exception("close() failed. Software device is streaming!"); - else if (!_is_opened) - throw wrong_api_call_sequence_exception("close() failed. Software device was not opened!"); - _is_opened = false; - set_active_streams({}); - } - - void software_sensor::start(frame_callback_ptr callback) - { - if (_is_streaming) - throw wrong_api_call_sequence_exception("start_streaming(...) failed. Software device is already streaming!"); - else if (!_is_opened) - throw wrong_api_call_sequence_exception("start_streaming(...) failed. Software device was not opened!"); - _source.get_published_size_option()->set(0); - _source.init(_metadata_parsers); - _source.set_sensor(this->shared_from_this()); - _source.set_callback(callback); - _is_streaming = true; - raise_on_before_streaming_changes(true); - } - - void software_sensor::stop() - { - if (!_is_streaming) - throw wrong_api_call_sequence_exception("stop_streaming() failed. Software device is not streaming!"); - - _is_streaming = false; - raise_on_before_streaming_changes(false); - _source.flush(); - _source.reset(); - } - - - void software_sensor::set_metadata( rs2_frame_metadata_value key, rs2_metadata_type value ) - { - _metadata_map[key] = { true, value }; - } - - - void software_sensor::erase_metadata( rs2_frame_metadata_value key ) - { - _metadata_map[key].is_valid = false; - } - - - frame_interface * software_sensor::allocate_new_frame( rs2_extension extension, - stream_profile_interface * profile, - frame_additional_data && data ) - { - auto frame = _source.alloc_frame( extension, 0, std::move( data ), false ); - if( ! frame ) - { - LOG_WARNING( "Failed to allocate frame " << data.frame_number << " type " << extension ); - } - else - { - frame->set_stream( std::dynamic_pointer_cast< stream_profile_interface >( profile->shared_from_this() ) ); - } - return frame; - } - - - frame_interface * software_sensor::allocate_new_video_frame( video_stream_profile_interface * profile, - int stride, - int bpp, - frame_additional_data && data ) - { - auto frame = allocate_new_frame( profile->get_stream_type() == RS2_STREAM_DEPTH ? RS2_EXTENSION_DEPTH_FRAME - : RS2_EXTENSION_VIDEO_FRAME, - profile, - std::move( data ) ); - if( frame ) - { - auto vid_frame = dynamic_cast< video_frame * >( frame ); - vid_frame->assign( profile->get_width(), profile->get_height(), stride, bpp * 8 ); - auto sd = dynamic_cast< software_device * >( _owner ); - sd->register_extrinsic( *profile ); - } - return frame; - } - - - void software_sensor::invoke_new_frame( frame_holder && frame, - void const * pixels, - std::function< void() > on_release ) - { - // The frame pixels/data are stored in the continuation object! - if( pixels ) - frame->attach_continuation( frame_continuation( on_release, pixels ) ); - _source.invoke_callback( std::move( frame ) ); - } - - - void software_sensor::on_video_frame( rs2_software_video_frame const & software_frame ) - { - deferred on_release( [deleter = software_frame.deleter, data = software_frame.pixels]() { deleter( data ); } ); - - stream_profile_interface * profile = software_frame.profile->profile; - auto vid_profile = dynamic_cast< video_stream_profile_interface * >( profile ); - if( ! vid_profile ) - throw invalid_value_exception( "Non-video profile provided to on_video_frame" ); - - if( ! _is_streaming ) - return; - - frame_additional_data data( _metadata_map ); - data.timestamp = software_frame.timestamp; - data.timestamp_domain = software_frame.domain; - data.frame_number = software_frame.frame_number; - - // Depth units, if not provided by the user, should default to the current sensor value of DEPTH_UNITS: - if( vid_profile->get_stream_type() != RS2_STREAM_DEPTH ) - data.depth_units = 0; - else if( software_frame.depth_units ) - data.depth_units = software_frame.depth_units; - else if( auto opt = get_option_handler( RS2_OPTION_DEPTH_UNITS ) ) - data.depth_units = opt->query(); - else - data.depth_units = 0.f; - - auto frame - = allocate_new_video_frame( vid_profile, software_frame.stride, software_frame.bpp, std::move( data ) ); - if( frame ) - invoke_new_frame( frame, software_frame.pixels, on_release.detach() ); - } - - void software_sensor::on_motion_frame( rs2_software_motion_frame const & software_frame ) - { - deferred on_release( [deleter = software_frame.deleter, data = software_frame.data]() { deleter( data ); } ); - if( ! _is_streaming ) - return; - - frame_additional_data data( _metadata_map ); - data.timestamp = software_frame.timestamp; - data.timestamp_domain = software_frame.domain; - data.frame_number = software_frame.frame_number; - - auto frame - = allocate_new_frame( RS2_EXTENSION_MOTION_FRAME, software_frame.profile->profile, std::move( data ) ); - if( frame ) - invoke_new_frame( frame, software_frame.data, on_release.detach() ); - } - - void software_sensor::on_pose_frame( rs2_software_pose_frame const & software_frame ) - { - deferred on_release( [deleter = software_frame.deleter, data = software_frame.data]() { deleter( data ); } ); - if( ! _is_streaming ) - return; - - frame_additional_data data( _metadata_map ); - data.timestamp = software_frame.timestamp; - data.timestamp_domain = software_frame.domain; - data.frame_number = software_frame.frame_number; - - auto frame = allocate_new_frame( RS2_EXTENSION_POSE_FRAME, software_frame.profile->profile, std::move( data ) ); - if( frame ) - invoke_new_frame( frame, software_frame.data, on_release.detach() ); - } - - void software_sensor::on_notification( rs2_software_notification const & notif ) - { - notification n{ notif.category, notif.type, notif.severity, notif.description }; - n.serialized_data = notif.serialized_data; - _notifications_processor->raise_notification(n); - } - - void software_sensor::add_read_only_option(rs2_option option, float val) - { - register_option( option, std::make_shared< const_value_option >( "bypass sensor read only option", val ) ); - } - - void software_sensor::update_read_only_option(rs2_option option, float val) - { - if (auto opt = dynamic_cast(&get_option(option))) - opt->update(val); - else - throw invalid_value_exception( rsutils::string::from() << "option " << get_string( option ) - << " is not read-only or is deprecated type" ); - } - - void software_sensor::add_option(rs2_option option, option_range range, bool is_writable) - { - register_option(option, (is_writable? std::make_shared(range) : - std::make_shared(range))); - } - - void software_recommended_proccesing_blocks::add_processing_block( std::shared_ptr< processing_block_interface > const & block ) - { - if( ! block ) - throw invalid_value_exception( "trying to add an empty software processing block" ); - - _blocks.push_back( block ); - } } // namespace librealsense diff --git a/src/software-device.h b/src/software-device.h index 256d141b33..08d6db06df 100644 --- a/src/software-device.h +++ b/src/software-device.h @@ -2,169 +2,44 @@ // Copyright(c) 2018 Intel Corporation. All Rights Reserved. #pragma once -#include "core/streaming.h" #include "device.h" -#include "sensor.h" -#include -#include +#include "core/extension.h" +#include -namespace librealsense + +namespace librealsense { + + +class software_sensor; + + +class software_device : public device { - class software_sensor; - class software_device_info; - class video_stream_profile_interface; - - class software_device : public device - { - public: - software_device( std::shared_ptr< const device_info > const & ); - virtual ~software_device(); - - software_sensor& add_software_sensor(const std::string& name); - - software_sensor& get_software_sensor( size_t index); - - void set_matcher_type(rs2_matchers matcher); - - std::shared_ptr create_matcher(const frame_holder& frame) const override; - - std::vector get_profiles_tags() const override - { - std::vector markers; - return markers; - }; - void register_extrinsic(const stream_interface& stream); - - void register_destruction_callback(software_device_destruction_callback_ptr); - - protected: - std::vector> _software_sensors; - librealsense::software_device_destruction_callback_ptr _user_destruction_callback; - rs2_matchers _matcher = RS2_MATCHER_DEFAULT; - }; +public: + software_device( std::shared_ptr< const device_info > const & ); + virtual ~software_device(); + + software_sensor& add_software_sensor(const std::string& name); + software_sensor& get_software_sensor( size_t index); + + void set_matcher_type(rs2_matchers matcher); + + std::shared_ptr create_matcher(const frame_holder& frame) const override; + + std::vector< tagged_profile > get_profiles_tags() const override { return {}; }; - class software_device_info : public device_info - { - std::weak_ptr< software_device > _dev; - std::string _address; - - public: - explicit software_device_info( std::shared_ptr< context > const & ctx ) - : device_info( ctx ) - , _address() // leave empty until set_device() - { - } - - void set_device( std::shared_ptr< software_device > const & dev ) - { - if( ! _address.empty() ) - throw wrong_api_call_sequence_exception( "software_device_info already initialized" ); - _dev = dev; - _address = rsutils::string::from() << "software-device://" << (unsigned long long)dev.get(); - } - - std::string get_address() const override { return _address; } - - bool is_same_as( std::shared_ptr< const device_info > const & other ) const override - { - if( auto rhs = std::dynamic_pointer_cast(other) ) - return _address == rhs->_address; - return false; - } - - std::shared_ptr< device_interface > create_device() override - { - return _dev.lock(); - } - }; - - class software_recommended_proccesing_blocks : public recommended_proccesing_blocks_interface - { - public: - processing_blocks get_recommended_processing_blocks() const override { - return _blocks; - } - ~software_recommended_proccesing_blocks() override {} - - void add_processing_block( std::shared_ptr< processing_block_interface > const & block ); - - private: - processing_blocks _blocks; - }; - - class software_sensor : public sensor_base, public extendable_interface - { - public: - software_sensor( std::string const & name, software_device * owner ); - - virtual std::shared_ptr add_video_stream(rs2_video_stream video_stream, bool is_default = false); - virtual std::shared_ptr add_motion_stream(rs2_motion_stream motion_stream, bool is_default = false); - virtual std::shared_ptr add_pose_stream(rs2_pose_stream pose_stream, bool is_default = false); - - bool extend_to(rs2_extension extension_type, void** ptr) override; - - stream_profiles init_stream_profiles() override; - stream_profiles const & get_raw_stream_profiles() const override { return _sw_profiles; } - - void open(const stream_profiles& requests) override; - void close() override; - - void start(frame_callback_ptr callback) override; - void stop() override; - - void on_video_frame( rs2_software_video_frame const & ); - void on_motion_frame( rs2_software_motion_frame const & ); - void on_pose_frame( rs2_software_pose_frame const & ); - void on_notification( rs2_software_notification const & ); - void add_read_only_option(rs2_option option, float val); - void update_read_only_option(rs2_option option, float val); - void add_option(rs2_option option, option_range range, bool is_writable); - void set_metadata( rs2_frame_metadata_value key, rs2_metadata_type value ); - void erase_metadata( rs2_frame_metadata_value key ); - - protected: - frame_interface * allocate_new_frame( rs2_extension, stream_profile_interface *, frame_additional_data && ); - frame_interface * allocate_new_video_frame( video_stream_profile_interface *, int stride, int bpp, frame_additional_data && ); - void invoke_new_frame( frame_holder &&, void const * pixels, std::function< void() > on_release ); - - metadata_array _metadata_map; - - processing_blocks get_recommended_processing_blocks() const override - { - return _pbs.get_recommended_processing_blocks(); - } - - software_recommended_proccesing_blocks & get_software_recommended_proccesing_blocks() { return _pbs; } - - // We build profiles using add_video_stream(), etc., and feed those into init_stream_profiles() which could in - // theory change them: so these are our "raw" profiles before initialization... - stream_profiles _sw_profiles; - - private: - friend class software_device; - uint64_t _unique_id; - - class stereo_extension : public depth_stereo_sensor - { - public: - stereo_extension(software_sensor* owner) : _owner(owner) {} - - float get_depth_scale() const override { - return _owner->get_option(RS2_OPTION_DEPTH_UNITS).query(); - } - - float get_stereo_baseline_mm() const override { - return _owner->get_option(RS2_OPTION_STEREO_BASELINE).query(); - } - - private: - software_sensor* _owner; - }; - - rsutils::lazy< stereo_extension > _stereo_extension; - - software_recommended_proccesing_blocks _pbs; - }; - MAP_EXTENSION(RS2_EXTENSION_SOFTWARE_SENSOR, software_sensor); - MAP_EXTENSION(RS2_EXTENSION_SOFTWARE_DEVICE, software_device); -} + void register_extrinsic(const stream_interface& stream); + + using destruction_callback_ptr = std::shared_ptr< rs2_software_device_destruction_callback >; + void register_destruction_callback( destruction_callback_ptr ); + +protected: + std::vector> _software_sensors; + destruction_callback_ptr _user_destruction_callback; + rs2_matchers _matcher = RS2_MATCHER_DEFAULT; +}; + +MAP_EXTENSION(RS2_EXTENSION_SOFTWARE_DEVICE, software_device); + + +} // namespace librealsense diff --git a/src/software-sensor.cpp b/src/software-sensor.cpp new file mode 100644 index 0000000000..9a23e413b3 --- /dev/null +++ b/src/software-sensor.cpp @@ -0,0 +1,375 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. + +#include "software-sensor.h" +#include "software-device.h" +#include "stream.h" +#include "option.h" +#include "core/video-frame.h" + +#include +#include + +using rsutils::deferred; + + +namespace librealsense { + + +static std::shared_ptr< metadata_parser_map > create_software_metadata_parser_map() +{ + auto md_parser_map = std::make_shared< metadata_parser_map >(); + for( int i = 0; i < static_cast< int >( rs2_frame_metadata_value::RS2_FRAME_METADATA_COUNT ); ++i ) + { + auto key = static_cast< rs2_frame_metadata_value >( i ); + md_parser_map->emplace( key, std::make_shared< md_array_parser >( key ) ); + } + return md_parser_map; +} + + +class software_sensor::stereo_extension : public depth_stereo_sensor +{ +public: + stereo_extension( software_sensor * owner ) + : _owner( owner ) + { + } + + float get_depth_scale() const override { return _owner->get_option( RS2_OPTION_DEPTH_UNITS ).query(); } + + float get_stereo_baseline_mm() const override { return _owner->get_option( RS2_OPTION_STEREO_BASELINE ).query(); } + +private: + software_sensor * _owner; +}; + + +software_sensor::software_sensor( std::string const & name, software_device * owner ) + : sensor_base( name, owner, &_pbs ) + , _stereo_extension( [this]() { return stereo_extension( this ); } ) + , _metadata_map{} // to all 0's +{ + // At this time (and therefore for backwards compatibility) no register_metadata is required for SW sensors, + // and metadata persists between frames (!!!!!!!). All SW sensors support ALL metadata. We can therefore + // also share their parsers: + static auto software_metadata_parser_map = create_software_metadata_parser_map(); + _metadata_parsers = software_metadata_parser_map; + _unique_id = unique_id::generate_id(); +} + + +software_sensor::~software_sensor() +{ +} + + +std::shared_ptr< stream_profile_interface > software_sensor::add_video_stream( rs2_video_stream video_stream, + bool is_default ) +{ + auto profile = std::make_shared< video_stream_profile >( platform::stream_profile{ (uint32_t)video_stream.width, + (uint32_t)video_stream.height, + (uint32_t)video_stream.fps, + 0 } ); + profile->set_dims( video_stream.width, video_stream.height ); + profile->set_format( video_stream.fmt ); + profile->set_framerate( video_stream.fps ); + profile->set_stream_index( video_stream.index ); + profile->set_stream_type( video_stream.type ); + profile->set_unique_id( video_stream.uid ); + profile->set_intrinsics( [=]() { return video_stream.intrinsics; } ); + if( is_default ) + profile->tag_profile( profile_tag::PROFILE_TAG_DEFAULT ); + _sw_profiles.push_back( profile ); + + return profile; +} + + +std::shared_ptr< stream_profile_interface > software_sensor::add_motion_stream( rs2_motion_stream motion_stream, + bool is_default ) +{ + auto profile + = std::make_shared< motion_stream_profile >( platform::stream_profile{ 0, 0, (uint32_t)motion_stream.fps, 0 } ); + profile->set_format( motion_stream.fmt ); + profile->set_framerate( motion_stream.fps ); + profile->set_stream_index( motion_stream.index ); + profile->set_stream_type( motion_stream.type ); + profile->set_unique_id( motion_stream.uid ); + profile->set_intrinsics( [=]() { return motion_stream.intrinsics; } ); + if( is_default ) + profile->tag_profile( profile_tag::PROFILE_TAG_DEFAULT ); + _sw_profiles.push_back( profile ); + + return std::move( profile ); +} + + +std::shared_ptr< stream_profile_interface > software_sensor::add_pose_stream( rs2_pose_stream pose_stream, + bool is_default ) +{ + auto profile + = std::make_shared< pose_stream_profile >( platform::stream_profile{ 0, 0, (uint32_t)pose_stream.fps, 0 } ); + if( ! profile ) + throw librealsense::invalid_value_exception( "null pointer passed for argument \"profile\"." ); + + profile->set_format( pose_stream.fmt ); + profile->set_framerate( pose_stream.fps ); + profile->set_stream_index( pose_stream.index ); + profile->set_stream_type( pose_stream.type ); + profile->set_unique_id( pose_stream.uid ); + if( is_default ) + profile->tag_profile( profile_tag::PROFILE_TAG_DEFAULT ); + _sw_profiles.push_back( profile ); + + return std::move( profile ); +} + + +bool software_sensor::extend_to( rs2_extension extension_type, void ** ptr ) +{ + if( extension_type == RS2_EXTENSION_DEPTH_SENSOR ) + { + if( supports_option( RS2_OPTION_DEPTH_UNITS ) ) + { + auto & ext = *_stereo_extension; + *ptr = static_cast< depth_sensor * >( &ext ); + return true; + } + } + else if( extension_type == RS2_EXTENSION_DEPTH_STEREO_SENSOR ) + { + if( supports_option( RS2_OPTION_DEPTH_UNITS ) && supports_option( RS2_OPTION_STEREO_BASELINE ) ) + { + *ptr = &( *_stereo_extension ); + return true; + } + } + return false; +} + + +stream_profiles software_sensor::init_stream_profiles() +{ + // By default the raw profiles we're given is what we output + return _sw_profiles; +} + + +void software_sensor::open( const stream_profiles & requests ) +{ + if( _is_streaming ) + throw wrong_api_call_sequence_exception( "open(...) failed. Software device is streaming!" ); + else if( _is_opened ) + throw wrong_api_call_sequence_exception( "open(...) failed. Software device is already opened!" ); + _is_opened = true; + set_active_streams( requests ); +} + + +void software_sensor::close() +{ + if( _is_streaming ) + throw wrong_api_call_sequence_exception( "close() failed. Software device is streaming!" ); + else if( ! _is_opened ) + throw wrong_api_call_sequence_exception( "close() failed. Software device was not opened!" ); + _is_opened = false; + set_active_streams( {} ); +} + + +void software_sensor::start( frame_callback_ptr callback ) +{ + if( _is_streaming ) + throw wrong_api_call_sequence_exception( "start_streaming(...) failed. Software device is already streaming!" ); + else if( ! _is_opened ) + throw wrong_api_call_sequence_exception( "start_streaming(...) failed. Software device was not opened!" ); + _source.get_published_size_option()->set( 0 ); + _source.init( _metadata_parsers ); + _source.set_sensor( this->shared_from_this() ); + _source.set_callback( callback ); + _is_streaming = true; + raise_on_before_streaming_changes( true ); +} + + +void software_sensor::stop() +{ + if( ! _is_streaming ) + throw wrong_api_call_sequence_exception( "stop_streaming() failed. Software device is not streaming!" ); + + _is_streaming = false; + raise_on_before_streaming_changes( false ); + _source.flush(); + _source.reset(); +} + + +void software_sensor::set_metadata( rs2_frame_metadata_value key, rs2_metadata_type value ) +{ + _metadata_map[key] = { true, value }; +} + + +void software_sensor::erase_metadata( rs2_frame_metadata_value key ) +{ + _metadata_map[key].is_valid = false; +} + + +frame_interface * software_sensor::allocate_new_frame( rs2_extension extension, + stream_profile_interface * profile, + frame_additional_data && data ) +{ + auto frame = _source.alloc_frame( extension, 0, std::move( data ), false ); + if( ! frame ) + { + LOG_WARNING( "Failed to allocate frame " << data.frame_number << " type " << extension ); + } + else + { + frame->set_stream( std::dynamic_pointer_cast< stream_profile_interface >( profile->shared_from_this() ) ); + } + return frame; +} + + +frame_interface * software_sensor::allocate_new_video_frame( video_stream_profile_interface * profile, + int stride, + int bpp, + frame_additional_data && data ) +{ + auto frame = allocate_new_frame( profile->get_stream_type() == RS2_STREAM_DEPTH ? RS2_EXTENSION_DEPTH_FRAME + : RS2_EXTENSION_VIDEO_FRAME, + profile, + std::move( data ) ); + if( frame ) + { + auto vid_frame = dynamic_cast< video_frame * >( frame ); + vid_frame->assign( profile->get_width(), profile->get_height(), stride, bpp * 8 ); + auto sd = dynamic_cast< software_device * >( _owner ); + sd->register_extrinsic( *profile ); + } + return frame; +} + + +void software_sensor::invoke_new_frame( frame_holder && frame, void const * pixels, std::function< void() > on_release ) +{ + // The frame pixels/data are stored in the continuation object! + if( pixels ) + frame->attach_continuation( frame_continuation( on_release, pixels ) ); + _source.invoke_callback( std::move( frame ) ); +} + + +void software_sensor::on_video_frame( rs2_software_video_frame const & software_frame ) +{ + deferred on_release( [deleter = software_frame.deleter, data = software_frame.pixels]() { deleter( data ); } ); + + stream_profile_interface * profile = software_frame.profile->profile; + auto vid_profile = dynamic_cast< video_stream_profile_interface * >( profile ); + if( ! vid_profile ) + throw invalid_value_exception( "Non-video profile provided to on_video_frame" ); + + if( ! _is_streaming ) + return; + + frame_additional_data data( _metadata_map ); + data.timestamp = software_frame.timestamp; + data.timestamp_domain = software_frame.domain; + data.frame_number = software_frame.frame_number; + + // Depth units, if not provided by the user, should default to the current sensor value of DEPTH_UNITS: + if( vid_profile->get_stream_type() != RS2_STREAM_DEPTH ) + data.depth_units = 0; + else if( software_frame.depth_units ) + data.depth_units = software_frame.depth_units; + else if( auto opt = get_option_handler( RS2_OPTION_DEPTH_UNITS ) ) + data.depth_units = opt->query(); + else + data.depth_units = 0.f; + + auto frame = allocate_new_video_frame( vid_profile, software_frame.stride, software_frame.bpp, std::move( data ) ); + if( frame ) + invoke_new_frame( frame, software_frame.pixels, on_release.detach() ); +} + + +void software_sensor::on_motion_frame( rs2_software_motion_frame const & software_frame ) +{ + deferred on_release( [deleter = software_frame.deleter, data = software_frame.data]() { deleter( data ); } ); + if( ! _is_streaming ) + return; + + frame_additional_data data( _metadata_map ); + data.timestamp = software_frame.timestamp; + data.timestamp_domain = software_frame.domain; + data.frame_number = software_frame.frame_number; + + auto frame = allocate_new_frame( RS2_EXTENSION_MOTION_FRAME, software_frame.profile->profile, std::move( data ) ); + if( frame ) + invoke_new_frame( frame, software_frame.data, on_release.detach() ); +} + + +void software_sensor::on_pose_frame( rs2_software_pose_frame const & software_frame ) +{ + deferred on_release( [deleter = software_frame.deleter, data = software_frame.data]() { deleter( data ); } ); + if( ! _is_streaming ) + return; + + frame_additional_data data( _metadata_map ); + data.timestamp = software_frame.timestamp; + data.timestamp_domain = software_frame.domain; + data.frame_number = software_frame.frame_number; + + auto frame = allocate_new_frame( RS2_EXTENSION_POSE_FRAME, software_frame.profile->profile, std::move( data ) ); + if( frame ) + invoke_new_frame( frame, software_frame.data, on_release.detach() ); +} + + +void software_sensor::on_notification( rs2_software_notification const & notif ) +{ + notification n{ notif.category, notif.type, notif.severity, notif.description }; + n.serialized_data = notif.serialized_data; + _notifications_processor->raise_notification( n ); +} + + +void software_sensor::add_read_only_option( rs2_option option, float val ) +{ + register_option( option, std::make_shared< const_value_option >( "bypass sensor read only option", val ) ); +} + + +void software_sensor::update_read_only_option( rs2_option option, float val ) +{ + if( auto opt = dynamic_cast< readonly_float_option * >( &get_option( option ) ) ) + opt->update( val ); + else + throw invalid_value_exception( rsutils::string::from() << "option " << get_string( option ) + << " is not read-only or is deprecated type" ); +} + + +void software_sensor::add_option( rs2_option option, option_range range, bool is_writable ) +{ + register_option( option, + ( is_writable ? std::make_shared< float_option >( range ) + : std::make_shared< readonly_float_option >( range ) ) ); +} + + +void software_recommended_proccesing_blocks::add_processing_block( + std::shared_ptr< processing_block_interface > const & block ) +{ + if( ! block ) + throw invalid_value_exception( "trying to add an empty software processing block" ); + + _blocks.push_back( block ); +} + + +} // namespace librealsense diff --git a/src/software-sensor.h b/src/software-sensor.h new file mode 100644 index 0000000000..e2bdd73e68 --- /dev/null +++ b/src/software-sensor.h @@ -0,0 +1,99 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2018 Intel Corporation. All Rights Reserved. +#pragma once + +#include "sensor.h" +#include "core/extension.h" +#include +#include +#include + +namespace librealsense { + + +class software_recommended_proccesing_blocks : public recommended_proccesing_blocks_interface +{ +public: + processing_blocks get_recommended_processing_blocks() const override { return _blocks; } + + void add_processing_block( std::shared_ptr< processing_block_interface > const & block ); + +private: + processing_blocks _blocks; +}; + + +class software_device; +class stream_profile_interface; +class video_stream_profile_interface; + + +class software_sensor + : public sensor_base + , public extendable_interface +{ +public: + software_sensor( std::string const & name, software_device * owner ); + ~software_sensor(); + + virtual std::shared_ptr< stream_profile_interface > add_video_stream( rs2_video_stream video_stream, + bool is_default = false ); + virtual std::shared_ptr< stream_profile_interface > add_motion_stream( rs2_motion_stream motion_stream, + bool is_default = false ); + virtual std::shared_ptr< stream_profile_interface > add_pose_stream( rs2_pose_stream pose_stream, + bool is_default = false ); + + bool extend_to( rs2_extension extension_type, void ** ptr ) override; + + stream_profiles init_stream_profiles() override; + stream_profiles const & get_raw_stream_profiles() const override { return _sw_profiles; } + + void open( const stream_profiles & requests ) override; + void close() override; + + using frame_callback_ptr = std::shared_ptr< rs2_frame_callback >; + void start( frame_callback_ptr callback ) override; + void stop() override; + + void on_video_frame( rs2_software_video_frame const & ); + void on_motion_frame( rs2_software_motion_frame const & ); + void on_pose_frame( rs2_software_pose_frame const & ); + void on_notification( rs2_software_notification const & ); + void add_read_only_option( rs2_option option, float val ); + void update_read_only_option( rs2_option option, float val ); + void add_option( rs2_option option, option_range range, bool is_writable ); + void set_metadata( rs2_frame_metadata_value key, rs2_metadata_type value ); + void erase_metadata( rs2_frame_metadata_value key ); + +protected: + frame_interface * allocate_new_frame( rs2_extension, stream_profile_interface *, frame_additional_data && ); + frame_interface * allocate_new_video_frame( video_stream_profile_interface *, int stride, int bpp, frame_additional_data && ); + void invoke_new_frame( frame_holder &&, void const * pixels, std::function< void() > on_release ); + + metadata_array _metadata_map; + + processing_blocks get_recommended_processing_blocks() const override + { + return _pbs.get_recommended_processing_blocks(); + } + + software_recommended_proccesing_blocks & get_software_recommended_proccesing_blocks() { return _pbs; } + + // We build profiles using add_video_stream(), etc., and feed those into init_stream_profiles() which could in + // theory change them: so these are our "raw" profiles before initialization... + stream_profiles _sw_profiles; + +private: + friend class software_device; + uint64_t _unique_id; + + class stereo_extension; + rsutils::lazy< stereo_extension > _stereo_extension; + + software_recommended_proccesing_blocks _pbs; +}; + +MAP_EXTENSION( RS2_EXTENSION_SOFTWARE_SENSOR, software_sensor ); + + +} // namespace librealsense diff --git a/src/types.h b/src/types.h index 6871da5bd0..47d2aadccf 100644 --- a/src/types.h +++ b/src/types.h @@ -591,7 +591,6 @@ namespace librealsense typedef std::shared_ptr frame_processor_callback_ptr; typedef std::shared_ptr notifications_callback_ptr; typedef std::shared_ptr calibration_change_callback_ptr; - typedef std::shared_ptr software_device_destruction_callback_ptr; typedef std::shared_ptr devices_changed_callback_ptr; typedef std::shared_ptr update_progress_callback_ptr;