From 071ee58ad096714977201cef045ff9a366175d32 Mon Sep 17 00:00:00 2001 From: Eran Date: Fri, 17 Nov 2023 14:57:03 +0200 Subject: [PATCH] add pp-block-factory --- src/CMakeLists.txt | 2 + src/context.cpp | 8 +++ src/context.h | 6 +++ src/core/CMakeLists.txt | 1 + src/core/pp-block-factory.h | 38 +++++++++++++++ src/dds/rs-dds-sensor-proxy.cpp | 46 ++++++------------ src/media/ros/ros_reader.cpp | 86 +++++++++++++++++---------------- src/media/ros/ros_reader.h | 6 ++- src/media/ros/ros_writer.cpp | 42 ++++++++-------- src/media/ros/ros_writer.h | 2 - src/rscore-pp-block-factory.cpp | 53 ++++++++++++++++++++ src/rscore-pp-block-factory.h | 19 ++++++++ 12 files changed, 213 insertions(+), 96 deletions(-) create mode 100644 src/core/pp-block-factory.h create mode 100644 src/rscore-pp-block-factory.cpp create mode 100644 src/rscore-pp-block-factory.h diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 9f172b9282c..ed9d48a5ab5 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -88,6 +88,8 @@ target_sources(${LRS_TARGET} "${CMAKE_CURRENT_LIST_DIR}/sensor.cpp" "${CMAKE_CURRENT_LIST_DIR}/hid-sensor.cpp" "${CMAKE_CURRENT_LIST_DIR}/uvc-sensor.cpp" + "${CMAKE_CURRENT_LIST_DIR}/rscore-pp-block-factory.h" + "${CMAKE_CURRENT_LIST_DIR}/rscore-pp-block-factory.cpp" "${CMAKE_CURRENT_LIST_DIR}/software-device.cpp" "${CMAKE_CURRENT_LIST_DIR}/software-device-info.cpp" "${CMAKE_CURRENT_LIST_DIR}/software-sensor.cpp" diff --git a/src/context.cpp b/src/context.cpp index dd9b1c2b415..0df08e9a136 100644 --- a/src/context.cpp +++ b/src/context.cpp @@ -8,6 +8,7 @@ #ifdef BUILD_WITH_DDS #include "dds/rsdds-device-factory.h" #endif +#include "rscore-pp-block-factory.h" #include // rs2_devices_changed_callback #include // RS2_API_FULL_VERSION_STR @@ -141,4 +142,11 @@ namespace librealsense } } + + std::shared_ptr< processing_block_interface > context::create_pp_block( std::string const & name, + nlohmann::json const & settings ) + { + return rscore_pp_block_factory().create_pp_block( name, settings ); + } + } diff --git a/src/context.h b/src/context.h index 7223c9c4eeb..629bad3a3ad 100644 --- a/src/context.h +++ b/src/context.h @@ -13,6 +13,7 @@ namespace librealsense { class device_factory; class device_info; + class processing_block_interface; class context : public std::enable_shared_from_this @@ -58,6 +59,11 @@ namespace librealsense const nlohmann::json & get_settings() const { return _settings; } + // Create processing blocks given a name and settings. + // + std::shared_ptr< processing_block_interface > create_pp_block( std::string const & name, + nlohmann::json const & settings ); + private: void invoke_devices_changed_callbacks( std::vector< std::shared_ptr< device_info > > const & devices_removed, std::vector< std::shared_ptr< device_info > > const & devices_added ); diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index 04a044d8c38..53de4f10671 100644 --- a/src/core/CMakeLists.txt +++ b/src/core/CMakeLists.txt @@ -32,6 +32,7 @@ target_sources(${LRS_TARGET} "${CMAKE_CURRENT_LIST_DIR}/info.h" "${CMAKE_CURRENT_LIST_DIR}/extension.h" "${CMAKE_CURRENT_LIST_DIR}/pose-frame.h" + "${CMAKE_CURRENT_LIST_DIR}/pp-block-factory.h" "${CMAKE_CURRENT_LIST_DIR}/processing.h" "${CMAKE_CURRENT_LIST_DIR}/processing-block-interface.h" "${CMAKE_CURRENT_LIST_DIR}/recommended-processing-blocks-interface.h" diff --git a/src/core/pp-block-factory.h b/src/core/pp-block-factory.h new file mode 100644 index 00000000000..85a76f6273a --- /dev/null +++ b/src/core/pp-block-factory.h @@ -0,0 +1,38 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. +#pragma once + +#include +#include + + +namespace librealsense { + + +class processing_block_interface; + + +// Instantiator of post-processing filters, AKA processing blocks, based on name and settings. +// These are the ones you see in the Viewer, and are also returned by +// sensor_interface::get_recommended_processing_blocks(). Their names are serialized to rosbags when recording and +// reinstantiated on playback. +// +// Do not use directly: the context manages these! +// +class pp_block_factory +{ +public: + virtual ~pp_block_factory() = default; + + // Creates a post-processing block. If the name is unrecognized, a null pointer is returned. Otherwise, if the + // settings are wrong or some other error condition is encountered, an exception is raised. + // + // The name is case-insensitive. + // + virtual std::shared_ptr< processing_block_interface > create_pp_block( std::string const & name, + nlohmann::json const & settings ) + = 0; +}; + + +} // namespace librealsense diff --git a/src/dds/rs-dds-sensor-proxy.cpp b/src/dds/rs-dds-sensor-proxy.cpp index 778a8b37139..1e4534c7764 100644 --- a/src/dds/rs-dds-sensor-proxy.cpp +++ b/src/dds/rs-dds-sensor-proxy.cpp @@ -17,16 +17,7 @@ #include #include -// Processing blocks for DDS SW sensors -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include #include @@ -563,28 +554,19 @@ void dds_sensor_proxy::add_processing_block( std::string const & filter_name ) if( processing_block_exists( get_recommended_processing_blocks(), filter_name ) ) return; // Already created by another stream of this sensor - if( filter_name.compare( "Decimation Filter" ) == 0 ) - // sensor.cpp sets format option based on sensor type, but the filter does not use it and selects the - // appropriate decimation algorithm based on processed frame profile format. - super::add_processing_block( std::make_shared< decimation_filter >() ); - else if( filter_name.compare( "HDR Merge" ) == 0 ) - super::add_processing_block( std::make_shared< hdr_merge >() ); - else if( filter_name.compare( "Filter By Sequence id" ) == 0 ) - super::add_processing_block( std::make_shared< sequence_id_filter >() ); - else if( filter_name.compare( "Threshold Filter" ) == 0 ) - super::add_processing_block( std::make_shared< threshold >() ); - else if( filter_name.compare( "Depth to Disparity" ) == 0 ) - super::add_processing_block( std::make_shared< disparity_transform >( true ) ); - else if( filter_name.compare( "Disparity to Depth" ) == 0 ) - super::add_processing_block( std::make_shared< disparity_transform >( false ) ); - else if( filter_name.compare( "Spatial Filter" ) == 0 ) - super::add_processing_block( std::make_shared< spatial_filter >() ); - else if( filter_name.compare( "Temporal Filter" ) == 0 ) - super::add_processing_block( std::make_shared< temporal_filter >() ); - else if( filter_name.compare( "Hole Filling Filter" ) == 0 ) - super::add_processing_block( std::make_shared< hole_filling_filter >() ); - else - throw std::runtime_error( "Unsupported processing block '" + filter_name + "' received" ); + try + { + auto ppb = get_device().get_context()->create_pp_block( filter_name, {} ); + if( ! ppb ) + LOG_WARNING( "Unsupported processing block '" + filter_name + "' received" ); + else + super::add_processing_block( ppb ); + } + catch( std::exception const & e ) + { + // Bad settings, error in configuration, etc. + LOG_ERROR( "Failed to create processing block '" << filter_name << "': " << e.what() ); + } } diff --git a/src/media/ros/ros_reader.cpp b/src/media/ros/ros_reader.cpp index bcd89eb4f01..52179d8442a 100644 --- a/src/media/ros/ros_reader.cpp +++ b/src/media/ros/ros_reader.cpp @@ -1,25 +1,18 @@ // License: Apache 2.0. See LICENSE file in root directory. // Copyright(c) 2019 Intel Corporation. All Rights Reserved. -#include #include "ros_reader.h" #include "ds/ds-device-common.h" #include "ds/d400/d400-private.h" -#include "proc/disparity-transform.h" -#include "proc/decimation-filter.h" -#include "proc/threshold.h" -#include "proc/spatial-filter.h" -#include "proc/temporal-filter.h" -#include "proc/hole-filling-filter.h" -#include "proc/hdr-merge.h" -#include "proc/sequence-id-filter.h" #include "std_msgs/Float32MultiArray.h" +#include #include #include #include #include #include +#include namespace librealsense @@ -885,7 +878,13 @@ namespace librealsense { throw invalid_value_exception("Failed to get options interface from sensor snapshots"); } - auto proccesing_blocks = read_proccesing_blocks(file, { get_device_index(), sensor_index }, time, options_api, file_version, pid, sensor_name); + auto proccesing_blocks = read_proccesing_blocks( file, + { get_device_index(), sensor_index }, + time, + options_api, + file_version, + pid, + sensor_name ); sensor_extensions[RS2_EXTENSION_RECOMMENDED_FILTERS] = proccesing_blocks; } @@ -1002,8 +1001,14 @@ namespace librealsense return std::make_shared(processing_blocks{}); } - std::shared_ptr ros_reader::read_proccesing_blocks(const rosbag::Bag& file, device_serializer::sensor_identifier sensor_id, const nanoseconds& timestamp, - std::shared_ptr options, uint32_t file_version, std::string pid, std::string sensor_name) + std::shared_ptr< recommended_proccesing_blocks_snapshot > + ros_reader::read_proccesing_blocks( const rosbag::Bag & file, + device_serializer::sensor_identifier sensor_id, + const nanoseconds & timestamp, + std::shared_ptr< options_interface > options, + uint32_t file_version, + std::string pid, + std::string sensor_name ) { processing_blocks blocks; std::shared_ptr res; @@ -1027,8 +1032,8 @@ namespace librealsense last_item = it++; auto block = create_processing_block(*last_item, depth_to_disparity, options); - assert(block); - blocks.push_back(block); + if( block ) + blocks.push_back(block); } res = std::make_shared(blocks); @@ -1434,35 +1439,34 @@ namespace librealsense return std::make_pair(id, std::make_shared(description, value)); } - std::shared_ptr ros_reader::create_processing_block(const rosbag::MessageInstance& value_message_instance, bool& depth_to_disparity, std::shared_ptr options) + std::shared_ptr< librealsense::processing_block_interface > + ros_reader::create_processing_block( const rosbag::MessageInstance & value_message_instance, + bool & depth_to_disparity, + std::shared_ptr< options_interface > sensor_options ) { auto processing_block_msg = instantiate_msg(value_message_instance); - rs2_extension id; - convert(processing_block_msg->data, id); - std::shared_ptr disparity; - - switch (id) - { - case RS2_EXTENSION_DECIMATION_FILTER: - return std::make_shared::type>(); - case RS2_EXTENSION_THRESHOLD_FILTER: - return std::make_shared::type>(); - case RS2_EXTENSION_DISPARITY_FILTER: - disparity = std::make_shared::type>(depth_to_disparity); - depth_to_disparity = false; - return disparity; - case RS2_EXTENSION_SPATIAL_FILTER: - return std::make_shared::type>(); - case RS2_EXTENSION_TEMPORAL_FILTER: - return std::make_shared::type>(); - case RS2_EXTENSION_HOLE_FILLING_FILTER: - return std::make_shared::type>(); - case RS2_EXTENSION_HDR_MERGE: - return std::make_shared::type>(); - case RS2_EXTENSION_SEQUENCE_ID_FILTER: - return std::make_shared::type>(); - default: - return nullptr; + std::string name = processing_block_msg->data; + if( name == "Disparity Filter" ) + { + // What was recorded was the extension type (without its settings!), but we need to create different + // variants. "Disparity Filter" gets recorded twice! This workaround ensures it's instantiated in its + // non-default flavor the second time: + if( depth_to_disparity ) + depth_to_disparity = false; + else + name = "Disparity to Depth"; + } + try + { + auto block = m_context->create_pp_block( name, {} ); + if( ! block ) + LOG_DEBUG( "unknown processing block '" << name << "'; ignored" ); + return block; + } + catch( std::exception const & e ) + { + LOG_DEBUG( "failed to create processing block '" << name << "': " << e.what() ); + return {}; } } diff --git a/src/media/ros/ros_reader.h b/src/media/ros/ros_reader.h index feb92e93574..00ad56e0cd0 100644 --- a/src/media/ros/ros_reader.h +++ b/src/media/ros/ros_reader.h @@ -115,7 +115,11 @@ namespace librealsense static std::pair> create_property(const rosbag::MessageInstance& property_message_instance); /*Starting version 3*/ static std::pair> create_option(const rosbag::Bag& file, const rosbag::MessageInstance& value_message_instance); - static std::shared_ptr create_processing_block(const rosbag::MessageInstance& value_message_instance, bool& depth_to_disparity, std::shared_ptr options); + + std::shared_ptr< processing_block_interface > + create_processing_block( const rosbag::MessageInstance & value_message_instance, + bool & depth_to_disparity, + std::shared_ptr< options_interface > options ); static notification create_notification(const rosbag::Bag& file, const rosbag::MessageInstance& message_instance); static std::shared_ptr read_sensor_options(const rosbag::Bag& file, device_serializer::sensor_identifier sensor_id, const nanoseconds& timestamp, uint32_t file_version); diff --git a/src/media/ros/ros_writer.cpp b/src/media/ros/ros_writer.cpp index 0373109e7f3..52988f417f4 100644 --- a/src/media/ros/ros_writer.cpp +++ b/src/media/ros/ros_writer.cpp @@ -492,11 +492,17 @@ namespace librealsense } } - rs2_extension ros_writer::get_processing_block_extension(const std::shared_ptr block) + static std::string get_processing_block_extension_name( const std::shared_ptr< processing_block_interface > block ) { -#define RETURN_IF_EXTENSION(E, T)\ - if (Is::type>(E))\ - return T;\ + // We want to write the block name (as opposed to the extension name): + // The block can behave differently and have a different name based on how it was created (e.g., the disparity + // filter). This makes new rosbag files incompatible with older librealsense versions. + if( block->supports_info( RS2_CAMERA_INFO_NAME ) ) + return block->get_info( RS2_CAMERA_INFO_NAME ); + +#define RETURN_IF_EXTENSION( B, E ) \ + if( Is< ExtensionToType< E >::type >( B ) ) \ + return rs2_extension_type_to_string( E ) RETURN_IF_EXTENSION(block, RS2_EXTENSION_DECIMATION_FILTER); RETURN_IF_EXTENSION(block, RS2_EXTENSION_THRESHOLD_FILTER); @@ -509,33 +515,29 @@ namespace librealsense #undef RETURN_IF_EXTENSION - throw invalid_value_exception( rsutils::string::from() - << "processing block " << block->get_info( RS2_CAMERA_INFO_NAME ) - << "has no map to extension" ); + return {}; } void ros_writer::write_sensor_processing_blocks(device_serializer::sensor_identifier sensor_id, const nanoseconds& timestamp, std::shared_ptr proccesing_blocks) { - rs2_extension ext = RS2_EXTENSION_UNKNOWN; for (auto block : proccesing_blocks->get_recommended_processing_blocks()) { + std::string name = get_processing_block_extension_name( block ); + if( name.empty() ) + { + LOG_WARNING( "Failed to get recommended processing block name for sensor " << sensor_id.sensor_index ); + continue; + } try { - try - { - ext = get_processing_block_extension(block); - } - catch (std::exception& e) - { - LOG_WARNING("Failed to write proccesing block " << " for sensor " << sensor_id.sensor_index << ". Exception: " << e.what()); - } std_msgs::String processing_block_msg; - processing_block_msg.data = rs2_extension_type_to_string(ext); - write_message(ros_topic::post_processing_blocks_topic(sensor_id), timestamp, processing_block_msg); + processing_block_msg.data = name; + write_message( ros_topic::post_processing_blocks_topic( sensor_id ), timestamp, processing_block_msg ); } - catch (std::exception& e) + catch( std::exception & e ) { - LOG_WARNING("Failed to get or write recommended proccesing blocks " << " for sensor " << sensor_id.sensor_index << ". Exception: " << e.what()); + LOG_WARNING( "Failed to write processing block '" << name << "' for sensor " << sensor_id.sensor_index + << ": " << e.what() ); } } } diff --git a/src/media/ros/ros_writer.h b/src/media/ros/ros_writer.h index a49f59ba12f..6fe6b483180 100644 --- a/src/media/ros/ros_writer.h +++ b/src/media/ros/ros_writer.h @@ -60,8 +60,6 @@ namespace librealsense void write_vendor_info(const std::string& topic, nanoseconds timestamp, std::shared_ptr info_snapshot); void write_sensor_option(device_serializer::sensor_identifier sensor_id, const nanoseconds& timestamp, rs2_option type, const librealsense::option& option); void write_sensor_options(device_serializer::sensor_identifier sensor_id, const nanoseconds& timestamp, std::shared_ptr options); - - rs2_extension get_processing_block_extension(const std::shared_ptr block); void write_sensor_processing_blocks(device_serializer::sensor_identifier sensor_id, const nanoseconds& timestamp, std::shared_ptr proccesing_blocks); template diff --git a/src/rscore-pp-block-factory.cpp b/src/rscore-pp-block-factory.cpp new file mode 100644 index 00000000000..df830fd1b0c --- /dev/null +++ b/src/rscore-pp-block-factory.cpp @@ -0,0 +1,53 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. + +#include "rscore-pp-block-factory.h" + +#include "proc/decimation-filter.h" +#include "proc/disparity-transform.h" +#include "proc/hdr-merge.h" +#include "proc/hole-filling-filter.h" +#include "proc/sequence-id-filter.h" +#include "proc/spatial-filter.h" +#include "proc/temporal-filter.h" +#include "proc/threshold.h" + +#include +#include + + +namespace librealsense { + + +std::shared_ptr< processing_block_interface > +rscore_pp_block_factory::create_pp_block( std::string const & name, nlohmann::json const & settings ) +{ + // These filters do not accept settings (nor are settings recorded in ros_writer) + (void *)&settings; + + if( rsutils::string::nocase_equal( name, "Decimation Filter" ) ) + return std::make_shared< decimation_filter >(); + if( rsutils::string::nocase_equal( name, "HDR Merge" ) ) // and Hdr Merge + return std::make_shared< hdr_merge >(); + if( rsutils::string::nocase_equal( name, "Filter By Sequence id" ) // name + || rsutils::string::nocase_equal( name, "Sequence Id Filter" ) ) // extension + return std::make_shared< sequence_id_filter >(); + if( rsutils::string::nocase_equal( name, "Threshold Filter" ) ) + return std::make_shared< threshold >(); + if( rsutils::string::nocase_equal( name, "Depth to Disparity" ) // name + || rsutils::string::nocase_equal( name, "Disparity Filter" ) ) // extension + return std::make_shared< disparity_transform >( true ); + if( rsutils::string::nocase_equal( name, "Disparity to Depth" ) ) + return std::make_shared< disparity_transform >( false ); + if( rsutils::string::nocase_equal( name, "Spatial Filter" ) ) + return std::make_shared< spatial_filter >(); + if( rsutils::string::nocase_equal( name, "Temporal Filter" ) ) + return std::make_shared< temporal_filter >(); + if( rsutils::string::nocase_equal( name, "Hole Filling Filter" ) ) + return std::make_shared< hole_filling_filter >(); + + return {}; +} + + +} // namespace librealsense diff --git a/src/rscore-pp-block-factory.h b/src/rscore-pp-block-factory.h new file mode 100644 index 00000000000..4671d54ea24 --- /dev/null +++ b/src/rscore-pp-block-factory.h @@ -0,0 +1,19 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. +#pragma once + +#include "core/pp-block-factory.h" + + +namespace librealsense { + + +class rscore_pp_block_factory : public pp_block_factory +{ +public: + std::shared_ptr< processing_block_interface > create_pp_block( std::string const & name, + nlohmann::json const & settings ) override; +}; + + +} // namespace librealsense