diff --git a/.github/workflows/static_analysis.yaml b/.github/workflows/static_analysis.yaml index b5840c5ab4..a811f1a0a9 100644 --- a/.github/workflows/static_analysis.yaml +++ b/.github/workflows/static_analysis.yaml @@ -194,3 +194,15 @@ jobs: echo "Error - The minimal CMake version required for LibRS is ${EXPECTED_CMAKE_MAJOR_VER}.${EXPECTED_CMAKE_MINOR_VER} but on this build the minimal CMake version that works is $CURRENT_CMAKE_MAJOR_VER.$CURRENT_CMAKE_MINOR_VER" exit 1 fi + + build_flags_docs: + name: "Generate build-flags.html" + timeout-minutes: 10 + runs-on: ubuntu-22.04 + steps: + - name: Checkout + uses: actions/checkout@f43a0e5ff2bd294095638e18286ca9a3d1956744 #v3 + + - name: Build docs + run: | + python3 scripts/lrs_options-to-html.py diff --git a/CMake/lrs_options.cmake b/CMake/lrs_options.cmake index 02ca70796b..94b25621ac 100644 --- a/CMake/lrs_options.cmake +++ b/CMake/lrs_options.cmake @@ -1,3 +1,9 @@ +## This file is also being used to generate our build flags document at https://intelrealsense.github.io/librealsense/build-flags-docs/build-flags.html +## Formatting notes for this file: +## Options are listed as: | [comment] | +## regular comments should be ABOVE their relevent option +## use double # for comments that should not show in the options doc + option(ENABLE_CCACHE "Build with ccache." ON) option(BUILD_WITH_CUDA "Enable CUDA" OFF) option(BUILD_GLSL_EXTENSIONS "Build GLSL extensions API" ON) diff --git a/common/d500-on-chip-calib.cpp b/common/d500-on-chip-calib.cpp index 5f36a508c6..59b7c29b73 100644 --- a/common/d500-on-chip-calib.cpp +++ b/common/d500-on-chip-calib.cpp @@ -88,7 +88,8 @@ namespace rs2 auto depth_sensor = _sub->s->as (); // disabling the depth visual preset change for D555 - not needed - if (get_device_pid() != "0B56" && get_device_pid() != "DDS") + std::string dev_name = _dev.supports( RS2_CAMERA_INFO_NAME ) ? _dev.get_info( RS2_CAMERA_INFO_NAME ) : ""; + if( dev_name.find( "D555" ) == std::string::npos ) { // set depth preset as default preset set_option_if_needed(depth_sensor, RS2_OPTION_VISUAL_PRESET, 1); diff --git a/common/device-model.cpp b/common/device-model.cpp index 319425eda3..72d37d2bb6 100644 --- a/common/device-model.cpp +++ b/common/device-model.cpp @@ -3351,10 +3351,10 @@ namespace rs2 { bool is_d555 = false; - if( dev.supports( RS2_CAMERA_INFO_PRODUCT_ID ) ) + if( dev.supports( RS2_CAMERA_INFO_NAME ) ) { - auto pid_str = std::string( dev.get_info( RS2_CAMERA_INFO_PRODUCT_ID ) ); - if( pid_str == "0B56" || pid_str == "DDS" ) + auto dev_name = std::string( dev.get_info( RS2_CAMERA_INFO_NAME ) ); + if( dev_name.find( "D555" ) != std::string::npos ) is_d555 = true; } diff --git a/common/output-model.cpp b/common/output-model.cpp index 937478a8b3..9ebe9b473a 100644 --- a/common/output-model.cpp +++ b/common/output-model.cpp @@ -893,6 +893,7 @@ void output_model::add_log(rs2_log_severity severity, std::string filename, int void output_model::run_command(std::string command, device_models_list & device_models) { + std::string opcode_error_as_string = ""; try { if (to_lower(command) == "clear") @@ -990,7 +991,11 @@ void output_model::run_command(std::string command, device_models_list & device_ { found = true; auto res = dbg.send_and_receive_raw_data(buffer); - + if (res.data()) + { + int8_t opcode = *res.data(); + opcode_error_as_string = dbg.get_opcode_string(opcode); + } std::string response = rsutils::string::from() << "\n" << terminal_parser.parse_response(to_lower(command), res); add_log(RS2_LOG_SEVERITY_INFO, __FILE__, 0, response); } @@ -1006,7 +1011,10 @@ void output_model::run_command(std::string command, device_models_list & device_ } catch(const std::exception& ex) { - add_log( RS2_LOG_SEVERITY_ERROR, __FILE__, __LINE__, ex.what() ); + std::string error_string = rsutils::string::from() << ex.what(); + if (opcode_error_as_string != "") + error_string = rsutils::string::from() << error_string << " (" << opcode_error_as_string << ")"; + add_log( RS2_LOG_SEVERITY_ERROR, __FILE__, __LINE__, error_string); } } diff --git a/common/subdevice-model.cpp b/common/subdevice-model.cpp index 3a7914f6cb..b4dbf10ce5 100644 --- a/common/subdevice-model.cpp +++ b/common/subdevice-model.cpp @@ -194,8 +194,6 @@ namespace rs2 } catch (...) {} - auto filters = s->get_recommended_filters(); - for (auto&& f : s->get_recommended_filters()) { auto shared_filter = std::make_shared(f); @@ -215,6 +213,9 @@ namespace rs2 model->enable(false); } + if( shared_filter->is< rotation_filter >() ) + model->enable( false ); + if (shared_filter->is()) { if (s->supports(RS2_CAMERA_INFO_PRODUCT_ID)) diff --git a/doc/build-flags.css b/doc/build-flags.css new file mode 100644 index 0000000000..57e78b2593 --- /dev/null +++ b/doc/build-flags.css @@ -0,0 +1,91 @@ +/* This file is used in lrs_options-to-html, creating the html build-flags.html file we use to keep + the flags updated */ +body { + font-family: 'Roboto', Arial, sans-serif; + background-color: #f5f5f5; + margin: 0; + padding: 0; +} + +.container { + width: 80%; + margin: 0 auto; + padding: 30px; +} + +h1 { + text-align: center; + margin-bottom: 20px; +} + +h2 { + text-align: center; + margin-bottom: 30px; +} + +table { + width: 100%; + border-collapse: collapse; + box-shadow: 0 0 20px rgba(0, 0, 0, 0.1); + border-radius: 8px; + overflow: visible; +} + +th, td { + padding: 16px; + text-align: left; + border: 1px solid #ddd; +} + +th { + background-color: #5873e0; + color: white; +} + +tr:hover { + background-color: #f0f0f0; +} + +code { + padding: 2px 4px; + border-radius: 4px; + font-family: 'Roboto Mono', monospace; +} +/* --- Tooltip --- */ +.tooltip { + position: relative; + display: block; +} + +.tooltip .tooltip-text { + visibility: hidden; + /*width: 120px;*/ + background-color: #555; + color: #fff; + text-align: center; + padding: 5%; + border-radius: 6px; + position: absolute; + z-index: 1; + bottom: 125%; + left: 50%; + margin-left: -60px; + opacity: 0; + transition: opacity 0.3s; +} + +.tooltip .tooltip-text::after { + content: ""; + position: absolute; + top: 100%; + left: 50%; + margin-left: -5px; + border-width: 5px; + border-style: solid; + border-color: #555 transparent transparent transparent; +} + +.tooltip:hover .tooltip-text { + visibility: visible; + opacity: 1; +} diff --git a/doc/build-flags.ico b/doc/build-flags.ico new file mode 100644 index 0000000000..ff97f560e5 Binary files /dev/null and b/doc/build-flags.ico differ diff --git a/examples/post-processing/rs-post-processing.cpp b/examples/post-processing/rs-post-processing.cpp index 96355e7ef1..fac5ccb246 100644 --- a/examples/post-processing/rs-post-processing.cpp +++ b/examples/post-processing/rs-post-processing.cpp @@ -22,6 +22,7 @@ struct filter_slider_ui std::string description; bool is_int; float value; + float step; rs2::option_range range; bool render(const float3& location, bool enabled); @@ -71,6 +72,7 @@ int main(int argc, char * argv[]) try // Declare filters rs2::decimation_filter dec_filter; // Decimation - reduces depth frame density + rs2::rotation_filter rot_filter; // Rotation - rotates frames rs2::threshold_filter thr_filter; // Threshold - removes values outside recommended range rs2::spatial_filter spat_filter; // Spatial - edge-preserving spatial smoothing rs2::temporal_filter temp_filter; // Temporal - reduces temporal noise @@ -85,6 +87,7 @@ int main(int argc, char * argv[]) try // The following order of emplacement will dictate the orders in which filters are applied filters.emplace_back("Decimate", dec_filter); + filters.emplace_back("Rotate", rot_filter); filters.emplace_back("Threshold", thr_filter); filters.emplace_back(disparity_filter_name, depth_to_disparity); filters.emplace_back("Spatial", spat_filter); @@ -115,11 +118,12 @@ int main(int argc, char * argv[]) try /* Apply filters. The implemented flow of the filters pipeline is in the following order: 1. apply decimation filter - 2. apply threshold filter - 3. transform the scene into disparity domain - 4. apply spatial filter - 5. apply temporal filter - 6. revert the results back (if step Disparity filter was applied + 2. apply rotation filter + 3. apply threshold filter + 4. transform the scene into disparity domain + 5. apply spatial filter + 6. apply temporal filter + 7. revert the results back (if step Disparity filter was applied to depth domain (each post processing block is optional and can be applied independantly). */ bool revert_disparity = false; @@ -269,11 +273,12 @@ void render_ui(float w, float h, std::vector& filters) ImGui::Checkbox(filter.filter_name.c_str(), &tmp_value); filter.is_enabled = tmp_value; ImGui::PopStyleColor(); - - if (filter.supported_options.size() == 0) + + if( filter.supported_options.size() == 0 ) { offset_y += elements_margin; } + // Draw a slider for each of the filter's options for (auto& option_slider_pair : filter.supported_options) { @@ -316,11 +321,21 @@ bool filter_slider_ui::render(const float3& location, bool enabled) { int value_as_int = static_cast(value); value_changed = ImGui::SliderInt(("##" + name).c_str(), &value_as_int, static_cast(range.min), static_cast(range.max), "%.0f"); + if( step > 1 ) + { + value_as_int = static_cast< int >( range.min ) + + ( ( value_as_int - static_cast< int >( range.min ) ) / static_cast< int >( step ) ) + * static_cast< int >( step ); + } value = static_cast(value_as_int); } else { value_changed = ImGui::SliderFloat(("##" + name).c_str(), &value, range.min, range.max, "%.3f", 1.0f); + if( step > 0.0f ) + { + value = range.min + round( ( value - range.min ) / step ) * step; + } } ImGui::PopItemWidth(); @@ -356,12 +371,13 @@ filter_options::filter_options(const std::string name, rs2::filter& flt) : filter(flt), is_enabled(true) { - const std::array possible_filter_options = { + const std::array possible_filter_options = { RS2_OPTION_FILTER_MAGNITUDE, RS2_OPTION_FILTER_SMOOTH_ALPHA, RS2_OPTION_MIN_DISTANCE, RS2_OPTION_MAX_DISTANCE, - RS2_OPTION_FILTER_SMOOTH_DELTA + RS2_OPTION_FILTER_SMOOTH_DELTA, + RS2_OPTION_ROTATION }; //Go over each filter option and create a slider for it @@ -378,6 +394,7 @@ filter_options::filter_options(const std::string name, rs2::filter& flt) : supported_options[opt].name = name + "_" + opt_name; std::string prefix = "Filter "; supported_options[opt].label = opt_name; + supported_options[opt].step = range.step; } } } diff --git a/include/librealsense2/h/rs_option.h b/include/librealsense2/h/rs_option.h index 10d03fe2e8..80c30bc853 100644 --- a/include/librealsense2/h/rs_option.h +++ b/include/librealsense2/h/rs_option.h @@ -125,6 +125,7 @@ extern "C" { RS2_OPTION_SOC_PVT_TEMPERATURE, /**< Temperature of PVT SOC */ RS2_OPTION_GYRO_SENSITIVITY,/**< Control of the gyro sensitivity level, see rs2_gyro_sensitivity for values */ RS2_OPTION_REGION_OF_INTEREST,/**< The rectangular area used from the streaming profile */ + RS2_OPTION_ROTATION,/**Rotates frames*/ RS2_OPTION_COUNT /**< Number of enumeration values. Not a valid input: intended to be used in for-loops. */ } rs2_option; diff --git a/include/librealsense2/h/rs_processing.h b/include/librealsense2/h/rs_processing.h index 745b72c54d..e13e1484a8 100644 --- a/include/librealsense2/h/rs_processing.h +++ b/include/librealsense2/h/rs_processing.h @@ -224,6 +224,12 @@ rs2_processing_block* rs2_create_align(rs2_stream align_to, rs2_error** error); */ rs2_processing_block* rs2_create_decimation_filter_block(rs2_error** error); +/** + * Creates post-processing filter block. This block accepts frames and applies rotation filter + * \param[out] error if non-null, receives any error that occurs during this call, otherwise, errors are ignored + */ +rs2_processing_block * rs2_create_rotation_filter_block( rs2_error ** error ); + /** * Creates Depth post-processing filter block. This block accepts depth frames, applies temporal filter * \param[out] error if non-null, receives any error that occurs during this call, otherwise, errors are ignored diff --git a/include/librealsense2/h/rs_types.h b/include/librealsense2/h/rs_types.h index 775fb5dd83..5543fa00fe 100644 --- a/include/librealsense2/h/rs_types.h +++ b/include/librealsense2/h/rs_types.h @@ -161,6 +161,7 @@ typedef enum rs2_extension RS2_EXTENSION_SOFTWARE_DEVICE, RS2_EXTENSION_SOFTWARE_SENSOR, RS2_EXTENSION_DECIMATION_FILTER, + RS2_EXTENSION_ROTATION_FILTER, RS2_EXTENSION_THRESHOLD_FILTER, RS2_EXTENSION_DISPARITY_FILTER, RS2_EXTENSION_SPATIAL_FILTER, diff --git a/include/librealsense2/hpp/rs_device.hpp b/include/librealsense2/hpp/rs_device.hpp index 4068dc5a53..bc7af22454 100644 --- a/include/librealsense2/hpp/rs_device.hpp +++ b/include/librealsense2/hpp/rs_device.hpp @@ -1042,6 +1042,14 @@ namespace rs2 return results; } + + std::string get_opcode_string(int opcode) + { + rs2_error* e = nullptr; + char buffer[1024]; + rs2_hw_monitor_get_opcode_string(opcode, buffer, sizeof(buffer), _dev.get(), &e); + return std::string(buffer); + } }; class device_list diff --git a/include/librealsense2/hpp/rs_processing.hpp b/include/librealsense2/hpp/rs_processing.hpp index 30a2181c50..3344a54860 100644 --- a/include/librealsense2/hpp/rs_processing.hpp +++ b/include/librealsense2/hpp/rs_processing.hpp @@ -851,6 +851,48 @@ namespace rs2 } }; + class rotation_filter : public filter + { + public: + /** + * Create rotation filter + * Rotation filter performs rotation of the frames + */ + rotation_filter() + : filter( init(), 1 ) + { + } + + rotation_filter( float value ) + : filter( init(), 1 ) + { + set_option( RS2_OPTION_ROTATION, value ); + } + + rotation_filter( filter f ) + : filter( f ) + { + rs2_error * e = nullptr; + if( ! rs2_is_processing_block_extendable_to( f.get(), RS2_EXTENSION_ROTATION_FILTER, &e ) && ! e ) + { + _block.reset(); + } + error::handle( e ); + } + + private: + friend class context; + + std::shared_ptr< rs2_processing_block > init() + { + rs2_error * e = nullptr; + auto block = std::shared_ptr< rs2_processing_block >( rs2_create_rotation_filter_block( &e ), + rs2_delete_processing_block ); + error::handle( e ); + return block; + } + }; + class temporal_filter : public filter { public: diff --git a/include/librealsense2/rs.h b/include/librealsense2/rs.h index fe0ef17edf..a712d4ba6e 100644 --- a/include/librealsense2/rs.h +++ b/include/librealsense2/rs.h @@ -136,6 +136,8 @@ float rs2_depth_frame_get_distance(const rs2_frame* frame_ref, int x, int y, rs2 */ rs2_time_t rs2_get_time( rs2_error** error); +void rs2_hw_monitor_get_opcode_string(int opcode, char* buffer, size_t buffer_size,rs2_device* device, rs2_error** error); + #ifdef __cplusplus } #endif diff --git a/scripts/lrs_options-to-html.py b/scripts/lrs_options-to-html.py new file mode 100644 index 0000000000..03ff168f59 --- /dev/null +++ b/scripts/lrs_options-to-html.py @@ -0,0 +1,146 @@ +import sys + + +def add_style(option, val): + # colorize ON and OFF keywords + val = val.replace('ON', 'ON') + val = val.replace('OFF', 'OFF') + + # if more complicated conditions are being used, this can be disabled - + # if we see an option more than once at add_row, just set its value to be 'Environment dependant' + # this should handle if/elseif/else structures of cmake + if in_cond: + # text will say Environment dependant, and if hovered will show the condition + val = f'Environment dependant{val}
' + elif in_elseif: + # append condition to tooltip + val = table_rows[option][2].replace("
", f',
{val}
') + + elif in_else: + # append otherwise to tooltip + val = table_rows[option][2].replace("
", f',
{val} otherwise') + + return val + + +def add_row(option, description, value): + if option in table_rows: + # update value if option exists + table_rows[option][2] = value + else: + # add to table new option + table_rows[option] = [option, description, value] + + +with open('CMake/lrs_options.cmake', 'r') as file: + lines = file.readlines() + +table_rows = {} +in_cond = False +in_elseif = False +in_else = False +current_condition = None +current_comment = "" + +for i, line in enumerate(lines): + if line.strip().startswith(('option(', 'set(')): + line = ' '.join(line.split()) # remove consecutive spaces + parts = line.strip().split(' ') + if line.strip().startswith('option('): + option = parts[0].strip('option(') + value = parts[-1].strip(")") # the last word in line is the value (ON/OFF by default) + + # concatenate rest of the line - expected to be just the description in quotes + description = ' '.join(parts[1:-1]).strip('"') + else: + option = parts[0].strip('set(') + value = parts[1] + value = f'{value}' + # parts[2] is expected to be 'CACHE' + vartype = parts[3] # INT, STRING ... + + # concatenate rest of the line - expected to be just the description in quotes + description = ' '.join(parts[4:-1]).strip('"') + f" (type {vartype})" + + if current_comment: + description += " " + current_comment + "" + current_comment = "" + + if in_cond or in_elseif: + value = f'{value} if {current_condition}' + + value = add_style(option, value) + add_row(option, description, value) + elif line.startswith('if'): + parts = line.strip().split('(', 1) + condition = parts[1][:-1] # remove last ")" - part of the 'if' + current_condition = condition + in_cond = True + elif line.startswith('elseif'): + parts = line.strip().split('(', 1) + condition = parts[1][:-1] # remove last ")" - part of the 'if' + current_condition = condition + in_cond = False + in_elseif = True + elif line.startswith('else'): + in_cond = False + in_elseif = False + in_else = True + elif line.startswith('endif'): + current_condition = None + in_cond = False + in_else = False + elif line.startswith('##'): + continue # ignore internal comments + elif line.startswith('#'): + current_comment += line.strip('# \n') + elif line.strip(): + # if we reach a line that doesn't match the pattern, throw an error as it's not handled (shouldn't happen) + raise Exception(f"{i, line} not handled") + + +def format_dict_values(): + return ''.join( + f'\n ' + f'\n\t\n\t {option}'f'\n\t' + f'\n\t\n\t {description}\n\t' + f'\n\t\n\t {value}\n\t' + f'\n ' + for option, description, value in table_rows.values()) + +def get_sdk_version(): + if len(sys.argv) > 1: + # the script can get the version number as a parameter + version = sys.argv[1] + return f'Version {version}' + else: + # if no parameter provided - no version number will be included + return "" + + +html = f''' + + + Build Customization Flags + + + + +
+

Intel RealSense™ SDK Build Customization Flags

+

{get_sdk_version()}

+ + + + + + {format_dict_values()} +
OptionDescriptionDefault
+
+ + +''' + +with open('doc/build-flags.html', 'w') as file: + file.write(html) + print("build-flags.html generated") diff --git a/src/core/debug.h b/src/core/debug.h index 1d5956e4aa..5d232b068d 100644 --- a/src/core/debug.h +++ b/src/core/debug.h @@ -4,6 +4,7 @@ #include "extension.h" #include +#include namespace librealsense { @@ -18,6 +19,7 @@ namespace librealsense uint32_t param4 = 0, uint8_t const * data = nullptr, size_t dataLength = 0) const = 0; + virtual std::string get_opcode_string(int opcode) const = 0; }; MAP_EXTENSION(RS2_EXTENSION_DEBUG, librealsense::debug_interface); diff --git a/src/dds/rs-dds-device-proxy.cpp b/src/dds/rs-dds-device-proxy.cpp index 10503106f5..bc01659712 100644 --- a/src/dds/rs-dds-device-proxy.cpp +++ b/src/dds/rs-dds-device-proxy.cpp @@ -28,6 +28,7 @@ #include #include +#include using rsutils::json; @@ -556,9 +557,9 @@ void dds_device_proxy::tag_default_profile_of_stream( // Superset = picked up in pipeline with enable_all_stream() config // We want color and depth to be default, and add infrared as superset int tag = PROFILE_TAG_SUPERSET; - if( stream->type_string() == "color" || stream->type_string() == "depth" ) + if( strcmp( stream->type_string(), "color" ) == 0 || strcmp( stream->type_string(), "depth" ) == 0 ) tag |= PROFILE_TAG_DEFAULT; - else if( stream->type_string() != "infrared" || profile->get_stream_index() >= 2 ) + else if( strcmp( stream->type_string(), "ir" ) != 0 || profile->get_stream_index() >= 2 ) return; // leave untagged profile->tag_profile( tag ); } @@ -578,6 +579,22 @@ void dds_device_proxy::hardware_reset() _dds_dev->send_control( control, &reply ); } +std::string dds_device_proxy::get_opcode_string(int opcode) const +{ + std::string product_line = get_info(RS2_CAMERA_INFO_PRODUCT_LINE); + if (product_line.find("D400") != std::string::npos) + { + // d400 device + return ds::d400_hwmon_response().hwmon_error2str(opcode); + } + else if (product_line.find("D500") != std::string::npos) + { + // d500 device + return ds::d500_hwmon_response().hwmon_error2str(opcode); + } + return ""; +} + std::vector< uint8_t > dds_device_proxy::send_receive_raw_data( const std::vector< uint8_t > & input ) { diff --git a/src/dds/rs-dds-device-proxy.h b/src/dds/rs-dds-device-proxy.h index f442b1f5b4..797cbf4e9a 100644 --- a/src/dds/rs-dds-device-proxy.h +++ b/src/dds/rs-dds-device-proxy.h @@ -84,6 +84,7 @@ class dds_device_proxy uint32_t param4 = 0, uint8_t const * data = nullptr, size_t dataLength = 0 ) const override; + std::string get_opcode_string(int opcode) const override; // updatable: unsigned, non-recovery-mode private: diff --git a/src/dds/rsdds-device-factory.cpp b/src/dds/rsdds-device-factory.cpp index 0338443dbb..a2fc144ab2 100644 --- a/src/dds/rsdds-device-factory.cpp +++ b/src/dds/rsdds-device-factory.cpp @@ -51,8 +51,15 @@ class rsdds_watcher_singleton _device_watcher->on_device_added( [this]( std::shared_ptr< realdds::dds_device > const & dev ) { - dev->wait_until_ready(); // make sure handshake is complete - _callbacks.raise( dev, true ); + try + { + dev->wait_until_ready(); // make sure handshake is complete, might throw + _callbacks.raise( dev, true ); + } + catch (std::runtime_error e) + { + LOG_ERROR( "Discovered DDS device failed to be ready within timeout" << e.what() ); + } } ); _device_watcher->on_device_removed( [this]( std::shared_ptr< realdds::dds_device > const & dev ) diff --git a/src/ds/d400/d400-device.cpp b/src/ds/d400/d400-device.cpp index 5018b15992..d63f190bf4 100644 --- a/src/ds/d400/d400-device.cpp +++ b/src/ds/d400/d400-device.cpp @@ -148,6 +148,11 @@ namespace librealsense return result; } + std::string d400_device::get_opcode_string(int opcode) const + { + return ds::d400_hwmon_response().hwmon_error2str(opcode); + } + class d400_depth_sensor : public synthetic_sensor , public video_sensor_interface diff --git a/src/ds/d400/d400-device.h b/src/ds/d400/d400-device.h index 0816b54d03..03de43540e 100644 --- a/src/ds/d400/d400-device.h +++ b/src/ds/d400/d400-device.h @@ -66,6 +66,7 @@ namespace librealsense std::vector backup_flash( rs2_update_progress_callback_sptr callback) override; void update_flash(const std::vector& image, rs2_update_progress_callback_sptr callback, int update_mode) override; bool check_fw_compatibility(const std::vector& image) const override; + std::string get_opcode_string(int opcode) const override; protected: std::shared_ptr _ds_device_common; diff --git a/src/ds/d500/d500-auto-calibration.cpp b/src/ds/d500/d500-auto-calibration.cpp index d461d2be9b..d53d303752 100644 --- a/src/ds/d500/d500-auto-calibration.cpp +++ b/src/ds/d500/d500-auto-calibration.cpp @@ -95,8 +95,8 @@ namespace librealsense { bool is_d555 = false; auto dev = As< device >( _debug_dev ); - std::string pid_str = dev ? dev->get_info( RS2_CAMERA_INFO_PRODUCT_ID ) : ""; - if( pid_str == "0B56" || pid_str == "DDS" ) + std::string dev_name = dev ? dev->get_info( RS2_CAMERA_INFO_NAME ) : ""; + if( dev_name.find( "D555" ) != std::string::npos ) is_d555 = true; if( is_d555 ) diff --git a/src/ds/d500/d500-device.cpp b/src/ds/d500/d500-device.cpp index b88393d107..843585f891 100644 --- a/src/ds/d500/d500-device.cpp +++ b/src/ds/d500/d500-device.cpp @@ -116,6 +116,11 @@ namespace librealsense throw not_implemented_exception("D500 device does not support unsigned FW update"); } + std::string d500_device::get_opcode_string(int opcode) const + { + return _hw_monitor_response->hwmon_error2str(opcode); + } + class d500_depth_sensor : public synthetic_sensor, public video_sensor_interface, public depth_stereo_sensor, public roi_sensor_base { public: @@ -281,8 +286,18 @@ namespace librealsense float d500_device::get_stereo_baseline_mm() const // to be d500 adapted { using namespace ds; - auto table = check_calib(*_coefficients_table_raw); - return fabs(table->baseline); + float baseline = 100.0f; // so we will have a non zero value if cannot read from table + try + { + auto table = check_calib(*_coefficients_table_raw); + baseline = fabs(table->baseline); + } + catch( const std::exception &e ) + { + LOG_ERROR("Failed reading stereo baseline, using default value --> " << e.what() ); + } + + return baseline; } std::vector d500_device::get_d500_raw_calibration_table(ds::d500_calibration_table_id table_id) const // to be d500 adapted @@ -382,7 +397,8 @@ namespace librealsense _depth_stream(new stream(RS2_STREAM_DEPTH)), _left_ir_stream(new stream(RS2_STREAM_INFRARED, 1)), _right_ir_stream(new stream(RS2_STREAM_INFRARED, 2)), - _color_stream(nullptr) + _color_stream(nullptr), + _hw_monitor_response(std::make_shared()) { _depth_device_idx = add_sensor( create_depth_device( dev_info->get_context(), dev_info->get_group().uvc_devices ) ); @@ -407,13 +423,13 @@ namespace librealsense _hw_monitor = std::make_shared( std::make_shared( std::make_shared( *raw_sensor, depth_xu, DS5_HWMONITOR ), - raw_sensor), std::make_shared()); + raw_sensor), _hw_monitor_response); } else { _hw_monitor = std::make_shared< hw_monitor_extended_buffers >( std::make_shared< locked_transfer >( get_backend()->create_usb_device( group.usb_devices.front() ), - raw_sensor ), std::make_shared()); + raw_sensor ), _hw_monitor_response); } _ds_device_common = std::make_shared(this, _hw_monitor); @@ -455,8 +471,19 @@ namespace librealsense bool advanced_mode = false; bool usb_modality = true; group_multiple_fw_calls(depth_sensor, [&]() { + + // D500 device can get enumerated before the whole HW in the camera is ready. + // Since GVD gather all information from all the HW, it might need some more time to finish all hand shakes. + // on this case it will return HW_NOT_READY error code. + // Note: D500 error codes list is different than D400. + + const std::set< int32_t > gvd_retry_errors{ _hw_monitor_response->HW_NOT_READY }; + + _hw_monitor->get_gvd( gvd_buff.size(), + gvd_buff.data(), + ds::fw_cmd::GVD, + &gvd_retry_errors ); - _hw_monitor->get_gvd(gvd_buff.size(), gvd_buff.data(), ds::fw_cmd::GVD); get_gvd_details(gvd_buff, &gvd_parsed_fields); _device_capabilities = ds_caps::CAP_ACTIVE_PROJECTOR | ds_caps::CAP_RGB_SENSOR | ds_caps::CAP_IMU_SENSOR | diff --git a/src/ds/d500/d500-device.h b/src/ds/d500/d500-device.h index a0c14ea43c..3679e16f5c 100644 --- a/src/ds/d500/d500-device.h +++ b/src/ds/d500/d500-device.h @@ -51,6 +51,8 @@ namespace librealsense d500_device( std::shared_ptr< const d500_info > const & ); + std::shared_ptr _hw_monitor_response; + std::vector send_receive_raw_data(const std::vector& input) override; std::vector build_command(uint32_t opcode, @@ -70,6 +72,7 @@ namespace librealsense std::vector backup_flash( rs2_update_progress_callback_sptr callback ) override; void update_flash(const std::vector& image, rs2_update_progress_callback_sptr callback, int update_mode) override; bool check_fw_compatibility( const std::vector& image ) const override { return true; }; + std::string get_opcode_string(int opcode) const override; protected: std::shared_ptr _ds_device_common; diff --git a/src/ds/d500/d500-private.cpp b/src/ds/d500/d500-private.cpp index fb597f8d0e..425b663877 100644 --- a/src/ds/d500/d500-private.cpp +++ b/src/ds/d500/d500-private.cpp @@ -47,15 +47,31 @@ namespace librealsense { case d500_calibration_table_id::depth_calibration_id: { - return get_d500_depth_intrinsic_by_resolution(raw_data, width, height, is_symmetrization_enabled); + if ( !raw_data.empty() ) + return get_d500_depth_intrinsic_by_resolution(raw_data, width, height, is_symmetrization_enabled); + else + LOG_ERROR("Cannot read depth table intrinsic values, using default values"); + break; } case d500_calibration_table_id::rgb_calibration_id: { - return get_d500_color_intrinsic_by_resolution(raw_data, width, height); + if ( !raw_data.empty() ) + return get_d500_color_intrinsic_by_resolution(raw_data, width, height); + else + LOG_ERROR("Cannot read color table intrinsic values, using default values"); + break; } default: throw invalid_value_exception(rsutils::string::from() << "Parsing Calibration table type " << static_cast(table_id) << " is not supported"); } + + // If we got here, the table is empty so continue with default values + rs2_intrinsics intrinsics = {0}; + intrinsics.height = height; + intrinsics.width = width; + intrinsics.ppx = intrinsics.fx = width / 2.f; + intrinsics.ppy = intrinsics.fy = height / 2.f; + return intrinsics; } // Algorithm prepared by Oscar Pelc in matlab: diff --git a/src/hw-monitor.cpp b/src/hw-monitor.cpp index aa6dce3681..6a01b540a2 100644 --- a/src/hw-monitor.cpp +++ b/src/hw-monitor.cpp @@ -2,6 +2,7 @@ // Copyright(c) 2015 Intel Corporation. All Rights Reserved. #include "hw-monitor.h" #include "types.h" +#include #include #include #include @@ -226,10 +227,36 @@ namespace librealsense } - void hw_monitor::get_gvd(size_t sz, unsigned char* gvd, uint8_t gvd_cmd) const + void hw_monitor::get_gvd( size_t sz, + unsigned char * gvd, + uint8_t gvd_cmd, + const std::set< int32_t > * retry_error_codes ) const { command command(gvd_cmd); - auto data = send(command); + hwmon_response_type response; + auto data = send( command, &response ); + // If we get an error code that match to the error code defined as require retry, + // we will retry the command until it succeed or we reach a timeout + bool should_retry = retry_error_codes && retry_error_codes->find( response ) != retry_error_codes->end(); + if( should_retry ) + { + constexpr size_t RETRIES = 50; + for( int i = 0; i < RETRIES; ++i ) + { + LOG_WARNING( "GVD not ready - retrying GET_GVD command" ); + std::this_thread::sleep_for( std::chrono::milliseconds( 100 ) ); + data = send( command, &response ); + if( response == _hwmon_response->success_value() ) + break; + // If we failed after 'RETRIES' retries or it is less `RETRIES` and the error + // code is not in the retry list than , raise an exception + if( i >= ( RETRIES - 1 ) || retry_error_codes->find( response ) == retry_error_codes->end() ) + throw io_exception( rsutils::string::from() + << "error in querying GVD, error:" + << _hwmon_response->hwmon_error2str( response ) ); + + } + } auto minSize = std::min(sz, data.size()); std::memcpy( gvd, data.data(), minSize ); } diff --git a/src/hw-monitor.h b/src/hw-monitor.h index 3f99198ba3..bc0b5221da 100644 --- a/src/hw-monitor.h +++ b/src/hw-monitor.h @@ -176,7 +176,10 @@ namespace librealsense uint8_t const * data = nullptr, size_t dataLength = 0); - void get_gvd(size_t sz, unsigned char* gvd, uint8_t gvd_cmd) const; + void get_gvd( size_t sz, + unsigned char * gvd, + uint8_t gvd_cmd, + const std::set< int32_t > * retry_error_codes = nullptr ) const; template std::string get_firmware_version_string( const std::vector< uint8_t > & buff, diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index ad271ec3eb..8889d3f50d 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -1205,7 +1205,10 @@ namespace librealsense } v4l_uvc_device::v4l_uvc_device(const uvc_device_info& info, bool use_memory_map) - : _name(""), _info(), + : _name(info.id), + _device_path(info.device_path), + _device_usb_spec(info.conn_spec), + _info(info), _is_capturing(false), _is_alive(true), _is_started(false), @@ -1217,19 +1220,6 @@ namespace librealsense _buf_dispatch(use_memory_map), _frame_drop_monitor(DEFAULT_KPI_FRAME_DROPS_PERCENTAGE) { - foreach_uvc_device([&info, this](const uvc_device_info& i, const std::string& name) - { - if (i == info) - { - _name = name; - _info = i; - _device_path = i.device_path; - _device_usb_spec = i.conn_spec; - } - }); - if (_name == "") - throw linux_backend_exception("device is no longer connected!"); - _named_mtx = std::unique_ptr(new named_mutex(_name, 5000)); } diff --git a/src/media/ros/ros_file_format.h b/src/media/ros/ros_file_format.h index 11c9c64a07..24a7c3a6c2 100644 --- a/src/media/ros/ros_file_format.h +++ b/src/media/ros/ros_file_format.h @@ -417,6 +417,7 @@ namespace librealsense case RS2_STREAM_GYRO: case RS2_STREAM_ACCEL: + case RS2_STREAM_MOTION: return ros_imu_type_str(); case RS2_STREAM_POSE: diff --git a/src/media/ros/ros_reader.cpp b/src/media/ros/ros_reader.cpp index ce5df08463..b98d2ec5e8 100644 --- a/src/media/ros/ros_reader.cpp +++ b/src/media/ros/ros_reader.cpp @@ -486,9 +486,11 @@ namespace librealsense get_frame_metadata(m_file, info_topic, stream_id, motion_data, additional_data); } + size_t size_of_imu_data = (stream_id.stream_type == RS2_STREAM_MOTION) ? sizeof(rs2_combined_motion) : 3 * sizeof(float); + frame_interface * frame = m_frame_source->alloc_frame( { stream_id.stream_type, stream_id.stream_index, RS2_EXTENSION_MOTION_FRAME }, - 3 * sizeof( float ), + size_of_imu_data, std::move( additional_data ), true ); if (frame == nullptr) @@ -518,6 +520,24 @@ namespace librealsense data[2] = static_cast(msg->angular_velocity.z); LOG_DEBUG("RS2_STREAM_GYRO " << motion_frame); } + else if (stream_id.stream_type == RS2_STREAM_MOTION) + { + auto data = reinterpret_cast(motion_frame->data.data()); + // orientation part + data->orientation.x = msg->orientation.x; + data->orientation.y = msg->orientation.y; + data->orientation.z = msg->orientation.z; + data->orientation.w = msg->orientation.w; + // GYRO part + data->angular_velocity.x = msg->angular_velocity.x; + data->angular_velocity.y = msg->angular_velocity.y; + data->angular_velocity.z = msg->angular_velocity.z; + // ACCEL part + data->linear_acceleration.x = msg->linear_acceleration.x; + data->linear_acceleration.y = msg->linear_acceleration.y; + data->linear_acceleration.z = msg->linear_acceleration.z; + LOG_DEBUG("RS2_STREAM_MOTION " << motion_frame); + } else { throw io_exception( rsutils::string::from() << "Unsupported stream type " << stream_id.stream_type ); diff --git a/src/media/ros/ros_writer.cpp b/src/media/ros/ros_writer.cpp index beb836715b..1c31701eda 100644 --- a/src/media/ros/ros_writer.cpp +++ b/src/media/ros/ros_writer.cpp @@ -2,6 +2,7 @@ // Copyright(c) 2019 Intel Corporation. All Rights Reserved. #include "proc/decimation-filter.h" +#include "proc/rotation-filter.h" #include "proc/threshold.h" #include "proc/disparity-transform.h" #include "proc/spatial-filter.h" @@ -226,6 +227,23 @@ namespace librealsense imu_msg.angular_velocity.y = data_ptr[1]; imu_msg.angular_velocity.z = data_ptr[2]; } + else if (stream_id.stream_type == RS2_STREAM_MOTION) + { + auto data_imu = *reinterpret_cast(frame.frame->get_frame_data()); + // orientation part + imu_msg.orientation.x = data_imu.orientation.x; + imu_msg.orientation.y = data_imu.orientation.y; + imu_msg.orientation.z = data_imu.orientation.z; + imu_msg.orientation.w = data_imu.orientation.w; + // GYRO part + imu_msg.angular_velocity.x = data_imu.angular_velocity.x; + imu_msg.angular_velocity.y = data_imu.angular_velocity.y; + imu_msg.angular_velocity.z = data_imu.angular_velocity.z; + // ACCEL part + imu_msg.linear_acceleration.x = data_imu.linear_acceleration.x; + imu_msg.linear_acceleration.y = data_imu.linear_acceleration.y; + imu_msg.linear_acceleration.z = data_imu.linear_acceleration.z; + } else { throw io_exception("Unsupported stream type for a motion frame"); @@ -513,6 +531,7 @@ namespace librealsense RETURN_IF_EXTENSION(block, RS2_EXTENSION_HOLE_FILLING_FILTER); RETURN_IF_EXTENSION(block, RS2_EXTENSION_HDR_MERGE); RETURN_IF_EXTENSION(block, RS2_EXTENSION_SEQUENCE_ID_FILTER); + RETURN_IF_EXTENSION(block, RS2_EXTENSION_ROTATION_FILTER); #undef RETURN_IF_EXTENSION diff --git a/src/proc/CMakeLists.txt b/src/proc/CMakeLists.txt index 1d37afe0af..35784d6991 100644 --- a/src/proc/CMakeLists.txt +++ b/src/proc/CMakeLists.txt @@ -20,6 +20,7 @@ target_sources(${LRS_TARGET} "${CMAKE_CURRENT_LIST_DIR}/synthetic-stream.cpp" "${CMAKE_CURRENT_LIST_DIR}/syncer-processing-block.cpp" "${CMAKE_CURRENT_LIST_DIR}/decimation-filter.cpp" + "${CMAKE_CURRENT_LIST_DIR}/rotation-filter.cpp" "${CMAKE_CURRENT_LIST_DIR}/spatial-filter.cpp" "${CMAKE_CURRENT_LIST_DIR}/temporal-filter.cpp" "${CMAKE_CURRENT_LIST_DIR}/hdr-merge.cpp" @@ -49,6 +50,7 @@ target_sources(${LRS_TARGET} "${CMAKE_CURRENT_LIST_DIR}/occlusion-filter.h" "${CMAKE_CURRENT_LIST_DIR}/synthetic-stream.h" "${CMAKE_CURRENT_LIST_DIR}/decimation-filter.h" + "${CMAKE_CURRENT_LIST_DIR}/rotation-filter.h" "${CMAKE_CURRENT_LIST_DIR}/spatial-filter.h" "${CMAKE_CURRENT_LIST_DIR}/temporal-filter.h" "${CMAKE_CURRENT_LIST_DIR}/hdr-merge.h" diff --git a/src/proc/rotation-filter.cpp b/src/proc/rotation-filter.cpp new file mode 100644 index 0000000000..c3654280ca --- /dev/null +++ b/src/proc/rotation-filter.cpp @@ -0,0 +1,225 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#include +#include "option.h" +#include "stream.h" +#include "core/video.h" +#include "proc/rotation-filter.h" + +namespace librealsense { + + const int rotation_min_val = -90; + const int rotation_max_val = 180; + const int rotation_default_val = 0; + const int rotation_step = 90; + + + rotation_filter::rotation_filter() + : stream_filter_processing_block( "Rotation Filter" ) + , _streams_to_rotate() + , _control_val( rotation_default_val ) + , _real_width( 0 ) + , _real_height( 0 ) + , _rotated_width( 0 ) + , _rotated_height( 0 ) + , _value( 0 ) + { + auto rotation_control = std::make_shared< ptr_option< int > >( rotation_min_val, + rotation_max_val, + rotation_step, + rotation_default_val, + &_control_val, + "Rotation angle" ); + + auto weak_rotation_control = std::weak_ptr< ptr_option< int > >( rotation_control ); + rotation_control->on_set( + [this, weak_rotation_control]( float val ) + { + auto strong_rotation_control = weak_rotation_control.lock(); + if( ! strong_rotation_control ) + return; + + std::lock_guard< std::mutex > lock( _mutex ); + + if( ! strong_rotation_control->is_valid( val ) ) + throw invalid_value_exception( rsutils::string::from() + << "Unsupported rotation scale " << val << " is out of range." ); + + _value = val; + } ); + + register_option( RS2_OPTION_ROTATION, rotation_control ); + } + + rs2::frame rotation_filter::process_frame(const rs2::frame_source& source, const rs2::frame& f) + { + if( _value == rotation_default_val ) + return f; + + rs2::stream_profile profile = f.get_profile(); + rs2_stream type = profile.stream_type(); + rs2_extension tgt_type; + if( type == RS2_STREAM_INFRARED ) + { + tgt_type = RS2_EXTENSION_VIDEO_FRAME; + } + else if( f.is< rs2::disparity_frame >() ) + { + tgt_type = RS2_EXTENSION_DISPARITY_FRAME; + } + else if( f.is< rs2::depth_frame >() ) + { + tgt_type = RS2_EXTENSION_DEPTH_FRAME; + } + else + { + return f; + } + + auto src = f.as< rs2::video_frame >(); + _target_stream_profile = profile; + + if( _value == 90 || _value == -90 ) + { + _rotated_width = src.get_height(); + _rotated_height = src.get_width(); + } + else if( _value == 180 ) + { + _rotated_width = src.get_width(); + _rotated_height = src.get_height(); + } + auto bpp = src.get_bytes_per_pixel(); + update_output_profile( f ); + + if (auto tgt = prepare_target_frame(f, source, tgt_type)) + { + switch( bpp ) + { + case 1: { + rotate_frame< 1 >( static_cast< uint8_t * >( const_cast< void * >( tgt.get_data() ) ), + static_cast< const uint8_t * >( src.get_data() ), + src.get_width(), + src.get_height() ); + break; + } + + case 2: { + rotate_frame< 2 >( static_cast< uint8_t * >( const_cast< void * >( tgt.get_data() ) ), + static_cast< const uint8_t * >( src.get_data() ) , + src.get_width(), + src.get_height()); + break; + } + + default: + LOG_ERROR( "Rotation transform does not support format: " + + std::string( rs2_format_to_string( tgt.get_profile().format() ) ) ); + } + return tgt; + } + return f; + } + + void rotation_filter::update_output_profile(const rs2::frame& f) + { + _source_stream_profile = f.get_profile(); + + _target_stream_profile = _source_stream_profile.clone( _source_stream_profile.stream_type(), + _source_stream_profile.stream_index(), + _source_stream_profile.format() ); + + + auto src_vspi = dynamic_cast< video_stream_profile_interface * >( _source_stream_profile.get()->profile ); + if( ! src_vspi ) + throw std::runtime_error( "Stream profile interface is not video stream profile interface" ); + + auto tgt_vspi = dynamic_cast< video_stream_profile_interface * >( _target_stream_profile.get()->profile ); + if( ! tgt_vspi ) + throw std::runtime_error( "Profile is not video stream profile" ); + + rs2_intrinsics src_intrin = src_vspi->get_intrinsics(); + rs2_intrinsics tgt_intrin = tgt_vspi->get_intrinsics(); + + // Adjust width and height based on the rotation angle + if( _value == 90 || _value == -90 ) // 90 or -90 degrees rotation + { + _rotated_width = src_intrin.height; + _rotated_height = src_intrin.width; + tgt_intrin.fx = src_intrin.fy; + tgt_intrin.fy = src_intrin.fx; + tgt_intrin.ppx = src_intrin.ppy; + tgt_intrin.ppy = src_intrin.ppx; + } + else if( _value == 180 ) // 180 degrees rotation + { + _rotated_width = src_intrin.width; + _rotated_height = src_intrin.height; + tgt_intrin = src_intrin; + } + else { throw std::invalid_argument( "Unsupported rotation angle" ); } + + tgt_intrin.width = _rotated_width; + tgt_intrin.height = _rotated_height; + + tgt_vspi->set_intrinsics( [tgt_intrin]() { return tgt_intrin; } ); + tgt_vspi->set_dims( _rotated_width, _rotated_height ); + } + + rs2::frame rotation_filter::prepare_target_frame(const rs2::frame& f, const rs2::frame_source& source, rs2_extension tgt_type) + { + auto vf = f.as(); + auto ret = source.allocate_video_frame(_target_stream_profile, f, + vf.get_bytes_per_pixel(), + _rotated_width, + _rotated_height, + _rotated_width * vf.get_bytes_per_pixel(), + tgt_type); + + return ret; + } + + template< size_t SIZE > + void rotation_filter::rotate_frame( uint8_t * const out, const uint8_t * source, int width, int height ) + { + if( _value != 90 && _value != -90 && _value != 180 ) + { + throw std::invalid_argument( "Invalid rotation angle. Only 90, -90, and 180 degrees are supported." ); + } + + // Define output dimensions + int width_out = ( _value == 90 || _value == -90 ) ? height : width; // rotate by 180 will keep the values as is + int height_out = ( _value == 90 || _value == -90 ) ? width : height; // rotate by 180 will keep the values as is + + // Perform rotation + for( int i = 0; i < height; ++i ) + { + for( int j = 0; j < width; ++j ) + { + // Calculate source index + size_t src_index = ( i * width + j ) * SIZE; + + // Determine output index based on rotation angle + size_t out_index; + if( _value == 90 ) + { + out_index = ( j * height + ( height - i - 1 ) ) * SIZE; + } + else if( _value == -90 ) + { + out_index = ( ( width - j - 1 ) * height + i ) * SIZE; + } + else // 180 degrees + { + out_index = ( ( height - i - 1 ) * width + ( width - j - 1 ) ) * SIZE; + } + + // Copy pixel data from source to destination + std::memcpy( &out[out_index], &source[src_index], SIZE ); + } + } + } + +} + diff --git a/src/proc/rotation-filter.h b/src/proc/rotation-filter.h new file mode 100644 index 0000000000..f5a542e21d --- /dev/null +++ b/src/proc/rotation-filter.h @@ -0,0 +1,40 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#pragma once + +#include +#include +#include "proc/synthetic-stream.h" + +namespace librealsense +{ + + class rotation_filter : public stream_filter_processing_block + { + public: + rotation_filter(); + + protected: + rs2::frame prepare_target_frame(const rs2::frame& f, const rs2::frame_source& source, rs2_extension tgt_type); + + template< size_t SIZE > + void rotate_frame( uint8_t * const out, const uint8_t * source, int width, int height ); + + rs2::frame process_frame(const rs2::frame_source& source, const rs2::frame& f) override; + + private: + void update_output_profile(const rs2::frame& f); + + std::vector< stream_filter > _streams_to_rotate; + int _control_val; + rs2::stream_profile _source_stream_profile; + rs2::stream_profile _target_stream_profile; + uint16_t _real_width; + uint16_t _real_height; + uint16_t _rotated_width; + uint16_t _rotated_height; + float _value; + }; + MAP_EXTENSION( RS2_EXTENSION_ROTATION_FILTER, librealsense::rotation_filter ); + } diff --git a/src/realsense.def b/src/realsense.def index 06f4507611..e1e7a9b79d 100644 --- a/src/realsense.def +++ b/src/realsense.def @@ -211,6 +211,7 @@ EXPORTS rs2_create_threshold rs2_create_units_transform rs2_create_decimation_filter_block + rs2_create_rotation_filter_block rs2_create_temporal_filter_block rs2_create_spatial_filter_block rs2_create_hole_filling_filter_block @@ -438,3 +439,4 @@ EXPORTS rs2_project_color_pixel_to_depth_pixel rs2_get_calibration_config rs2_set_calibration_config + rs2_hw_monitor_get_opcode_string \ No newline at end of file diff --git a/src/rs.cpp b/src/rs.cpp index ba1dd7649c..035380315b 100644 --- a/src/rs.cpp +++ b/src/rs.cpp @@ -30,6 +30,7 @@ #include "proc/disparity-transform.h" #include "proc/syncer-processing-block.h" #include "proc/decimation-filter.h" +#include "proc/rotation-filter.h" #include "proc/spatial-filter.h" #include "proc/hole-filling-filter.h" #include "proc/color-formats-converter.h" @@ -1944,6 +1945,7 @@ int rs2_is_processing_block_extendable_to(const rs2_processing_block* f, rs2_ext switch (extension_type) { case RS2_EXTENSION_DECIMATION_FILTER: return VALIDATE_INTERFACE_NO_THROW((processing_block_interface*)(f->block.get()), librealsense::decimation_filter) != nullptr; + case RS2_EXTENSION_ROTATION_FILTER: return VALIDATE_INTERFACE_NO_THROW((processing_block_interface*)(f->block.get()), librealsense::rotation_filter) != nullptr; case RS2_EXTENSION_THRESHOLD_FILTER: return VALIDATE_INTERFACE_NO_THROW((processing_block_interface*)(f->block.get()), librealsense::threshold) != nullptr; case RS2_EXTENSION_DISPARITY_FILTER: return VALIDATE_INTERFACE_NO_THROW((processing_block_interface*)(f->block.get()), librealsense::disparity_transform) != nullptr; case RS2_EXTENSION_SPATIAL_FILTER: return VALIDATE_INTERFACE_NO_THROW((processing_block_interface*)(f->block.get()), librealsense::spatial_filter) != nullptr; @@ -2779,6 +2781,14 @@ rs2_processing_block* rs2_create_decimation_filter_block(rs2_error** error) BEGI } NOARGS_HANDLE_EXCEPTIONS_AND_RETURN(nullptr) +rs2_processing_block * rs2_create_rotation_filter_block( rs2_error ** error ) BEGIN_API_CALL +{ + auto block = std::make_shared< librealsense::rotation_filter >(); + + return new rs2_processing_block{ block }; +} +NOARGS_HANDLE_EXCEPTIONS_AND_RETURN( nullptr ) + rs2_processing_block* rs2_create_temporal_filter_block(rs2_error** error) BEGIN_API_CALL { auto block = std::make_shared(); @@ -4013,7 +4023,9 @@ rs2_raw_data_buffer* rs2_terminal_parse_response(rs2_terminal_parser* terminal_p VALIDATE_NOT_NULL(command); VALIDATE_NOT_NULL(response); VALIDATE_LE(size_of_command, 1000); //bufer shall be less than 1000 bytes or similar - VALIDATE_LE(size_of_response, 5000);//bufer shall be less than 5000 bytes or similar + + // some commands may return a longer length as a response + //VALIDATE_LE(size_of_response, 5000);//bufer shall be less than 5000 bytes or similar std::string command_string; @@ -4452,3 +4464,13 @@ void rs2_set_calibration_config( auto_calib->set_calibration_config(calibration_config_json_str); } HANDLE_EXCEPTIONS_AND_RETURN(, device, calibration_config_json_str) + +void rs2_hw_monitor_get_opcode_string(int opcode, char* buffer, size_t buffer_size, + rs2_device* device, + rs2_error** error) BEGIN_API_CALL +{ + VALIDATE_NOT_NULL(device); + auto device_interface = VALIDATE_INTERFACE(device->device, librealsense::debug_interface); + strncpy(buffer, device_interface->get_opcode_string(opcode).c_str(), buffer_size); +} +HANDLE_EXCEPTIONS_AND_RETURN(, device) diff --git a/src/rscore-pp-block-factory.cpp b/src/rscore-pp-block-factory.cpp index 679bcf63e0..d09fd7978b 100644 --- a/src/rscore-pp-block-factory.cpp +++ b/src/rscore-pp-block-factory.cpp @@ -4,6 +4,7 @@ #include "rscore-pp-block-factory.h" #include "proc/decimation-filter.h" +#include "proc/rotation-filter.h" #include "proc/disparity-transform.h" #include "proc/hdr-merge.h" #include "proc/hole-filling-filter.h" @@ -27,6 +28,8 @@ rscore_pp_block_factory::create_pp_block( std::string const & name, rsutils::jso if( rsutils::string::nocase_equal( name, "Decimation Filter" ) ) return std::make_shared< decimation_filter >(); + if( rsutils::string::nocase_equal( name, "Rotation Filter" ) ) + return std::make_shared< rotation_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 diff --git a/src/sensor.cpp b/src/sensor.cpp index f051cb2af4..686a4dead6 100644 --- a/src/sensor.cpp +++ b/src/sensor.cpp @@ -10,6 +10,7 @@ #include "image.h" #include "proc/synthetic-stream.h" #include "proc/decimation-filter.h" +#include "proc/rotation-filter.h" #include "global_timestamp_reader.h" #include "device-calibration.h" #include "core/notification.h" @@ -262,6 +263,7 @@ void log_callback_end( uint32_t fps, dec->get_option(RS2_OPTION_STREAM_FORMAT_FILTER).set(RS2_FORMAT_Z16); res.push_back(dec); } + res.push_back( std::make_shared< rotation_filter >( )); return res; } diff --git a/src/terminal-parser.cpp b/src/terminal-parser.cpp index 63a55ec7ce..942a937ca3 100644 --- a/src/terminal-parser.cpp +++ b/src/terminal-parser.cpp @@ -61,10 +61,13 @@ namespace librealsense data_vec.insert(data_vec.begin(), data.begin(), data.end()); return data_vec; } - else + else if (response.size() == 4) // if the response contains only the opcode, there's no response other than the opcode, return success message { - return response; + unsigned opcode = response[0]; + auto str = (rsutils::string::from() << "Command succeeded, returned 0x" << hex << opcode).str(); + return std::vector(str.begin(), str.end()); } + return response; } vector terminal_parser::build_raw_command_data(const command_from_xml& command, const vector& params) const diff --git a/src/to-string.cpp b/src/to-string.cpp index 16f1dad3da..7bc1d95bf4 100644 --- a/src/to-string.cpp +++ b/src/to-string.cpp @@ -313,6 +313,7 @@ const char * get_string( rs2_extension value ) CASE( MAX_USABLE_RANGE_SENSOR ) CASE( DEBUG_STREAM_SENSOR ) CASE( CALIBRATION_CHANGE_DEVICE ) + CASE( ROTATION_FILTER ) default: assert( ! is_valid( value ) ); return UNKNOWN_VALUE; @@ -459,6 +460,7 @@ std::string const & get_string_( rs2_option value ) CASE( OHM_TEMPERATURE ) CASE( SOC_PVT_TEMPERATURE ) CASE( GYRO_SENSITIVITY ) + CASE( ROTATION ) arr[RS2_OPTION_REGION_OF_INTEREST] = "Region of Interest"; #undef CASE return arr; diff --git a/tools/enumerate-devices/rs-enumerate-devices.cpp b/tools/enumerate-devices/rs-enumerate-devices.cpp index ffcd152983..54dd737f24 100644 --- a/tools/enumerate-devices/rs-enumerate-devices.cpp +++ b/tools/enumerate-devices/rs-enumerate-devices.cpp @@ -393,7 +393,7 @@ int main(int argc, char** argv) try if( !device_count ) { cout << "No device detected. Is it plugged in?\n"; - return EXIT_SUCCESS; + return EXIT_FAILURE; } if (short_view || compact_view) diff --git a/unit-tests/post-processing/test-rotation-filter.py b/unit-tests/post-processing/test-rotation-filter.py new file mode 100644 index 0000000000..8b515c7f73 --- /dev/null +++ b/unit-tests/post-processing/test-rotation-filter.py @@ -0,0 +1,131 @@ +# License: Apache 2.0. See LICENSE file in root directory. +# Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#temporary fix to prevent the test from running on Win_SH_Py_DDS_CI +#test:donotrun:dds + +from rspy import test, repo +import pyrealsense2 as rs +import numpy as np + +# Parameters for frame creation and depth intrinsics +input_res_x = 640 +input_res_y = 480 +focal_length = 600 +depth_units = 0.001 +stereo_baseline_mm = 50 +frames = 5 # Number of frames to process + + +# Function to create depth intrinsics directly +def create_depth_intrinsics(): + depth_intrinsics = rs.intrinsics() + depth_intrinsics.width = input_res_x + depth_intrinsics.height = input_res_y + depth_intrinsics.ppx = input_res_x / 2.0 + depth_intrinsics.ppy = input_res_y / 2.0 + depth_intrinsics.fx = focal_length + depth_intrinsics.fy = focal_length + depth_intrinsics.model = rs.distortion.brown_conrady + depth_intrinsics.coeffs = [0, 0, 0, 0, 0] + return depth_intrinsics + + +# Function to create a video stream with specified parameters +def create_video_stream(depth_intrinsics): + vs = rs.video_stream() + vs.type = rs.stream.depth + vs.index = 0 + vs.uid = 0 + vs.width = input_res_x + vs.height = input_res_y + vs.fps = 30 + vs.bpp = 2 + vs.fmt = rs.format.z16 + vs.intrinsics = depth_intrinsics + return vs + + +# Function to create a synthetic frame +def create_frame(depth_stream_profile, index): + frame = rs.software_video_frame() + data = np.arange(input_res_x * input_res_y, dtype=np.uint16).reshape((input_res_y, input_res_x)) + frame.pixels = data.tobytes() + frame.bpp = 2 + frame.stride = input_res_x * 2 + frame.timestamp = index * 33 + frame.domain = rs.timestamp_domain.system_time + frame.frame_number = index + frame.profile = depth_stream_profile.as_video_stream_profile() + frame.depth_units = depth_units + return frame + + +# Function to validate rotated results based on the angle +def validate_rotation_results(filtered_frame, angle): + rotated_height = filtered_frame.profile.as_video_stream_profile().height() + rotated_width = filtered_frame.profile.as_video_stream_profile().width() + + # Reshape the rotated data according to its actual dimensions + rotated_data = np.frombuffer(filtered_frame.get_data(), dtype=np.uint16).reshape((rotated_height, rotated_width)) + + # Original data for comparison + original_data = np.arange(input_res_x * input_res_y, dtype=np.uint16).reshape((input_res_y, input_res_x)) + + # Determine the expected rotated result based on the angle + if angle == 90: + expected_data = np.rot90(original_data, k=-1) # Clockwise + elif angle == 180: + expected_data = np.rot90(original_data, k=2) # 180 degrees + elif angle == -90: + expected_data = np.rot90(original_data, k=1) # Counterclockwise + + # Convert numpy arrays to lists before comparison + rotated_list = rotated_data.flatten().tolist() + expected_list = expected_data.flatten().tolist() + + # Compare the flattened lists + test.check_equal_lists(rotated_list, expected_list) + + +################################################################################################ +with test.closure("Test rotation filter"): + # Set up software device and depth sensor + sw_dev = rs.software_device() + depth_sensor = sw_dev.add_sensor("Depth") + + # Initialize intrinsics and video stream profile + depth_intrinsics = create_depth_intrinsics() + vs = create_video_stream(depth_intrinsics) + depth_stream_profile = depth_sensor.add_video_stream(vs) + + frame_queue = rs.frame_queue(15) + + # Define rotation angles to test + rotation_angles = [90, 180, -90] + for angle in rotation_angles: + rotation_filter = rs.rotation_filter() + rotation_filter.set_option(rs.option.rotation, angle) + + # Start depth sensor + depth_sensor.open(depth_stream_profile) + depth_sensor.start(frame_queue) + + for i in range(frames): + # Create and process each frame + frame = create_frame(depth_stream_profile, i) + depth_sensor.on_video_frame(frame) + + # Wait for frames and apply rotation filter + depth_frame = frame_queue.wait_for_frame() + filtered_depth = rotation_filter.process(depth_frame) + + # Validate rotated frame results + validate_rotation_results(filtered_depth, angle) + + # Stop and close the sensor after each angle test + depth_sensor.stop() + depth_sensor.close() + +################################################################################################ +test.print_results_and_exit() \ No newline at end of file diff --git a/wrappers/python/pyrs_processing.cpp b/wrappers/python/pyrs_processing.cpp index 29dd9db7b4..5dd80f2f0a 100644 --- a/wrappers/python/pyrs_processing.cpp +++ b/wrappers/python/pyrs_processing.cpp @@ -65,6 +65,7 @@ void init_processing(py::module &m) { return new rs2::filter(filter_function, queue_size); }), "filter_function"_a, "queue_size"_a = 1) .def(BIND_DOWNCAST(filter, decimation_filter)) + .def( BIND_DOWNCAST( filter, rotation_filter ) ) .def(BIND_DOWNCAST(filter, disparity_transform)) .def(BIND_DOWNCAST(filter, hole_filling_filter)) .def(BIND_DOWNCAST(filter, spatial_filter)) @@ -157,6 +158,9 @@ void init_processing(py::module &m) { decimation_filter.def(py::init<>()) .def(py::init(), "magnitude"_a); + py::class_< rs2::rotation_filter, rs2::filter > rotation_filter(m, "rotation_filter","Performs rotation of frames." ); + rotation_filter.def( py::init<>() ).def( py::init< float >(), "value"_a ); + py::class_ temporal_filter(m, "temporal_filter", "Temporal filter smooths the image by calculating multiple frames " "with alpha and delta settings. Alpha defines the weight of current frame, and delta defines the" "threshold for edge classification and preserving.");