From ec5c7fcb9f30a78f77e7c0e05924e55bbd4ab7fc Mon Sep 17 00:00:00 2001 From: Remi Bettan Date: Tue, 3 Sep 2024 16:49:46 +0300 Subject: [PATCH 01/75] metadata field used for API RS2_FRAME_METADATA_SENSOR_TIMESTAMP for mipi updated as in usb --- src/ds/d400/d400-device.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/ds/d400/d400-device.cpp b/src/ds/d400/d400-device.cpp index 5dfc8232fc..62cc070760 100644 --- a/src/ds/d400/d400-device.cpp +++ b/src/ds/d400/d400-device.cpp @@ -1083,14 +1083,10 @@ namespace librealsense auto md_prop_offset = offsetof(metadata_mipi_depth_raw, depth_mode); depth_sensor.register_metadata(RS2_FRAME_METADATA_SENSOR_TIMESTAMP, - make_attribute_parser(&md_mipi_depth_mode::hw_timestamp, - md_mipi_depth_control_attributes::hw_timestamp_attribute, - md_prop_offset)); - - depth_sensor.register_metadata(RS2_FRAME_METADATA_SENSOR_TIMESTAMP, - make_attribute_parser(&md_mipi_depth_mode::optical_timestamp, - md_mipi_depth_control_attributes::optical_timestamp_attribute, - md_prop_offset)); + make_rs400_sensor_ts_parser(make_uvc_header_parser(&platform::uvc_header::timestamp), + make_attribute_parser(&md_mipi_depth_mode::optical_timestamp, + md_mipi_depth_control_attributes::optical_timestamp_attribute, + md_prop_offset))); depth_sensor.register_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE, make_attribute_parser(&md_mipi_depth_mode::exposure_time, From 9e9530ee517891407cb6de1fa28c80f36888d3f7 Mon Sep 17 00:00:00 2001 From: Remi Bettan Date: Tue, 3 Sep 2024 16:51:57 +0300 Subject: [PATCH 02/75] comment added --- src/ds/d400/d400-device.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ds/d400/d400-device.cpp b/src/ds/d400/d400-device.cpp index 62cc070760..68980de97c 100644 --- a/src/ds/d400/d400-device.cpp +++ b/src/ds/d400/d400-device.cpp @@ -1082,6 +1082,7 @@ namespace librealsense // attributes of md_mipi_depth_control structure auto md_prop_offset = offsetof(metadata_mipi_depth_raw, depth_mode); + // optical_timestamp contains value of exposure/2 depth_sensor.register_metadata(RS2_FRAME_METADATA_SENSOR_TIMESTAMP, make_rs400_sensor_ts_parser(make_uvc_header_parser(&platform::uvc_header::timestamp), make_attribute_parser(&md_mipi_depth_mode::optical_timestamp, From 6a60c6ac8b73d24d4b1387c9cff3a58ea12d4ce5 Mon Sep 17 00:00:00 2001 From: ohadmeir Date: Tue, 3 Sep 2024 11:15:22 +0300 Subject: [PATCH 03/75] Add occ and tare to D555 menu. Implement GT calculation for D500 --- common/d500-on-chip-calib.cpp | 2 +- common/d500-on-chip-calib.h | 2 +- common/device-model.cpp | 196 ++++++++++++++++---------- common/on-chip-calib.cpp | 2 +- common/on-chip-calib.h | 2 +- src/ds/d500/d500-auto-calibration.cpp | 35 ++++- src/ds/ds-calib-common.cpp | 15 +- 7 files changed, 167 insertions(+), 87 deletions(-) diff --git a/common/d500-on-chip-calib.cpp b/common/d500-on-chip-calib.cpp index 9fe4994951..5f36a508c6 100644 --- a/common/d500-on-chip-calib.cpp +++ b/common/d500-on-chip-calib.cpp @@ -111,7 +111,7 @@ namespace rs2 } d500_autocalib_notification_model::d500_autocalib_notification_model(std::string name, - std::shared_ptr manager, bool exp) + std::shared_ptr manager, bool exp) : process_notification_model(manager) { enable_expand = false; diff --git a/common/d500-on-chip-calib.h b/common/d500-on-chip-calib.h index ca97644a76..041a6cc2f7 100644 --- a/common/d500-on-chip-calib.h +++ b/common/d500-on-chip-calib.h @@ -85,7 +85,7 @@ namespace rs2 RS2_CALIB_STATE_ABORT_CALLED }; - d500_autocalib_notification_model(std::string name, std::shared_ptr manager, bool expaned); + d500_autocalib_notification_model(std::string name, std::shared_ptr manager, bool expaned); d500_on_chip_calib_manager& get_manager() { return *std::dynamic_pointer_cast(update_manager); } void draw_content(ux_window& win, int x, int y, float t, std::string& error_message) override; diff --git a/common/device-model.cpp b/common/device-model.cpp index 2ec5e9ec21..5818df1636 100644 --- a/common/device-model.cpp +++ b/common/device-model.cpp @@ -3349,21 +3349,40 @@ namespace rs2 { if (sub->supports_on_chip_calib() && !has_autocalib) { + bool is_d555 = false; + + if( dev.supports( RS2_CAMERA_INFO_PRODUCT_ID ) ) + { + auto pid_str = std::string( dev.get_info( RS2_CAMERA_INFO_PRODUCT_ID ) ); + if( pid_str == "0B56" || pid_str == "DDS" ) + is_d555 = true; + } + if (ImGui::Selectable("On-Chip Calibration", false, avoid_selection_flag)) { try { - auto manager = std::make_shared(viewer, sub, *this, dev); - auto n = std::make_shared("", manager, false); - viewer.not_model->add_notification(n); + std::shared_ptr< process_manager > manager; + std::shared_ptr< process_notification_model > n; + if( is_d555 ) + { + manager = std::make_shared< on_chip_calib_manager >( viewer, sub, *this, dev ); + n = std::make_shared< autocalib_notification_model >( "", manager, false ); + } + else + { + manager = std::make_shared< d500_on_chip_calib_manager >( viewer, sub, *this, dev ); + n = std::make_shared< d500_autocalib_notification_model >( "", manager, false ); + } + viewer.not_model->add_notification( n ); n->forced = true; n->update_state = d500_autocalib_notification_model::RS2_CALIB_STATE_INIT_CALIB; - for (auto&& n : related_notifications) - if (dynamic_cast(n.get())) - n->dismiss(false); + for( auto && n : related_notifications ) + if( dynamic_cast< d500_autocalib_notification_model * >( n.get() ) ) + n->dismiss( false ); - related_notifications.push_back(n); + related_notifications.push_back( n ); } catch (const error& e) { @@ -3376,93 +3395,126 @@ namespace rs2 } if (ImGui::IsItemHovered()) { - std::string tooltip = rsutils::string::from() - << "On-Chip Calibration" - << (streaming ? " (Disabled while streaming)" : ""); + std::string tooltip; + if( is_d555 ) + tooltip = "This will improve the depth noise.\n" + "Point at a scene that normally would have > 50 %% valid depth pixels,\n" + "then press calibrate." + "The health-check will be calculated.\n" + "If >0.25 we recommend applying the new calibration.\n" + "\"White wall\" mode should only be used when pointing at a flat white wall " + "with projector on"; + else + tooltip = rsutils::string::from() << "On-Chip Calibration" << (streaming ? " (Disabled while streaming)" : ""); ImGui::SetTooltip("%s", tooltip.c_str()); } - - if (ImGui::Selectable("Dry Run On-Chip Calibration", false, avoid_selection_flag)) + + if( !is_d555 ) { - try + if( ImGui::Selectable( "Dry Run On-Chip Calibration", false, avoid_selection_flag ) ) { - auto manager = std::make_shared(viewer, sub, *this, dev); - auto n = std::make_shared("", manager, false); - viewer.not_model->add_notification(n); - n->forced = true; - n->update_state = d500_autocalib_notification_model::RS2_CALIB_STATE_INIT_DRY_RUN; + try + { + auto manager = std::make_shared(viewer, sub, *this, dev); + auto n = std::make_shared("", manager, false); + viewer.not_model->add_notification(n); + n->forced = true; + n->update_state = d500_autocalib_notification_model::RS2_CALIB_STATE_INIT_DRY_RUN; - for (auto&& n : related_notifications) - if (dynamic_cast(n.get())) - n->dismiss(false); + for (auto&& n : related_notifications) + if (dynamic_cast(n.get())) + n->dismiss(false); - related_notifications.push_back(n); - } - catch (const error& e) - { - error_message = error_to_string(e); + related_notifications.push_back(n); + } + catch (const error& e) + { + error_message = error_to_string(e); + } + catch (const std::exception& e) + { + error_message = e.what(); + } } - catch (const std::exception& e) + if (ImGui::IsItemHovered()) { - error_message = e.what(); + std::string tooltip = rsutils::string::from() + << "Dry Run On-Chip Calibration" + << ( streaming ? " (Disabled while streaming)" : "" ); + ImGui::SetTooltip("%s", tooltip.c_str()); } } - if (ImGui::IsItemHovered()) - { - std::string tooltip = rsutils::string::from() - << "Dry Run On-Chip Calibration" - << (streaming ? " (Disabled while streaming)" : ""); - ImGui::SetTooltip("%s", tooltip.c_str()); - } - - bool is_d555 = false; - - if (dev.supports(RS2_CAMERA_INFO_PRODUCT_ID)) - { - auto pid_str = std::string(dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID)); - if (pid_str == "0B56" || pid_str == "DDS") - is_d555 = true; - } - - if( is_d555 && ImGui::Selectable( "Focal Length Calibration" ) ) + else { - try + if( ImGui::Selectable( "Focal Length Calibration" ) ) { - std::shared_ptr< subdevice_model > sub_color; - for( auto && sub2 : subdevices ) + try { - if( sub2->s->is< rs2::color_sensor >() ) + std::shared_ptr< subdevice_model > sub_color; + for( auto && sub2 : subdevices ) { - sub_color = sub2; - break; + if( sub2->s->is< rs2::color_sensor >() ) + { + sub_color = sub2; + break; + } } - } - auto manager = std::make_shared< on_chip_calib_manager >( viewer, sub, *this, dev, sub_color ); - auto n = std::make_shared< autocalib_notification_model >( "", manager, false ); - viewer.not_model->add_notification( n ); - n->forced = true; - n->update_state = autocalib_notification_model::RS2_CALIB_STATE_FL_INPUT; + auto manager = std::make_shared< on_chip_calib_manager >( viewer, sub, *this, dev, sub_color ); + auto n = std::make_shared< autocalib_notification_model >( "", manager, false ); + viewer.not_model->add_notification( n ); + n->forced = true; + n->update_state = autocalib_notification_model::RS2_CALIB_STATE_FL_INPUT; - for( auto && n : related_notifications ) - if( dynamic_cast< autocalib_notification_model * >( n.get() ) ) - n->dismiss( false ); + for( auto && n : related_notifications ) + if( dynamic_cast< autocalib_notification_model * >( n.get() ) ) + n->dismiss( false ); - related_notifications.push_back( n ); - manager->start_fl_viewer(); - } - catch( const error & e ) - { - error_message = error_to_string( e ); + related_notifications.push_back( n ); + manager->start_fl_viewer(); + } + catch( const error & e ) + { + error_message = error_to_string( e ); + } + catch( const std::exception & e ) + { + error_message = e.what(); + } } - catch( const std::exception & e ) + if( ImGui::IsItemHovered() ) + ImGui::SetTooltip( "Focal length calibration is used to adjust camera focal length with specific target." ); + + if( ImGui::Selectable( "Tare Calibration" ) ) { - error_message = e.what(); + try + { + auto manager = std::make_shared< on_chip_calib_manager >( viewer, sub, *this, dev ); + auto n = std::make_shared< autocalib_notification_model >( "", manager, false ); + viewer.not_model->add_notification( n ); + n->forced = true; + n->update_state = autocalib_notification_model::RS2_CALIB_STATE_TARE_INPUT; + + for( auto && n : related_notifications ) + if( dynamic_cast< autocalib_notification_model * >( n.get() ) ) + n->dismiss( false ); + + related_notifications.push_back( n ); + } + catch( const error & e ) + { + error_message = error_to_string( e ); + } + catch( const std::exception & e ) + { + error_message = e.what(); + } } + if( ImGui::IsItemHovered() ) + ImGui::SetTooltip( "Tare calibration is used to adjust camera absolute distance to flat target.\n" + "User needs either to enter the known ground truth or use the get button\n" + "with specific target to get the ground truth." ); } - if( ImGui::IsItemHovered() ) - ImGui::SetTooltip( - "Focal length calibration is used to adjust camera focal length with specific target." ); has_autocalib = true; } diff --git a/common/on-chip-calib.cpp b/common/on-chip-calib.cpp index c0c7d602c2..5693a73f86 100644 --- a/common/on-chip-calib.cpp +++ b/common/on-chip-calib.cpp @@ -3130,7 +3130,7 @@ namespace rs2 } } - autocalib_notification_model::autocalib_notification_model(std::string name, std::shared_ptr manager, bool exp) + autocalib_notification_model::autocalib_notification_model(std::string name, std::shared_ptr manager, bool exp) : process_notification_model(manager) { enable_expand = false; diff --git a/common/on-chip-calib.h b/common/on-chip-calib.h index 1bddb10a47..2b9af69386 100644 --- a/common/on-chip-calib.h +++ b/common/on-chip-calib.h @@ -199,7 +199,7 @@ namespace rs2 RS2_CALIB_STATE_FL_PLUS_INPUT, // Collect input parameters for focal length plus calib }; - autocalib_notification_model(std::string name, std::shared_ptr manager, bool expaned); + autocalib_notification_model(std::string name, std::shared_ptr manager, bool expaned); on_chip_calib_manager& get_manager() { return *std::dynamic_pointer_cast(update_manager); } diff --git a/src/ds/d500/d500-auto-calibration.cpp b/src/ds/d500/d500-auto-calibration.cpp index 87511b9ed8..f998c9fe5c 100644 --- a/src/ds/d500/d500-auto-calibration.cpp +++ b/src/ds/d500/d500-auto-calibration.cpp @@ -309,10 +309,39 @@ namespace librealsense throw not_implemented_exception(rsutils::string::from() << "UV Map Calibration not applicable for this device"); } - float d500_auto_calibrated::calculate_target_z(rs2_frame_queue* queue1, rs2_frame_queue* queue2, rs2_frame_queue* queue3, - float target_w, float target_h, rs2_update_progress_callback_sptr progress_callback) + float d500_auto_calibrated::calculate_target_z( rs2_frame_queue* queue1, rs2_frame_queue* queue2, rs2_frame_queue* queue3, + float target_w, float target_h, + rs2_update_progress_callback_sptr progress_callback ) { - throw not_implemented_exception(rsutils::string::from() << "Calculate T not applicable for this device"); + constexpr size_t min_frames_required = 10; + + rs2_error * e = nullptr; + int queue_size = rs2_frame_queue_size( queue1, &e ); + if( queue_size < min_frames_required ) + throw std::runtime_error( rsutils::string::from() << "Target distance calculation requires at least " + << min_frames_required << " frames, aborting" ); + + float target_fw = 0; + float target_fh = 0; + std::array< float, 4 > rect_sides{}; + ds_calib_common::get_target_rect_info( queue1, rect_sides.data(), target_fw, target_fh, 50, progress_callback ); // Report 50% progress + + float gt[4] = { 0 }; + gt[0] = target_fw * target_w / rect_sides[0]; + gt[1] = target_fw * target_w / rect_sides[1]; + gt[2] = target_fh * target_h / rect_sides[2]; + gt[3] = target_fh * target_h / rect_sides[3]; + + if( gt[0] <= 0.1f || gt[1] <= 0.1f || gt[2] <= 0.1f || gt[3] <= 0.1f ) + throw std::runtime_error( "Target distance calculation failed" ); + + // Target's plane Z value is the average of the four calculated corners + float target_z_value = 0.f; + for( int i = 0; i < 4; ++i ) + target_z_value += gt[i]; + target_z_value /= 4.f; + + return target_z_value; } std::string d500_auto_calibrated::get_calibration_config() const diff --git a/src/ds/ds-calib-common.cpp b/src/ds/ds-calib-common.cpp index af963650f1..39141c4441 100644 --- a/src/ds/ds-calib-common.cpp +++ b/src/ds/ds-calib-common.cpp @@ -64,29 +64,28 @@ namespace librealsense int progress, rs2_update_progress_callback_sptr progress_callback ) { - fx = -1.0f; - std::vector< std::array< float, 4 > > rect_sides_arr; - rs2_error * e = nullptr; - rs2_frame * f = nullptr; - int queue_size = rs2_frame_queue_size( frames, &e ); if( queue_size == 0 ) throw std::runtime_error( "Extract target rectangle info - no frames in input queue!" ); - int fc = 0; - while( ( fc++ < queue_size ) && rs2_poll_for_frame( frames, &f, &e ) ) + int frame_counter = 0; + bool first_time = true; + rs2_frame * f = nullptr; + std::vector< std::array< float, 4 > > rect_sides_arr; + while( ( frame_counter++ < queue_size ) && rs2_poll_for_frame( frames, &f, &e ) ) { rs2::frame ff( f ); if( ff.get_data() ) { - if( fx < 0.0f ) + if( first_time ) { auto p = ff.get_profile(); auto vsp = p.as< rs2::video_stream_profile >(); rs2_intrinsics intrin = vsp.get_intrinsics(); fx = intrin.fx; fy = intrin.fy; + first_time = false; } std::array< float, 4 > rec_sides_cur{}; From 3941d0860c6364cde9cbe3856b970ba2b1e8c2f4 Mon Sep 17 00:00:00 2001 From: ohadmeir Date: Sun, 8 Sep 2024 11:44:33 +0300 Subject: [PATCH 04/75] Move OCC functions to common, implement OCC for D555. Some naming convention changes in D400 --- src/dds/rs-dds-device-proxy.cpp | 4 +- src/ds/d400/d400-auto-calibration.cpp | 351 ++++++++------------------ src/ds/d400/d400-auto-calibration.h | 23 +- src/ds/d500/d500-auto-calibration.cpp | 195 ++++++++++++-- src/ds/d500/d500-auto-calibration.h | 15 +- src/ds/d500/d500-device.cpp | 2 +- src/ds/ds-calib-common.cpp | 99 ++++++++ src/ds/ds-calib-common.h | 104 ++++++++ 8 files changed, 501 insertions(+), 292 deletions(-) diff --git a/src/dds/rs-dds-device-proxy.cpp b/src/dds/rs-dds-device-proxy.cpp index 1ad4bb7bac..d52007eb9e 100644 --- a/src/dds/rs-dds-device-proxy.cpp +++ b/src/dds/rs-dds-device-proxy.cpp @@ -392,8 +392,8 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const & if (supports_info(RS2_CAMERA_INFO_PRODUCT_LINE) && !strcmp(get_info(RS2_CAMERA_INFO_PRODUCT_LINE).c_str(), "D500")) - set_auto_calibration_capability(std::make_shared( - std::make_shared(this))); + set_auto_calibration_capability( std::make_shared< d500_auto_calibrated >( + std::make_shared< d500_debug_protocol_calibration_engine >( this ), this ) ); _calibration_changed_subscription = _dds_dev->on_calibration_changed( [this]( std::shared_ptr< const realdds::dds_stream > const & stream ) diff --git a/src/ds/d400/d400-auto-calibration.cpp b/src/ds/d400/d400-auto-calibration.cpp index cbb6515dc0..9f0261f72e 100644 --- a/src/ds/d400/d400-auto-calibration.cpp +++ b/src/ds/d400/d400-auto-calibration.cpp @@ -9,7 +9,6 @@ #include "librealsense2/rsutil.h" #include "algo.h" #include -#include #include #include @@ -61,57 +60,15 @@ namespace librealsense uint16_t tableSize; // 512 bytes }; - struct DscResultParams - { - uint16_t m_status; - float m_healthCheck; - }; - struct DscResultBuffer { - uint16_t m_paramSize; - DscResultParams m_dscResultParams; - uint16_t m_tableSize; - }; - - enum rs2_dsc_status : uint16_t - { - RS2_DSC_STATUS_SUCCESS = 0, /**< Self calibration succeeded*/ - RS2_DSC_STATUS_RESULT_NOT_READY = 1, /**< Self calibration result is not ready yet*/ - RS2_DSC_STATUS_FILL_FACTOR_TOO_LOW = 2, /**< There are too little textures in the scene*/ - RS2_DSC_STATUS_EDGE_TOO_CLOSE = 3, /**< Self calibration range is too small*/ - RS2_DSC_STATUS_NOT_CONVERGE = 4, /**< For tare calibration only*/ - RS2_DSC_STATUS_BURN_SUCCESS = 5, - RS2_DSC_STATUS_BURN_ERROR = 6, - RS2_DSC_STATUS_NO_DEPTH_AVERAGE = 7 + uint16_t paramSize; + uint16_t status; + float healthCheck; + uint16_t tableSize; }; - #pragma pack(pop) - enum auto_calib_sub_cmd : uint8_t - { - py_rx_calib_check_status = 0x03, - interactive_scan_control = 0x05, - py_rx_calib_begin = 0x08, - tare_calib_begin = 0x0b, - tare_calib_check_status = 0x0c, - get_calibration_result = 0x0d, - focal_length_calib_begin = 0x11, - get_focal_legth_calib_result = 0x12, - py_rx_plus_fl_calib_begin = 0x13, - get_py_rx_plus_fl_calib_result = 0x14, - set_coefficients = 0x19 - }; - - enum auto_calib_speed : uint8_t - { - speed_very_fast = 0, - speed_fast = 1, - speed_medium = 2, - speed_slow = 3, - speed_white_wall = 4 - }; - enum class host_assistance_type { no_assistance = 0, @@ -128,18 +85,6 @@ namespace librealsense low = 3 //(0.2%) }; - enum data_sampling - { - polling = 0, - interrupt = 1 - }; - - enum scan_parameter - { - py_scan = 0, - rx_scan = 1 - }; - struct tare_params3 { uint8_t average_step_count; @@ -172,17 +117,17 @@ namespace librealsense const int DEFAULT_AVERAGE_STEP_COUNT = 20; const int DEFAULT_STEP_COUNT = 20; const int DEFAULT_ACCURACY = subpixel_accuracy::medium; - const auto_calib_speed DEFAULT_SPEED = auto_calib_speed::speed_slow; - const int DEFAULT_SCAN = scan_parameter::py_scan; - const int DEFAULT_SAMPLING = data_sampling::interrupt; + const ds_calib_common::auto_calib_speed DEFAULT_SPEED = ds_calib_common::SPEED_SLOW; + const int DEFAULT_SCAN = ds_calib_common::PY_SCAN; + const int DEFAULT_SAMPLING = ds_calib_common::INTERRUPT; const int DEFAULT_FL_STEP_COUNT = 100; const int DEFAULT_FY_SCAN_RANGE = 40; const int DEFAULT_KEEP_NEW_VALUE_AFTER_SUCESSFUL_SCAN = 1; - const int DEFAULT_FL_SAMPLING = data_sampling::interrupt; + const int DEFAULT_FL_SAMPLING = ds_calib_common::INTERRUPT; const int DEFAULT_ADJUST_BOTH_SIDES = 0; - const int DEFAULT_TARE_SAMPLING = data_sampling::polling; + const int DEFAULT_TARE_SAMPLING = ds_calib_common::POLLING; const int DEFAULT_OCC_FL_SCAN_LOCATION = 0; const int DEFAULT_FY_SCAN_DIRECTION = 0; @@ -202,82 +147,10 @@ namespace librealsense _skipped_frames(0) {} - std::map auto_calibrated::parse_json(std::string json_content) + ds_calib_common::dsc_check_status_result + auto_calibrated::get_calibration_status( int timeout_ms, std::function< void( const int count ) > progress_func, bool wait_for_final_results ) { - auto j = rsutils::json::parse(json_content); - - std::map values; - - for (auto it = j.begin(); it != j.end(); ++it) - { - values[it.key()] = it.value(); - } - - return values; - } - - void try_fetch(std::map jsn, std::string key, int* value) - { - std::replace(key.begin(), key.end(), '_', ' '); // Treat _ as space - if (jsn.find(key) != jsn.end()) - { - *value = jsn[key]; - } - } - - // RAII to handle auto-calibration with the thermal compensation disabled - class thermal_compensation_guard - { - public: - thermal_compensation_guard(auto_calibrated_interface* handle) : - restart_tl(false), snr(nullptr) - { - if (Is(handle)) - { - try - { - // The depth sensor is assigned first by design - snr = &(As(handle)->get_sensor(0)); - - if (snr->supports_option(RS2_OPTION_THERMAL_COMPENSATION)) - restart_tl = static_cast(snr->get_option(RS2_OPTION_THERMAL_COMPENSATION).query() != 0); - if (restart_tl) - { - snr->get_option(RS2_OPTION_THERMAL_COMPENSATION).set(0.f); - // Allow for FW changes to propagate - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - } - catch(...) { - LOG_WARNING("Thermal Compensation guard failed to invoke"); - } - } - } - virtual ~thermal_compensation_guard() - { - try - { - if (snr && restart_tl) - snr->get_option(RS2_OPTION_THERMAL_COMPENSATION).set(1.f); - } - catch (...) { - LOG_WARNING("Thermal Compensation guard failed to complete"); - } - } - - protected: - bool restart_tl; - librealsense::sensor_interface* snr; - - private: - // Disable copy/assignment ctors - thermal_compensation_guard(const thermal_compensation_guard&); - thermal_compensation_guard& operator=(const thermal_compensation_guard&); - }; - - DirectSearchCalibrationResult auto_calibrated::get_calibration_status(int timeout_ms, std::function progress_func, bool wait_for_final_results) - { - DirectSearchCalibrationResult result{}; + ds_calib_common::dsc_check_status_result result{}; int count = 0; int retries = 0; @@ -293,9 +166,9 @@ namespace librealsense try { // Check calibration status - auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, py_rx_calib_check_status }); + auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, ds_calib_common::PY_RX_CALIB_CHECK_STATUS }); LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << " check occ status, res size = " << res.size())); - if (res.size() < sizeof(DirectSearchCalibrationResult)) + if( res.size() < sizeof( ds_calib_common::dsc_check_status_result ) ) { if (!((retries++) % 5)) // Add log debug once in a sec { @@ -304,8 +177,8 @@ namespace librealsense } else { - result = *reinterpret_cast(res.data()); - done = !wait_for_final_results || result.status != RS2_DSC_STATUS_RESULT_NOT_READY; + result = *reinterpret_cast< ds_calib_common::dsc_check_status_result * >( res.data() ); + done = !wait_for_final_results || result.status != ds_calib_common::STATUS_RESULT_NOT_READY; } } catch (const invalid_value_exception& e) @@ -337,8 +210,8 @@ namespace librealsense { int calib_type = DEFAULT_CALIB_TYPE; - auto_calib_speed speed(DEFAULT_SPEED); - int speed_fl = auto_calib_speed::speed_slow; + ds_calib_common::auto_calib_speed speed( DEFAULT_SPEED ); + int speed_fl = ds_calib_common::SPEED_SLOW; int scan_parameter = DEFAULT_SCAN; int data_sampling = DEFAULT_SAMPLING; int apply_preset = 1; @@ -369,21 +242,21 @@ namespace librealsense { int tmp_speed(DEFAULT_SPEED); int tmp_host_assistance(0); - auto jsn = parse_json(json); - try_fetch(jsn, "calib type", &calib_type); + auto jsn = ds_calib_common::parse_json( json ); + ds_calib_common::update_value_if_exists(jsn, "calib type", calib_type); if (calib_type == 0) { - try_fetch(jsn, "speed", &tmp_speed); - if (tmp_speed < speed_very_fast || tmp_speed > speed_white_wall) + ds_calib_common::update_value_if_exists(jsn, "speed", tmp_speed); + if( tmp_speed < ds_calib_common::SPEED_VERY_FAST || tmp_speed > ds_calib_common::SPEED_WHITE_WALL ) throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'speed' " << speed << " is out of range (0 - 4)." ); - speed = auto_calib_speed(tmp_speed); + speed = ds_calib_common::auto_calib_speed( tmp_speed ); } else - try_fetch(jsn, "speed", &speed_fl); + ds_calib_common::update_value_if_exists(jsn, "speed", speed_fl); - try_fetch(jsn, "host assistance", &tmp_host_assistance); + ds_calib_common::update_value_if_exists(jsn, "host assistance", tmp_host_assistance); if (tmp_host_assistance < (int)host_assistance_type::no_assistance || tmp_host_assistance > (int)host_assistance_type::assistance_second_feed) throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'host assistance' " @@ -391,25 +264,25 @@ namespace librealsense << (int)host_assistance_type::assistance_second_feed << ")." ); host_assistance = host_assistance_type(tmp_host_assistance); - try_fetch(jsn, "scan parameter", &scan_parameter); - try_fetch(jsn, "data sampling", &data_sampling); - try_fetch(jsn, "apply preset", &apply_preset); + ds_calib_common::update_value_if_exists(jsn, "scan parameter", scan_parameter); + ds_calib_common::update_value_if_exists(jsn, "data sampling", data_sampling); + ds_calib_common::update_value_if_exists(jsn, "apply preset", apply_preset); - try_fetch(jsn, "fl step count", &fl_step_count); - try_fetch(jsn, "fy scan range", &fy_scan_range); - try_fetch(jsn, "keep new value after sucessful scan", &keep_new_value_after_sucessful_scan); - try_fetch(jsn, "fl data sampling", &fl_data_sampling); - try_fetch(jsn, "adjust both sides", &adjust_both_sides); + ds_calib_common::update_value_if_exists(jsn, "fl step count", fl_step_count); + ds_calib_common::update_value_if_exists(jsn, "fy scan range", fy_scan_range); + ds_calib_common::update_value_if_exists(jsn, "keep new value after sucessful scan", keep_new_value_after_sucessful_scan); + ds_calib_common::update_value_if_exists(jsn, "fl data sampling", fl_data_sampling); + ds_calib_common::update_value_if_exists(jsn, "adjust both sides", adjust_both_sides); - try_fetch(jsn, "fl scan location", &fl_scan_location); - try_fetch(jsn, "fy scan direction", &fy_scan_direction); - try_fetch(jsn, "white wall mode", &white_wall_mode); + ds_calib_common::update_value_if_exists(jsn, "fl scan location", fl_scan_location); + ds_calib_common::update_value_if_exists(jsn, "fy scan direction", fy_scan_direction); + ds_calib_common::update_value_if_exists(jsn, "white wall mode", white_wall_mode); - try_fetch(jsn, "scan only", &scan_only_v3); - try_fetch(jsn, "interactive scan", &interactive_scan_v3); + ds_calib_common::update_value_if_exists(jsn, "scan only", scan_only_v3); + ds_calib_common::update_value_if_exists(jsn, "interactive scan", interactive_scan_v3); int val = 0; - try_fetch(jsn, "step count v3", &val); + ds_calib_common::update_value_if_exists(jsn, "step count v3", val); step_count_v3 = static_cast(val); if (step_count_v3 > 0) @@ -419,11 +292,11 @@ namespace librealsense val = 0; std::stringstream ss; ss << "fill factor " << i; - try_fetch(jsn, ss.str(), &val); + ds_calib_common::update_value_if_exists(jsn, ss.str(), val); fill_factor[i] = static_cast(val); } } - try_fetch(jsn, "resize factor", &_resize_factor); + ds_calib_common::update_value_if_exists(jsn, "resize factor", _resize_factor); } std::vector res; @@ -436,24 +309,24 @@ namespace librealsense _interactive_scan = false; // Production code must enforce non-interactive runs. Interactive scans for development only switch (speed) { - case auto_calib_speed::speed_very_fast: + case ds_calib_common::SPEED_VERY_FAST: _total_frames = 60; break; - case auto_calib_speed::speed_fast: + case ds_calib_common::SPEED_FAST: _total_frames = 120; break; - case auto_calib_speed::speed_medium: + case ds_calib_common::SPEED_MEDIUM: _total_frames = 256; break; - case auto_calib_speed::speed_slow: + case ds_calib_common::SPEED_SLOW: _total_frames = 256; break; - case auto_calib_speed::speed_white_wall: + case ds_calib_common::SPEED_WHITE_WALL: _total_frames = 120; break; } std::fill_n(_fill_factor, 256, 0); - DirectSearchCalibrationResult result = get_calibration_status(timeout_ms, [progress_callback, host_assistance, speed](int count) + ds_calib_common::dsc_check_status_result result = get_calibration_status(timeout_ms, [progress_callback, host_assistance, speed](int count) { if (progress_callback) { @@ -467,7 +340,7 @@ namespace librealsense } }, false); // Handle errors from firmware - rs2_dsc_status status = (rs2_dsc_status)result.status; + ds_calib_common::dsc_status status = (ds_calib_common::dsc_status)result.status; if (result.maxDepth == 0) { @@ -482,7 +355,7 @@ namespace librealsense if (calib_type == 0) { LOG_DEBUG("run_on_chip_calibration with parameters: speed = " << speed << " scan_parameter = " << scan_parameter << " data_sampling = " << data_sampling); - check_params(speed, scan_parameter, data_sampling); + ds_calib_common::check_params(speed, scan_parameter, data_sampling); uint32_t p4 = 0; if (scan_parameter) @@ -496,7 +369,7 @@ namespace librealsense if (interactive_scan_v3) p4 |= (1 << 9); - if (speed == speed_white_wall && apply_preset) + if( speed == ds_calib_common::SPEED_WHITE_WALL && apply_preset ) { preset_recover = change_preset(); std::this_thread::sleep_for(std::chrono::milliseconds(200)); @@ -505,7 +378,7 @@ namespace librealsense // Begin auto-calibration if (host_assistance == host_assistance_type::no_assistance || host_assistance == host_assistance_type::assistance_start) { - auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, py_rx_calib_begin, speed, 0, p4 }); + auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, ds_calib_common::PY_RX_CALIB_BEGIN, speed, 0, p4 }); LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << " send occ py_rx_calib_begin, speed = " << speed << ", p4 = " << p4 << " res size = " << res.size())); } @@ -514,7 +387,7 @@ namespace librealsense { if (host_assistance == host_assistance_type::assistance_first_feed) { - command cmd(ds::AUTO_CALIB, interactive_scan_control, 0, 0); + command cmd( ds::AUTO_CALIB, ds_calib_common::INTERACTIVE_SCAN_CONTROL, 0, 0 ); LOG_OCC_WARN(" occ interactive_scan_control 0,0 - save statistics "); uint8_t* p = reinterpret_cast(&step_count_v3); cmd.data.push_back(p[0]); @@ -545,7 +418,7 @@ namespace librealsense while(( ++iter < 3) && (!success)); } - DirectSearchCalibrationResult result = get_calibration_status(timeout_ms, [progress_callback, host_assistance, speed](int count) + ds_calib_common::dsc_check_status_result result = get_calibration_status(timeout_ms, [progress_callback, host_assistance, speed](int count) { if( progress_callback ) { @@ -560,10 +433,10 @@ namespace librealsense }); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - auto status = (rs2_dsc_status)result.status; + auto status = (ds_calib_common::dsc_status)result.status; // Handle errors from firmware - if (status != RS2_DSC_STATUS_SUCCESS) + if (status != ds_calib_common::STATUS_SUCCESS) { handle_calibration_error(status); } @@ -600,7 +473,7 @@ namespace librealsense if (interactive_scan_v3) p4 |= (1 << 9); - if (speed == speed_white_wall && apply_preset) + if( speed == ds_calib_common::SPEED_WHITE_WALL && apply_preset ) { preset_recover = change_preset(); std::this_thread::sleep_for(std::chrono::milliseconds(200)); @@ -609,7 +482,7 @@ namespace librealsense if( host_assistance == host_assistance_type::no_assistance || host_assistance == host_assistance_type::assistance_start ) { _hw_monitor->send( command{ ds::AUTO_CALIB, - focal_length_calib_begin, + ds_calib_common::FOCAL_LENGTH_CALIB_BEGIN, static_cast< uint32_t >( fl_step_count ), static_cast< uint32_t >( fy_scan_range ), p4 } ); @@ -619,7 +492,7 @@ namespace librealsense { if (host_assistance == host_assistance_type::assistance_first_feed) { - command cmd(ds::AUTO_CALIB, interactive_scan_control, 0, 0); + command cmd( ds::AUTO_CALIB, ds_calib_common::INTERACTIVE_SCAN_CONTROL, 0, 0 ); LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << "occ interactive_scan_control 0,0 " << " res size = " << res.size())); uint8_t* p = reinterpret_cast(&step_count_v3); cmd.data.push_back(p[0]); @@ -648,14 +521,14 @@ namespace librealsense std::this_thread::sleep_for(std::chrono::milliseconds(200)); // Check calibration status - auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, get_focal_legth_calib_result }); + auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, ds_calib_common::GET_FOCAL_LEGTH_CALIB_RESULT }); if (res.size() < sizeof(FocalLengthCalibrationResult)) LOG_WARNING("Not enough data from CALIB_STATUS!"); else { result = *reinterpret_cast(res.data()); - done = result.status != RS2_DSC_STATUS_RESULT_NOT_READY; + done = result.status != ds_calib_common::STATUS_RESULT_NOT_READY; } if( progress_callback ) @@ -683,10 +556,10 @@ namespace librealsense std::this_thread::sleep_for(std::chrono::milliseconds(100)); - auto status = (rs2_dsc_status)result.status; + auto status = (ds_calib_common::dsc_status)result.status; // Handle errors from firmware - if (status != RS2_DSC_STATUS_SUCCESS) + if (status != ds_calib_common::STATUS_SUCCESS) { handle_calibration_error(status); } @@ -722,7 +595,7 @@ namespace librealsense if (interactive_scan_v3) p4 |= (1 << 9); - if (speed == speed_white_wall && apply_preset) + if( speed == ds_calib_common::SPEED_WHITE_WALL && apply_preset ) { preset_recover = change_preset(); std::this_thread::sleep_for(std::chrono::milliseconds(200)); @@ -732,7 +605,7 @@ namespace librealsense if (host_assistance == host_assistance_type::no_assistance || host_assistance == host_assistance_type::assistance_start) { _hw_monitor->send( command{ ds::AUTO_CALIB, - py_rx_plus_fl_calib_begin, + ds_calib_common::PY_RX_PLUS_FL_CALIB_BEGIN, static_cast< uint32_t >( speed_fl ), 0, p4 } ); @@ -743,7 +616,7 @@ namespace librealsense { if ((host_assistance == host_assistance_type::assistance_first_feed) || (host_assistance == host_assistance_type::assistance_second_feed)) { - command cmd(ds::AUTO_CALIB, interactive_scan_control, 0, 0); + command cmd( ds::AUTO_CALIB, ds_calib_common::INTERACTIVE_SCAN_CONTROL, 0, 0 ); LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << "occ interactive_scan_control 0,0")); uint8_t* p = reinterpret_cast(&step_count_v3); cmd.data.push_back(p[0]); @@ -777,14 +650,14 @@ namespace librealsense // Check calibration status try { - auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, get_py_rx_plus_fl_calib_result }); + auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, ds_calib_common::GET_PY_RX_PLUS_FL_CALIB_RESULT }); LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << "occ get_py_rx_plus_fl_calib_result res size = " << res.size() )); if (res.size() < sizeof(DscPyRxFLCalibrationTableResult)) throw std::runtime_error("Not enough data from CALIB_STATUS!"); result = *reinterpret_cast(res.data()); - done = result.status != RS2_DSC_STATUS_RESULT_NOT_READY; + done = result.status != ds_calib_common::STATUS_RESULT_NOT_READY; } catch (const std::exception& ex) { @@ -815,10 +688,10 @@ namespace librealsense std::this_thread::sleep_for(std::chrono::milliseconds(100)); - auto status = (rs2_dsc_status)result.status; + auto status = (ds_calib_common::dsc_status)result.status; // Handle errors from firmware - if (status != RS2_DSC_STATUS_SUCCESS) + if (status != ds_calib_common::STATUS_SUCCESS) handle_calibration_error(status); res = get_PyRxFL_calibration_results(&h_1, &h_2); @@ -864,24 +737,24 @@ namespace librealsense if (json.size() > 0) { - auto jsn = parse_json(json); - try_fetch(jsn, "speed", &speed); - try_fetch(jsn, "average step count", &average_step_count); - try_fetch(jsn, "step count", &step_count); - try_fetch(jsn, "accuracy", &accuracy); - try_fetch(jsn, "scan parameter", &scan_parameter); - try_fetch(jsn, "data sampling", &data_sampling); - try_fetch(jsn, "apply preset", &apply_preset); + auto jsn = ds_calib_common::parse_json( json ); + ds_calib_common::update_value_if_exists(jsn, "speed", speed); + ds_calib_common::update_value_if_exists(jsn, "average step count", average_step_count); + ds_calib_common::update_value_if_exists(jsn, "step count", step_count); + ds_calib_common::update_value_if_exists(jsn, "accuracy", accuracy); + ds_calib_common::update_value_if_exists(jsn, "scan parameter", scan_parameter); + ds_calib_common::update_value_if_exists(jsn, "data sampling", data_sampling); + ds_calib_common::update_value_if_exists(jsn, "apply preset", apply_preset); int tmp_host_assistance(0); - try_fetch(jsn, "host assistance", &tmp_host_assistance); + ds_calib_common::update_value_if_exists(jsn, "host assistance", tmp_host_assistance); if (tmp_host_assistance < (int)host_assistance_type::no_assistance || tmp_host_assistance >(int)host_assistance_type::assistance_second_feed) throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'host assistance' " << tmp_host_assistance << " is out of range (0 - " << (int)host_assistance_type::assistance_second_feed << ")." ); host_assistance = host_assistance_type(tmp_host_assistance); - try_fetch(jsn, "depth", &depth); - try_fetch(jsn, "resize factor", &_resize_factor); + ds_calib_common::update_value_if_exists(jsn, "depth", depth); + ds_calib_common::update_value_if_exists(jsn, "resize factor", _resize_factor); } if (host_assistance != host_assistance_type::no_assistance && _interactive_state == interactive_calibration_state::RS2_OCC_STATE_NOT_ACTIVE) @@ -893,7 +766,7 @@ namespace librealsense _action = auto_calib_action::RS2_OCC_ACTION_TARE_CALIB; _interactive_state = interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CAMERA_START; - DirectSearchCalibrationResult result = get_calibration_status(timeout_ms, [progress_callback, host_assistance, speed](int count) + ds_calib_common::dsc_check_status_result result = get_calibration_status(timeout_ms, [progress_callback, host_assistance, speed](int count) { if (progress_callback) { @@ -904,7 +777,7 @@ namespace librealsense } }, false); // Handle errors from firmware - rs2_dsc_status status = (rs2_dsc_status)result.status; + ds_calib_common::dsc_status status = (ds_calib_common::dsc_status)result.status; if (result.maxDepth == 0) { @@ -920,7 +793,7 @@ namespace librealsense { LOG_OCC_WARN("run_tare_calibration interactive control (2) with parameters: depth = " << depth); _hw_monitor->send( command{ ds::AUTO_CALIB, - interactive_scan_control, + ds_calib_common::INTERACTIVE_SCAN_CONTROL, 2, static_cast< uint32_t >( depth ) } ); } @@ -964,7 +837,7 @@ namespace librealsense } if (depth == 0) - _hw_monitor->send(command{ ds::AUTO_CALIB, tare_calib_begin, param2, param3.param3, param.param_4 }); + _hw_monitor->send(command{ ds::AUTO_CALIB, ds_calib_common::TARE_CALIB_BEGIN, param2, param3.param3, param.param_4 }); } if (host_assistance == host_assistance_type::no_assistance || depth < 0) @@ -985,7 +858,7 @@ namespace librealsense // Check calibration status try { - res = _hw_monitor->send(command{ ds::AUTO_CALIB, tare_calib_check_status }); + res = _hw_monitor->send(command{ ds::AUTO_CALIB, ds_calib_common::TARE_CALIB_CHECK_STATUS }); if (res.size() < sizeof(TareCalibrationResult)) { if (depth < 0) @@ -994,7 +867,7 @@ namespace librealsense } result = *reinterpret_cast(res.data()); - done = result.status != RS2_DSC_STATUS_RESULT_NOT_READY; + done = result.status != ds_calib_common::STATUS_RESULT_NOT_READY; } catch (const std::exception& ex) { @@ -1023,7 +896,7 @@ namespace librealsense "Calibration did not converge on time"); } - auto status = (rs2_dsc_status)result.status; + auto status = (ds_calib_common::dsc_status)result.status; uint8_t* p = res.data() + sizeof(TareCalibrationResult) + 2 * result.iterations * sizeof(uint32_t); float* ph = reinterpret_cast(p); @@ -1035,7 +908,7 @@ namespace librealsense LOG_INFO("Z calculated from health check numbers : before=" << (ph[0] + 1) * ground_truth_mm << ", after=" << (ph[1] + 1) * ground_truth_mm); // Handle errors from firmware - if (status != RS2_DSC_STATUS_SUCCESS) + if (status != ds_calib_common::STATUS_SUCCESS) handle_calibration_error(status); res = get_calibration_results(); @@ -1413,7 +1286,7 @@ namespace librealsense if (_interactive_scan) { - auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, interactive_scan_control, 1}); + auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, ds_calib_common::INTERACTIVE_SCAN_CONTROL, 1}); LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << " occ interactive_scan_control 1,")); } _skipped_frames = 0; @@ -1486,7 +1359,7 @@ namespace librealsense } else if (_interactive_scan) { - _hw_monitor->send(command{ ds::AUTO_CALIB, interactive_scan_control, 1 }); + _hw_monitor->send( command{ ds::AUTO_CALIB, ds_calib_common::INTERACTIVE_SCAN_CONTROL, 1 } ); LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << "Call for interactive_scan_control, 1")); } } @@ -1624,25 +1497,9 @@ namespace librealsense _preset_change = false; } - void auto_calibrated::check_params(int speed, int scan_parameter, int data_sampling) const - { - if (speed < speed_very_fast || speed > speed_white_wall) - throw invalid_value_exception( rsutils::string::from() - << "Auto calibration failed! Given value of 'speed' " << speed - << " is out of range (0 - 4)." ); - if (scan_parameter != py_scan && scan_parameter != rx_scan) - throw invalid_value_exception( rsutils::string::from() - << "Auto calibration failed! Given value of 'scan parameter' " - << scan_parameter << " is out of range (0 - 1)." ); - if (data_sampling != polling && data_sampling != interrupt) - throw invalid_value_exception( rsutils::string::from() - << "Auto calibration failed! Given value of 'data sampling' " << data_sampling - << " is out of range (0 - 1)." ); - } - void auto_calibrated::check_tare_params(int speed, int scan_parameter, int data_sampling, int average_step_count, int step_count, int accuracy) { - check_params(speed, scan_parameter, data_sampling); + ds_calib_common::check_params( speed, scan_parameter, data_sampling ); if (average_step_count < 1 || average_step_count > 30) throw invalid_value_exception( rsutils::string::from() @@ -1660,11 +1517,11 @@ namespace librealsense void auto_calibrated::check_one_button_params(int speed, int keep_new_value_after_sucessful_scan, int data_sampling, int adjust_both_sides, int fl_scan_location, int fy_scan_direction, int white_wall_mode) const { - if (speed < speed_very_fast || speed > speed_white_wall) + if( speed < ds_calib_common::SPEED_VERY_FAST || speed > ds_calib_common::SPEED_WHITE_WALL ) throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'speed' " << speed << " is out of range (0 - 4)."); if (keep_new_value_after_sucessful_scan < 0 || keep_new_value_after_sucessful_scan > 1) throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'keep_new_value_after_sucessful_scan' " << keep_new_value_after_sucessful_scan << " is out of range (0 - 1)."); - if (data_sampling != polling && data_sampling != interrupt) + if( data_sampling != ds_calib_common::POLLING && data_sampling != ds_calib_common::INTERRUPT ) throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'data sampling' " << data_sampling << " is out of range (0 - 1)."); if (adjust_both_sides < 0 || adjust_both_sides > 1) throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'adjust_both_sides' " << adjust_both_sides << " is out of range (0 - 1)."); @@ -1678,22 +1535,22 @@ namespace librealsense void auto_calibrated::handle_calibration_error(int status) const { - if (status == RS2_DSC_STATUS_EDGE_TOO_CLOSE) + if (status == ds_calib_common::STATUS_EDGE_TOO_CLOSE) { throw std::runtime_error("Calibration didn't converge! - edges too close\n" "Please retry in different lighting conditions"); } - else if (status == RS2_DSC_STATUS_FILL_FACTOR_TOO_LOW) + else if (status == ds_calib_common::STATUS_FILL_FACTOR_TOO_LOW) { throw std::runtime_error("Not enough depth pixels! - low fill factor)\n" "Please retry in different lighting conditions"); } - else if (status == RS2_DSC_STATUS_NOT_CONVERGE) + else if (status == ds_calib_common::STATUS_NOT_CONVERGE) { throw std::runtime_error("Calibration failed to converge\n" "Please retry in different lighting conditions"); } - else if (status == RS2_DSC_STATUS_NO_DEPTH_AVERAGE) + else if (status == ds_calib_common::STATUS_NO_DEPTH_AVERAGE) { throw std::runtime_error("Calibration didn't converge! - no depth average\n" "Please retry in different lighting conditions"); @@ -1708,15 +1565,15 @@ namespace librealsense using namespace ds; // Get new calibration from the firmware - auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, get_calibration_result}); - if (res.size() < sizeof(DscResultBuffer)) + auto res = _hw_monitor->send( command{ ds::AUTO_CALIB, ds_calib_common::GET_CALIBRATION_RESULT } ); + if( res.size() < sizeof( ds_calib_common::dsc_result ) ) throw std::runtime_error("Not enough data from CALIB_STATUS!"); - auto reslt = (DscResultBuffer*)(res.data()); + auto reslt = (ds_calib_common::dsc_result *)( res.data() ); - table_header* header = reinterpret_cast(res.data() + sizeof(DscResultBuffer)); + table_header * header = reinterpret_cast< table_header * >( res.data() + sizeof( ds_calib_common::dsc_result ) ); - if (res.size() < sizeof(DscResultBuffer) + sizeof(table_header) + header->table_size) + if( res.size() < sizeof( ds_calib_common::dsc_result ) + sizeof( table_header ) + header->table_size ) throw std::runtime_error("Table truncated in CALIB_STATUS!"); std::vector calib; @@ -1725,7 +1582,7 @@ namespace librealsense memcpy(calib.data(), header, calib.size()); // Copy to new_calib if(health) - *health = reslt->m_dscResultParams.m_healthCheck; + *health = reslt->healthCheck; return calib; } @@ -1735,7 +1592,7 @@ namespace librealsense using namespace ds; // Get new calibration from the firmware - auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, get_py_rx_plus_fl_calib_result }); + auto res = _hw_monitor->send( command{ ds::AUTO_CALIB, ds_calib_common::GET_PY_RX_PLUS_FL_CALIB_RESULT } ); if (res.size() < sizeof(DscPyRxFLCalibrationTableResult)) throw std::runtime_error("Not enough data from CALIB_STATUS!"); diff --git a/src/ds/d400/d400-auto-calibration.h b/src/ds/d400/d400-auto-calibration.h index 0e5a3e7bba..962a9ea087 100644 --- a/src/ds/d400/d400-auto-calibration.h +++ b/src/ds/d400/d400-auto-calibration.h @@ -5,27 +5,11 @@ #include "auto-calibrated-device.h" #include "../../core/advanced_mode.h" +#include namespace librealsense { -#pragma pack(push, 1) -#pragma pack(1) - struct DirectSearchCalibrationResult - { - uint16_t status; // DscStatus - uint16_t stepCount; - uint16_t stepSize; // 1/1000 of a pixel - uint32_t pixelCountThreshold; // minimum number of pixels in - // selected bin - uint16_t minDepth; // Depth range for FWHM - uint16_t maxDepth; - uint32_t rightPy; // 1/1000000 of normalized unit - float healthCheck; - float rightRotation[9]; // Right rotation - }; -#pragma pack(pop) - class auto_calibrated : public auto_calibrated_interface { enum class auto_calib_action @@ -69,11 +53,8 @@ namespace librealsense void handle_calibration_error(int status) const; std::map parse_json(std::string json); std::shared_ptr< ds_advanced_mode_base> change_preset(); - void check_params(int speed, int scan_parameter, int data_sampling) const; void check_tare_params(int speed, int scan_parameter, int data_sampling, int average_step_count, int step_count, int accuracy); - void check_focal_length_params(int step_count, int fy_scan_range, int keep_new_value_after_sucessful_scan, int interrrupt_data_samling, int adjust_both_sides, int fl_scan_location, int fy_scan_direction, int white_wall_mode) const; void check_one_button_params(int speed, int keep_new_value_after_sucessful_scan, int data_sampling, int adjust_both_sides, int fl_scan_location, int fy_scan_direction, int white_wall_mode) const; - void get_target_rect_info(rs2_frame_queue* frames, float rect_sides[4], float& fx, float& fy, int progress, rs2_update_progress_callback_sptr progress_callback); void undistort(uint8_t* img, const rs2_intrinsics& intrin, int roi_ws, int roi_hs, int roi_we, int roi_he); void change_preset_and_stay(); void restore_preset(); @@ -83,7 +64,7 @@ namespace librealsense void fill_missing_data(uint16_t data[256], int size); void remove_outliers(uint16_t data[256], int size); void collect_depth_frame_sum(const rs2_frame* f); - DirectSearchCalibrationResult get_calibration_status(int timeout_ms, std::function progress_func, bool wait_for_final_results = true); + ds_calib_common::dsc_check_status_result get_calibration_status( int timeout_ms, std::function< void( const int count ) > progress_func, bool wait_for_final_results = true ); std::vector _curr_calibration; std::shared_ptr _hw_monitor; diff --git a/src/ds/d500/d500-auto-calibration.cpp b/src/ds/d500/d500-auto-calibration.cpp index f998c9fe5c..5109ec1b33 100644 --- a/src/ds/d500/d500-auto-calibration.cpp +++ b/src/ds/d500/d500-auto-calibration.cpp @@ -28,12 +28,17 @@ namespace librealsense "Failed to Run" }; - d500_auto_calibrated::d500_auto_calibrated(std::shared_ptr calib_engine) : - _calib_engine(calib_engine), - _mode (calibration_mode::RESERVED), - _state (calibration_state::IDLE), - _result(calibration_result::UNKNOWN) - {} + d500_auto_calibrated::d500_auto_calibrated( std::shared_ptr< d500_debug_protocol_calibration_engine > calib_engine, + debug_interface * debug_dev ) + : _calib_engine( calib_engine ) + , _mode( calibration_mode::RESERVED ) + , _state( calibration_state::IDLE ) + , _result( calibration_result::UNKNOWN ) + , _debug_dev( debug_dev ) + { + if( ! _debug_dev ) + throw not_implemented_exception( " debug_interface must be supplied to d500_auto_calibrated" ); + } void d500_auto_calibrated::check_preconditions_and_set_state() { @@ -77,44 +82,116 @@ namespace librealsense throw std::runtime_error("run_on_chip_calibration called with wrong content in json file"); } - std::vector d500_auto_calibrated::run_on_chip_calibration(int timeout_ms, std::string json, - float* const health, rs2_update_progress_callback_sptr progress_callback) + std::vector d500_auto_calibrated::run_on_chip_calibration( int timeout_ms, + std::string json, + float * const health, + rs2_update_progress_callback_sptr progress_callback ) { - std::vector res; + bool is_d555 = false; + auto dev = As< device >( this ); + std::string pid_str = dev ? dev->get_info( RS2_CAMERA_INFO_PRODUCT_ID ) : ""; + if( pid_str == "0B56" || pid_str == "DDS" ) + is_d555 = true; + + if( is_d555 ) + return run_occ( timeout_ms, json, health, progress_callback ); + + return run_triggered_calibration( timeout_ms, json, progress_callback ); + } + + std::vector< uint8_t > d500_auto_calibrated::run_triggered_calibration( int timeout_ms, + std::string json, + rs2_update_progress_callback_sptr progress_callback ) + { + std::vector< uint8_t > res; + try { - get_mode_from_json(json); + get_mode_from_json( json ); // checking preconditions check_preconditions_and_set_state(); // sending command to start calibration - res = _calib_engine->run_triggered_calibration(_mode); + res = _calib_engine->run_triggered_calibration( _mode ); - if (_mode == calibration_mode::RUN || - _mode == calibration_mode::DRY_RUN) + if( _mode == calibration_mode::RUN || _mode == calibration_mode::DRY_RUN ) { - res = update_calibration_status(timeout_ms, progress_callback); + res = update_calibration_status( timeout_ms, progress_callback ); } - else if (_mode == calibration_mode::ABORT) + else if( _mode == calibration_mode::ABORT ) { res = update_abort_status(); } } - catch (std::runtime_error&) + catch( std::runtime_error & ) { throw; } - catch(...) + catch( ... ) { std::string error_message_prefix = "\nRUN OCC "; - if (_mode == calibration_mode::DRY_RUN) + if( _mode == calibration_mode::DRY_RUN ) error_message_prefix = "\nDRY RUN OCC "; - else if (_mode == calibration_mode::ABORT) + else if( _mode == calibration_mode::ABORT ) error_message_prefix = "\nABORT OCC "; - throw std::runtime_error(rsutils::string::from() << error_message_prefix + "Could not be triggered"); + throw std::runtime_error( rsutils::string::from() << error_message_prefix + "Could not be triggered" ); + } + + return res; + } + + std::vector< uint8_t > d500_auto_calibrated::run_occ( int timeout_ms, std::string json, float * const health, + rs2_update_progress_callback_sptr progress_callback ) + { + int speed = ds_calib_common::SPEED_SLOW; + int scan_parameter = ds_calib_common::PY_SCAN; + int data_sampling = ds_calib_common::INTERRUPT; + + volatile thermal_compensation_guard grd(this); //Enforce Thermal Compensation off during OCC + + if (json.size() > 0) + { + auto jsn = ds_calib_common::parse_json( json ); + + ds_calib_common::update_value_if_exists( jsn, "speed", speed ); + ds_calib_common::update_value_if_exists( jsn, "scan parameter", scan_parameter ); + ds_calib_common::update_value_if_exists( jsn, "data sampling", data_sampling ); + + ds_calib_common::check_params( speed, scan_parameter, data_sampling ); + } + + LOG_DEBUG("run_on_chip_calibration with parameters: speed = " << speed << " scan_parameter = " << scan_parameter << " data_sampling = " << data_sampling); + + uint32_t p4 = 0; + if (scan_parameter) + p4 |= 1; + if (data_sampling) + p4 |= (1 << 3); + + // Begin auto-calibration + auto cmd = _debug_dev->build_command( ds::AUTO_CALIB, ds_calib_common::PY_RX_CALIB_BEGIN, speed, 0, p4 ); + std::vector< uint8_t > res = _debug_dev->send_receive_raw_data( cmd ); // TODO - need to remove res first 4 bytes? + + ds_calib_common::dsc_check_status_result result = get_calibration_status(timeout_ms, [progress_callback, speed](int count) + { + if( progress_callback ) + progress_callback->on_update_progress( count++ * ( 2.f * static_cast< int >( speed ) ) ); // currently this number does not reflect the actual progress + }); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + auto status = (ds_calib_common::dsc_status)result.status; + + // Handle errors from firmware + if (status != ds_calib_common::STATUS_SUCCESS) + { + ds_calib_common::handle_calibration_error( status ); } + if (progress_callback) + progress_callback->on_update_progress(static_cast(100)); + res = get_calibration_results(health); + return res; } @@ -354,4 +431,82 @@ namespace librealsense _calib_engine->set_calibration_config(calibration_config_json_str); } + ds_calib_common::dsc_check_status_result + d500_auto_calibrated::get_calibration_status( int timeout_ms, + std::function< void( const int count ) > progress_func, + bool wait_for_final_results ) const + { + ds_calib_common::dsc_check_status_result result{}; + + int count = 0; + int retries = 0; + bool done = false; + + auto start = std::chrono::high_resolution_clock::now(); + auto now = start; + + // While not ready... + do + { + std::this_thread::sleep_for( std::chrono::milliseconds( 200 ) ); + try + { + // Check calibration status + auto cmd = _debug_dev->build_command( ds::AUTO_CALIB, ds_calib_common::PY_RX_CALIB_CHECK_STATUS ); + auto res = _debug_dev->send_receive_raw_data( cmd ); // TODO - need to remove res first 4 bytes? + if( res.size() < sizeof( ds_calib_common::dsc_check_status_result ) ) + { + if( ! ( ( retries++ ) % 5 ) ) // Add log debug once a sec + LOG_DEBUG( "Not enough data from CALIB_STATUS!" ); + } + else + { + result = *reinterpret_cast< ds_calib_common::dsc_check_status_result * >( res.data() ); + done = ! wait_for_final_results || result.status != ds_calib_common::STATUS_RESULT_NOT_READY; + } + } + catch( const invalid_value_exception & e ) + { + LOG_DEBUG( "error: " << e.what() ); // Asked for status while firmware is still in progress. + } + + if( progress_func ) + { + progress_func( count ); + } + + now = std::chrono::high_resolution_clock::now(); + } + while( now - start < std::chrono::milliseconds( timeout_ms ) && ! done ); + + + // If we exit due to timeout, report timeout + if( ! done ) + throw std::runtime_error( "Operation timed-out!\nCalibration state did not converge on time" ); + + return result; + } + + std::vector< uint8_t > d500_auto_calibrated::get_calibration_results( float * const health ) const + { + // Get new calibration from the firmware + auto cmd = _debug_dev->build_command( ds::AUTO_CALIB, ds_calib_common::GET_CALIBRATION_RESULT ); + auto res = _debug_dev->send_receive_raw_data( cmd ); // TODO - need to remove res first 4 bytes? + if( res.size() < sizeof( ds_calib_common::dsc_result ) ) + throw std::runtime_error( "Not enough data from CALIB_STATUS!" ); + + auto * header = reinterpret_cast< ds::table_header * >( res.data() + sizeof( ds_calib_common::dsc_result ) ); + if( res.size() < sizeof( ds_calib_common::dsc_result ) + sizeof( ds::table_header ) + header->table_size ) + throw std::runtime_error( "Table truncated in CALIB_STATUS!" ); + + std::vector< uint8_t > calib; + calib.resize( sizeof( ds::table_header ) + header->table_size, 0 ); + memcpy( calib.data(), header, calib.size() ); // Copy to new_calib + + auto reslt = reinterpret_cast< ds_calib_common::dsc_result * >( res.data() ); + if( health ) + *health = reslt->healthCheck; + + return calib; + } } diff --git a/src/ds/d500/d500-auto-calibration.h b/src/ds/d500/d500-auto-calibration.h index f98d5fd9f4..f0a94addc3 100644 --- a/src/ds/d500/d500-auto-calibration.h +++ b/src/ds/d500/d500-auto-calibration.h @@ -5,15 +5,19 @@ #include #include // should remain for members _mode, _state, _result +#include namespace librealsense { + class debug_interface; class d500_debug_protocol_calibration_engine; + class d500_auto_calibrated : public auto_calibrated_interface { public: - d500_auto_calibrated(std::shared_ptr calib_engine); + d500_auto_calibrated( std::shared_ptr< d500_debug_protocol_calibration_engine > calib_engine, + debug_interface * debug_dev ); void write_calibration() const override; std::vector run_on_chip_calibration(int timeout_ms, std::string json, float* const health, rs2_update_progress_callback_sptr progress_callback) override; std::vector run_tare_calibration(int timeout_ms, float ground_truth_mm, std::string json, float* const health, rs2_update_progress_callback_sptr progress_callback) override; @@ -35,11 +39,20 @@ namespace librealsense void get_mode_from_json(const std::string& json); std::vector update_calibration_status(int timeout_ms, rs2_update_progress_callback_sptr progress_callback); std::vector update_abort_status(); + std::vector< uint8_t > run_triggered_calibration( int timeout_ms, std::string json, + rs2_update_progress_callback_sptr progress_callback ); + std::vector< uint8_t > run_occ( int timeout_ms, std::string json, float * const health, + rs2_update_progress_callback_sptr progress_callback ); + ds_calib_common::dsc_check_status_result get_calibration_status( int timeout_ms, + std::function< void( const int count ) > progress_func, + bool wait_for_final_results = true ) const; + std::vector< uint8_t > get_calibration_results( float * const health = nullptr ) const; mutable std::vector< uint8_t > _curr_calibration; std::shared_ptr _calib_engine; calibration_mode _mode; calibration_state _state; calibration_result _result; + debug_interface * _debug_dev; }; } diff --git a/src/ds/d500/d500-device.cpp b/src/ds/d500/d500-device.cpp index 9c38d0aeeb..f8386aa7a9 100644 --- a/src/ds/d500/d500-device.cpp +++ b/src/ds/d500/d500-device.cpp @@ -369,7 +369,7 @@ namespace librealsense d500_device::d500_device( std::shared_ptr< const d500_info > const & dev_info ) : backend_device(dev_info), global_time_interface(), - d500_auto_calibrated(std::make_shared(this)), + d500_auto_calibrated(std::make_shared(this), this), _device_capabilities(ds::ds_caps::CAP_UNDEFINED), _depth_stream(new stream(RS2_STREAM_DEPTH)), _left_ir_stream(new stream(RS2_STREAM_INFRARED, 1)), diff --git a/src/ds/ds-calib-common.cpp b/src/ds/ds-calib-common.cpp index 39141c4441..c9455a0056 100644 --- a/src/ds/ds-calib-common.cpp +++ b/src/ds/ds-calib-common.cpp @@ -5,15 +5,93 @@ #include #include +#include +#include #include +#include +#include #include +#include #include #include namespace librealsense { + thermal_compensation_guard::thermal_compensation_guard( auto_calibrated_interface * handle ) : restart_tl(false), snr(nullptr) + { + if( Is< librealsense::device >( handle ) ) + { + try + { + snr = &( As< librealsense::device >( handle )->get_sensor( 0 ) ); // Depth sensor is assigned first by design + + if( snr->supports_option( RS2_OPTION_THERMAL_COMPENSATION ) ) + restart_tl = ( snr->get_option( RS2_OPTION_THERMAL_COMPENSATION ).query() != 0 ); + + if( restart_tl ) + { + snr->get_option( RS2_OPTION_THERMAL_COMPENSATION ).set( 0.f ); + std::this_thread::sleep_for( std::chrono::milliseconds( 100 ) ); // Allow for FW changes to propagate + } + } + catch( ... ) + { + LOG_WARNING( "Thermal Compensation guard failed to invoke" ); + } + } + } + + thermal_compensation_guard::~thermal_compensation_guard() + { + try + { + if (snr && restart_tl) + snr->get_option(RS2_OPTION_THERMAL_COMPENSATION).set(1.f); + } + catch (...) { + LOG_WARNING("Thermal Compensation guard failed to complete"); + } + } + + std::map< std::string, int > ds_calib_common::parse_json( const std::string & json_content ) + { + auto j = rsutils::json::parse( json_content ); + + std::map< std::string, int > values; + + for( auto it = j.begin(); it != j.end(); ++it ) + { + values[it.key()] = it.value(); + } + + return values; + } + + void ds_calib_common::update_value_if_exists( const std::map< std::string, int > & jsn, const std::string & key, int & value ) + { + auto it = jsn.find( key ); + if( it != jsn.end() ) + value = it->second; + } + + void ds_calib_common::check_params( int speed, int scan_parameter, int data_sampling ) + { + if( speed < ds_calib_common::SPEED_VERY_FAST || speed > ds_calib_common::SPEED_WHITE_WALL ) + throw invalid_value_exception( rsutils::string::from() + << "Auto calibration failed! Given value of 'speed' " + << speed << " is out of range (0 - 4)." ); + if( scan_parameter != ds_calib_common::PY_SCAN && scan_parameter != ds_calib_common::RX_SCAN ) + throw invalid_value_exception( rsutils::string::from() + << "Auto calibration failed! Given value of 'scan parameter' " + << scan_parameter << " is out of range (0 - 1)." ); + if( data_sampling != ds_calib_common::POLLING && data_sampling != ds_calib_common::INTERRUPT ) + throw invalid_value_exception( rsutils::string::from() + << "Auto calibration failed! Given value of 'data sampling' " + << data_sampling << " is out of range (0 - 1)." ); + } + void ds_calib_common::check_focal_length_params( int step_count, int fy_scan_range, int keep_new_value_after_sucessful_scan, @@ -226,4 +304,25 @@ namespace librealsense return ratio_to_apply; } + void ds_calib_common::handle_calibration_error( int status ) + { + switch( status ) + { + case ds_calib_common::STATUS_EDGE_TOO_CLOSE: + throw std::runtime_error( "Calibration didn't converge! - edges too close\n" + "Please retry in different lighting conditions" ); + case ds_calib_common::STATUS_FILL_FACTOR_TOO_LOW: + throw std::runtime_error( "Not enough depth pixels! - low fill factor)\n" + "Please retry in different lighting conditions" ); + case ds_calib_common::STATUS_NOT_CONVERGE: + throw std::runtime_error( "Calibration failed to converge\n" + "Please retry in different lighting conditions" ); + case ds_calib_common::STATUS_NO_DEPTH_AVERAGE: + throw std::runtime_error( "Calibration didn't converge! - no depth average\n" + "Please retry in different lighting conditions" ); + default: + throw std::runtime_error( rsutils::string::from() << + "Calibration didn't converge! (RESULT=" << int( status ) << ")" ); + } + } } // namespace librealsense diff --git a/src/ds/ds-calib-common.h b/src/ds/ds-calib-common.h index a8e6d5e035..e1833dbf03 100644 --- a/src/ds/ds-calib-common.h +++ b/src/ds/ds-calib-common.h @@ -5,11 +5,113 @@ #include +#include + namespace librealsense { + class auto_calibrated_interface; + class sensor_interface; + + // RAII to handle auto-calibration with the thermal compensation disabled + class thermal_compensation_guard + { + public: + thermal_compensation_guard( auto_calibrated_interface * handle ); + virtual ~thermal_compensation_guard(); + + protected: + bool restart_tl; + librealsense::sensor_interface* snr; + + private: + // Disable copy/assignment ctors + thermal_compensation_guard( const thermal_compensation_guard & ) = delete; + thermal_compensation_guard & operator=( const thermal_compensation_guard & ) = delete; + }; + class ds_calib_common { public: + enum auto_calib_speed : uint8_t + { + SPEED_VERY_FAST = 0, + SPEED_FAST = 1, + SPEED_MEDIUM = 2, + SPEED_SLOW = 3, + SPEED_WHITE_WALL = 4 + }; + + enum dsc_status : uint16_t + { + STATUS_SUCCESS = 0, + STATUS_RESULT_NOT_READY = 1, // Self calibration result is not ready yet + STATUS_FILL_FACTOR_TOO_LOW = 2, // There are too little textures in the scene + STATUS_EDGE_TOO_CLOSE = 3, // Self calibration range is too small + STATUS_NOT_CONVERGE = 4, // For tare calibration only + STATUS_BURN_SUCCESS = 5, + STATUS_BURN_ERROR = 6, + STATUS_NO_DEPTH_AVERAGE = 7 + }; + +#pragma pack( push, 1 ) + struct dsc_check_status_result + { + uint16_t status; + uint16_t stepCount; + uint16_t stepSize; // 1/1000 of a pixel + uint32_t pixelCountThreshold; // minimum number of pixels in selected bin + uint16_t minDepth; // Depth range for FWHM + uint16_t maxDepth; + uint32_t rightPy; // 1/1000000 of normalized unit + float healthCheck; + float rightRotation[9]; + }; + + struct dsc_result + { + uint16_t paramSize; + uint16_t status; + float healthCheck; + uint16_t tableSize; + }; +#pragma pack( pop ) + + enum data_sampling + { + POLLING = 0, + INTERRUPT = 1 + }; + + enum scan_parameter + { + PY_SCAN = 0, + RX_SCAN = 1 + }; + + enum auto_calib_sub_cmd : uint8_t + { + PY_RX_CALIB_CHECK_STATUS = 0X03, + INTERACTIVE_SCAN_CONTROL = 0X05, + PY_RX_CALIB_BEGIN = 0X08, + TARE_CALIB_BEGIN = 0X0B, + TARE_CALIB_CHECK_STATUS = 0X0C, + GET_CALIBRATION_RESULT = 0X0D, + FOCAL_LENGTH_CALIB_BEGIN = 0X11, + GET_FOCAL_LEGTH_CALIB_RESULT = 0X12, + PY_RX_PLUS_FL_CALIB_BEGIN = 0X13, + GET_PY_RX_PLUS_FL_CALIB_RESULT = 0X14, + SET_COEFFICIENTS = 0X19 + }; + + // Convert json to string-int pairs. + static std::map< std::string, int > parse_json( const std::string & json_content ); + + // If map contains requested key-value pair, update value. + static void update_value_if_exists( const std::map< std::string, int > & jsn, const std::string & key, int & value ); + + // Check validity of common parameters. + static void check_params( int speed, int scan_parameter, int data_sampling ); + // Checks validity of focal length caliration parameters. Throws on error. static void check_focal_length_params( int step_count, int fy_scan_range, @@ -38,5 +140,7 @@ namespace librealsense const float baseline, float & ratio, float & angle ); + + static void handle_calibration_error( int status ); }; } From 600391b0bbae50673e583fd38af79f2db17b5a22 Mon Sep 17 00:00:00 2001 From: ohadmeir Date: Tue, 10 Sep 2024 09:50:44 +0300 Subject: [PATCH 05/75] Tare implementation for D555 --- src/dds/rs-dds-device-proxy.cpp | 16 +- src/ds/advanced_mode/advanced_mode.cpp | 2 + src/ds/d400/d400-auto-calibration.cpp | 96 ++---------- src/ds/d400/d400-auto-calibration.h | 2 - src/ds/d500/d500-auto-calibration.cpp | 193 ++++++++++++++++++++++--- src/ds/d500/d500-auto-calibration.h | 8 +- src/ds/d500/d500-device.cpp | 2 + src/ds/ds-calib-common.cpp | 19 +++ src/ds/ds-calib-common.h | 58 +++++++- 9 files changed, 290 insertions(+), 106 deletions(-) diff --git a/src/dds/rs-dds-device-proxy.cpp b/src/dds/rs-dds-device-proxy.cpp index d52007eb9e..0f540a6650 100644 --- a/src/dds/rs-dds-device-proxy.cpp +++ b/src/dds/rs-dds-device-proxy.cpp @@ -390,11 +390,19 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const & } set_matcher_type( matcher ); - if (supports_info(RS2_CAMERA_INFO_PRODUCT_LINE) && - !strcmp(get_info(RS2_CAMERA_INFO_PRODUCT_LINE).c_str(), "D500")) + if( supports_info( RS2_CAMERA_INFO_PRODUCT_LINE ) + && ! strcmp( get_info( RS2_CAMERA_INFO_PRODUCT_LINE ).c_str(), "D500" ) ) + { + // Find depth sensor to pass into d500_auto_calibrated object + sensor_base * depth_sensor = nullptr; + for( auto & sensor : sensor_name_to_info ) + if( sensor.second.type == RS2_STREAM_DEPTH ) + depth_sensor = sensor.second.proxy.get(); + set_auto_calibration_capability( std::make_shared< d500_auto_calibrated >( - std::make_shared< d500_debug_protocol_calibration_engine >( this ), this ) ); - + std::make_shared< d500_debug_protocol_calibration_engine >( this ), this, depth_sensor ) ); + } + _calibration_changed_subscription = _dds_dev->on_calibration_changed( [this]( std::shared_ptr< const realdds::dds_stream > const & stream ) { diff --git a/src/ds/advanced_mode/advanced_mode.cpp b/src/ds/advanced_mode/advanced_mode.cpp index 728e106f86..5627f19c47 100644 --- a/src/ds/advanced_mode/advanced_mode.cpp +++ b/src/ds/advanced_mode/advanced_mode.cpp @@ -5,6 +5,7 @@ #include "json_loader.hpp" #include "ds/d400/d400-color.h" #include "ds/d500/d500-color.h" +#include "ds/d500/d500-private.h" #include #include @@ -117,6 +118,7 @@ namespace librealsense break; case ds::RS455_PID: case ds::RS457_PID: + case ds::D555_PID: default_450_mid_low_res(p); switch (res) { diff --git a/src/ds/d400/d400-auto-calibration.cpp b/src/ds/d400/d400-auto-calibration.cpp index 9f0261f72e..a1d5310aac 100644 --- a/src/ds/d400/d400-auto-calibration.cpp +++ b/src/ds/d400/d400-auto-calibration.cpp @@ -25,19 +25,6 @@ namespace librealsense { #pragma pack(push, 1) #pragma pack(1) - struct TareCalibrationResult - { - uint16_t status; // DscStatus - uint32_t tareDepth; // Tare depth in 1/100 of depth unit - uint32_t aveDepth; // Average depth in 1/100 of depth unit - int32_t curPx; // Current Px in 1/1000000 of normalized unit - int32_t calPx; // Calibrated Px in 1/1000000 of normalized unit - float curRightRotation[9]; // Current right rotation - float calRightRotation[9]; // Calibrated right rotation - uint16_t accuracyLevel; // [0-3] (Very High/High/Medium/Low) - uint16_t iterations; // Number of iterations it took to converge - }; - struct FocalLengthCalibrationResult { uint16_t status; // DscStatus @@ -77,46 +64,13 @@ namespace librealsense assistance_second_feed, }; - enum subpixel_accuracy - { - very_high = 0, //(0.025%) - high = 1, //(0.05%) - medium = 2, //(0.1%) - low = 3 //(0.2%) - }; - - struct tare_params3 - { - uint8_t average_step_count; - uint8_t step_count; - uint8_t accuracy; - uint8_t reserved; - }; - - struct params4 - { - int scan_parameter : 1; - int reserved : 2; - int data_sampling : 1; - }; - - union tare_calibration_params - { - tare_params3 param3_struct; - uint32_t param3; - }; - union param4 - { - params4 param4_struct; - uint32_t param_4; - }; const int DEFAULT_CALIB_TYPE = 0; const int DEFAULT_AVERAGE_STEP_COUNT = 20; const int DEFAULT_STEP_COUNT = 20; - const int DEFAULT_ACCURACY = subpixel_accuracy::medium; + const int DEFAULT_ACCURACY = ds_calib_common::ACCURACY_MEDIUM; const ds_calib_common::auto_calib_speed DEFAULT_SPEED = ds_calib_common::SPEED_SLOW; const int DEFAULT_SCAN = ds_calib_common::PY_SCAN; const int DEFAULT_SAMPLING = ds_calib_common::INTERRUPT; @@ -812,21 +766,21 @@ namespace librealsense } LOG_DEBUG("run_tare_calibration with parameters: speed = " << speed << " average_step_count = " << average_step_count << " step_count = " << step_count << " accuracy = " << accuracy << " scan_parameter = " << scan_parameter << " data_sampling = " << data_sampling); - check_tare_params(speed, scan_parameter, data_sampling, average_step_count, step_count, accuracy); + ds_calib_common::check_tare_params(speed, scan_parameter, data_sampling, average_step_count, step_count, accuracy); auto param2 = static_cast< uint32_t >( ground_truth_mm ) * 100; - tare_calibration_params param3{ static_cast< uint8_t >( average_step_count ), - static_cast< uint8_t >( step_count ), - static_cast< uint8_t >( accuracy ), - 0 }; + ds_calib_common::param3 p3{ static_cast< uint8_t >( average_step_count ), + static_cast< uint8_t >( step_count ), + static_cast< uint8_t >( accuracy ), + 0 }; - param4 param{ static_cast< uint8_t >( scan_parameter ), - 0, - static_cast< uint8_t >( data_sampling ) }; + ds_calib_common::param4 p4{ static_cast< uint8_t >( scan_parameter ), + 0, + static_cast< uint8_t >( data_sampling ) }; if (host_assistance != host_assistance_type::no_assistance) - param.param_4 |= (1 << 8); + p4.as_uint32 |= (1 << 8); // Log the current preset auto advanced_mode = dynamic_cast(this); @@ -837,12 +791,12 @@ namespace librealsense } if (depth == 0) - _hw_monitor->send(command{ ds::AUTO_CALIB, ds_calib_common::TARE_CALIB_BEGIN, param2, param3.param3, param.param_4 }); + _hw_monitor->send(command{ ds::AUTO_CALIB, ds_calib_common::TARE_CALIB_BEGIN, param2, p3.as_uint32, p4.as_uint32 }); } if (host_assistance == host_assistance_type::no_assistance || depth < 0) { - TareCalibrationResult result; + ds_calib_common::TareCalibrationResult result; // While not ready... int count = 0; @@ -852,21 +806,21 @@ namespace librealsense auto now = start; do { - memset(&result, 0, sizeof(TareCalibrationResult)); + memset(&result, 0, sizeof(ds_calib_common::TareCalibrationResult)); std::this_thread::sleep_for(std::chrono::milliseconds(200)); // Check calibration status try { res = _hw_monitor->send(command{ ds::AUTO_CALIB, ds_calib_common::TARE_CALIB_CHECK_STATUS }); - if (res.size() < sizeof(TareCalibrationResult)) + if (res.size() < sizeof(ds_calib_common::TareCalibrationResult)) { if (depth < 0) restore_preset(); throw std::runtime_error("Not enough data from CALIB_STATUS!"); } - result = *reinterpret_cast(res.data()); + result = *reinterpret_cast(res.data()); done = result.status != ds_calib_common::STATUS_RESULT_NOT_READY; } catch (const std::exception& ex) @@ -898,7 +852,7 @@ namespace librealsense auto status = (ds_calib_common::dsc_status)result.status; - uint8_t* p = res.data() + sizeof(TareCalibrationResult) + 2 * result.iterations * sizeof(uint32_t); + uint8_t* p = res.data() + sizeof(ds_calib_common::TareCalibrationResult) + 2 * result.iterations * sizeof(uint32_t); float* ph = reinterpret_cast(p); health[0] = ph[0]; health[1] = ph[1]; @@ -1497,24 +1451,6 @@ namespace librealsense _preset_change = false; } - void auto_calibrated::check_tare_params(int speed, int scan_parameter, int data_sampling, int average_step_count, int step_count, int accuracy) - { - ds_calib_common::check_params( speed, scan_parameter, data_sampling ); - - if (average_step_count < 1 || average_step_count > 30) - throw invalid_value_exception( rsutils::string::from() - << "Auto calibration failed! Given value of 'number of frames to average' " - << average_step_count << " is out of range (1 - 30)." ); - if (step_count < 5 || step_count > 30) - throw invalid_value_exception( rsutils::string::from() - << "Auto calibration failed! Given value of 'max iteration steps' " - << step_count << " is out of range (5 - 30)." ); - if (accuracy < very_high || accuracy > low) - throw invalid_value_exception( rsutils::string::from() - << "Auto calibration failed! Given value of 'subpixel accuracy' " << accuracy - << " is out of range (0 - 3)." ); - } - void auto_calibrated::check_one_button_params(int speed, int keep_new_value_after_sucessful_scan, int data_sampling, int adjust_both_sides, int fl_scan_location, int fy_scan_direction, int white_wall_mode) const { if( speed < ds_calib_common::SPEED_VERY_FAST || speed > ds_calib_common::SPEED_WHITE_WALL ) diff --git a/src/ds/d400/d400-auto-calibration.h b/src/ds/d400/d400-auto-calibration.h index 962a9ea087..c8560b631f 100644 --- a/src/ds/d400/d400-auto-calibration.h +++ b/src/ds/d400/d400-auto-calibration.h @@ -51,9 +51,7 @@ namespace librealsense std::vector get_calibration_results(float* const health = nullptr) const; std::vector get_PyRxFL_calibration_results(float* const health = nullptr, float* health_fl = nullptr) const; void handle_calibration_error(int status) const; - std::map parse_json(std::string json); std::shared_ptr< ds_advanced_mode_base> change_preset(); - void check_tare_params(int speed, int scan_parameter, int data_sampling, int average_step_count, int step_count, int accuracy); void check_one_button_params(int speed, int keep_new_value_after_sucessful_scan, int data_sampling, int adjust_both_sides, int fl_scan_location, int fy_scan_direction, int white_wall_mode) const; void undistort(uint8_t* img, const rs2_intrinsics& intrin, int roi_ws, int roi_hs, int roi_we, int roi_he); void change_preset_and_stay(); diff --git a/src/ds/d500/d500-auto-calibration.cpp b/src/ds/d500/d500-auto-calibration.cpp index 5109ec1b33..b8992a5ce0 100644 --- a/src/ds/d500/d500-auto-calibration.cpp +++ b/src/ds/d500/d500-auto-calibration.cpp @@ -4,11 +4,13 @@ #include "d500-auto-calibration.h" #include +#include +#include +#include +#include -#include #include -#include "d500-device.h" -#include +#include namespace librealsense { @@ -29,11 +31,13 @@ namespace librealsense }; d500_auto_calibrated::d500_auto_calibrated( std::shared_ptr< d500_debug_protocol_calibration_engine > calib_engine, - debug_interface * debug_dev ) + debug_interface * debug_dev, + sensor_base * ds ) : _calib_engine( calib_engine ) , _mode( calibration_mode::RESERVED ) , _state( calibration_state::IDLE ) , _result( calibration_result::UNKNOWN ) + , _depth_sensor( ds ) , _debug_dev( debug_dev ) { if( ! _debug_dev ) @@ -164,33 +168,31 @@ namespace librealsense LOG_DEBUG("run_on_chip_calibration with parameters: speed = " << speed << " scan_parameter = " << scan_parameter << " data_sampling = " << data_sampling); - uint32_t p4 = 0; - if (scan_parameter) - p4 |= 1; - if (data_sampling) - p4 |= (1 << 3); + ds_calib_common::param4 p4; + p4.scan_parameter = scan_parameter; + p4.data_sampling = data_sampling; // Begin auto-calibration - auto cmd = _debug_dev->build_command( ds::AUTO_CALIB, ds_calib_common::PY_RX_CALIB_BEGIN, speed, 0, p4 ); + auto cmd = _debug_dev->build_command( ds::AUTO_CALIB, ds_calib_common::PY_RX_CALIB_BEGIN, speed, 0, p4.as_uint32 ); std::vector< uint8_t > res = _debug_dev->send_receive_raw_data( cmd ); // TODO - need to remove res first 4 bytes? ds_calib_common::dsc_check_status_result result = get_calibration_status(timeout_ms, [progress_callback, speed](int count) { if( progress_callback ) - progress_callback->on_update_progress( count++ * ( 2.f * static_cast< int >( speed ) ) ); // currently this number does not reflect the actual progress + progress_callback->on_update_progress( count++ * ( 2.f * static_cast< int >( speed ) ) ); // Currently this number does not reflect the actual progress }); std::this_thread::sleep_for(std::chrono::milliseconds(100)); - auto status = (ds_calib_common::dsc_status)result.status; - // Handle errors from firmware - if (status != ds_calib_common::STATUS_SUCCESS) + if( result.status != ds_calib_common::STATUS_SUCCESS ) { - ds_calib_common::handle_calibration_error( status ); + ds_calib_common::handle_calibration_error( result.status ); } + + res = get_calibration_results(health); + if (progress_callback) progress_callback->on_update_progress(static_cast(100)); - res = get_calibration_results(health); return res; } @@ -287,7 +289,131 @@ namespace librealsense std::vector d500_auto_calibrated::run_tare_calibration(int timeout_ms, float ground_truth_mm, std::string json, float* const health, rs2_update_progress_callback_sptr progress_callback) { - throw not_implemented_exception(rsutils::string::from() << "Tare Calibration not applicable for this device"); + int average_step_count = 20; + int step_count = 20; + int accuracy = ds_calib_common::ACCURACY_MEDIUM; + int speed = ds_calib_common::SPEED_SLOW; + int scan_parameter = ds_calib_common::PY_SCAN; + int data_sampling = ds_calib_common::POLLING; + int apply_preset = 1; + + volatile thermal_compensation_guard grd( this ); // Enforce Thermal Compensation off during Tare calibration + + if( json.size() > 0 ) + { + auto jsn = ds_calib_common::parse_json( json ); + ds_calib_common::update_value_if_exists( jsn, "average step count", average_step_count ); + ds_calib_common::update_value_if_exists( jsn, "step count", step_count ); + ds_calib_common::update_value_if_exists( jsn, "accuracy", accuracy ); + ds_calib_common::update_value_if_exists( jsn, "speed", speed ); + ds_calib_common::update_value_if_exists( jsn, "scan parameter", scan_parameter ); + ds_calib_common::update_value_if_exists( jsn, "data sampling", data_sampling ); + ds_calib_common::update_value_if_exists( jsn, "apply preset", apply_preset ); + } + + std::shared_ptr< option > preset_recover; + if( apply_preset ) + { + preset_recover = change_preset(); + std::this_thread::sleep_for( std::chrono::milliseconds( 200 ) ); + } + + LOG_DEBUG( "run_tare_calibration with parameters: speed = " << speed + << " average_step_count = " << average_step_count << " step_count = " << step_count + << " accuracy = " << accuracy << " scan_parameter = " << scan_parameter + << " data_sampling = " << data_sampling ); + ds_calib_common::check_tare_params( speed, scan_parameter, data_sampling, average_step_count, step_count, accuracy ); + + auto p2 = static_cast< uint32_t >( ground_truth_mm ) * 100; + + ds_calib_common::param3 p3; + p3.average_step_count = average_step_count; + p3.step_count = step_count; + p3.accuracy = accuracy; + + ds_calib_common::param4 p4; + p4.scan_parameter = scan_parameter; + p4.data_sampling = data_sampling; + + // Log the current preset + //auto advanced_mode = dynamic_cast< ds_advanced_mode_base * >( this ); + //if( advanced_mode ) + //{ + // auto cur_preset = (rs2_rs400_visual_preset)(int)advanced_mode->_preset_opt->query(); + // LOG_DEBUG( + // "run_tare_calibration with preset: " << rs2_rs400_visual_preset_to_string( cur_preset ) ); + //} + + auto cmd = _debug_dev->build_command( ds::AUTO_CALIB, ds_calib_common::TARE_CALIB_BEGIN, p2, p3.as_uint32, p4.as_uint32 ); + _debug_dev->send_receive_raw_data( cmd ); // TODO - need to remove res first 4 bytes? + + ds_calib_common::TareCalibrationResult result; + + // While not ready... + int count = 0; + bool done = false; + + std::vector< uint8_t > res; + auto start = std::chrono::high_resolution_clock::now(); + auto now = start; + do + { + memset( &result, 0, sizeof( ds_calib_common::TareCalibrationResult ) ); + std::this_thread::sleep_for( std::chrono::milliseconds( 200 ) ); + + // Check calibration status + try + { + auto cmd = _debug_dev->build_command( ds::AUTO_CALIB, ds_calib_common::TARE_CALIB_CHECK_STATUS ); + res = _debug_dev->send_receive_raw_data( cmd ); // TODO - need to remove res first 4 bytes? + if( res.size() < sizeof( ds_calib_common::TareCalibrationResult ) ) + { + throw std::runtime_error( "Not enough data from CALIB_STATUS!" ); + } + + result = *reinterpret_cast< ds_calib_common::TareCalibrationResult * >( res.data() ); + done = result.status != ds_calib_common::STATUS_RESULT_NOT_READY; + } + catch( const std::exception & ex ) + { + LOG_INFO( ex.what() ); + } + + if( progress_callback ) + { + progress_callback->on_update_progress( count++ * ( 2.f * speed ) ); // Currently this number does not reflect the actual progress + } + + now = std::chrono::high_resolution_clock::now(); + } + while( now - start < std::chrono::milliseconds( timeout_ms ) && ! done ); + + // If we exit due to timeout, report timeout + if( ! done ) + { + throw std::runtime_error( "Operation timed-out!\nCalibration did not converge on time" ); + } + + uint8_t * p = res.data() + sizeof( ds_calib_common::TareCalibrationResult ) + 2 * result.iterations * sizeof( uint32_t ); + float * ph = reinterpret_cast< float * >( p ); + health[0] = ph[0]; + health[1] = ph[1]; + + LOG_INFO( "Ground truth: " << ground_truth_mm << "mm" ); + LOG_INFO( "Health check numbers from TareCalibrationResult(0x0C): before=" << ph[0] << ", after=" << ph[1] ); + LOG_INFO( "Z calculated from health check numbers : before=" + << ( ph[0] + 1 ) * ground_truth_mm << ", after=" << ( ph[1] + 1 ) * ground_truth_mm ); + + // Handle errors from firmware + if( result.status != ds_calib_common::STATUS_SUCCESS ) + ds_calib_common::handle_calibration_error( result.status ); + + res = get_calibration_results(); + + if( progress_callback ) + progress_callback->on_update_progress( static_cast< float >( 100 ) ); + + return res; } std::vector d500_auto_calibrated::process_calibration_frame(int timeout_ms, const rs2_frame* f, float* const health, rs2_update_progress_callback_sptr progress_callback) @@ -509,4 +635,37 @@ namespace librealsense return calib; } + + std::shared_ptr< option > d500_auto_calibrated::change_preset() + { + preset old_preset_values{}; + rs2_rs400_visual_preset old_preset = { RS2_RS400_VISUAL_PRESET_DEFAULT }; + + if( ! _depth_sensor ) + throw not_implemented_exception( " Depth sensor must be supplied to d500_auto_calibrated" ); + + if( _depth_sensor->supports_option( RS2_OPTION_VISUAL_PRESET ) ) + { + auto & opt = _depth_sensor->get_option( RS2_OPTION_VISUAL_PRESET ); + old_preset = static_cast< rs2_rs400_visual_preset >( opt.query() ); + //if( old_preset == RS2_RS400_VISUAL_PRESET_CUSTOM ) + // old_preset_values = advanced_mode->get_all(); + opt.set( RS2_RS400_VISUAL_PRESET_HIGH_ACCURACY ); + + std::shared_ptr< option > recover_option( &opt, [old_preset, old_preset_values]( option * opt ) + { + if( old_preset == RS2_RS400_VISUAL_PRESET_CUSTOM ) + { + opt->set( RS2_RS400_VISUAL_PRESET_CUSTOM ); + //adv->set_all( old_preset_values ); + } + else + opt->set( static_cast< float >( old_preset ) ); + }); + + return recover_option; + } + + return {}; + } } diff --git a/src/ds/d500/d500-auto-calibration.h b/src/ds/d500/d500-auto-calibration.h index f0a94addc3..99b5f108db 100644 --- a/src/ds/d500/d500-auto-calibration.h +++ b/src/ds/d500/d500-auto-calibration.h @@ -12,12 +12,15 @@ namespace librealsense { class debug_interface; class d500_debug_protocol_calibration_engine; + class ds_advanced_mode_base; + class sensor_base; class d500_auto_calibrated : public auto_calibrated_interface { public: d500_auto_calibrated( std::shared_ptr< d500_debug_protocol_calibration_engine > calib_engine, - debug_interface * debug_dev ); + debug_interface * debug_dev, + sensor_base * ds = nullptr ); void write_calibration() const override; std::vector run_on_chip_calibration(int timeout_ms, std::string json, float* const health, rs2_update_progress_callback_sptr progress_callback) override; std::vector run_tare_calibration(int timeout_ms, float ground_truth_mm, std::string json, float* const health, rs2_update_progress_callback_sptr progress_callback) override; @@ -33,6 +36,7 @@ namespace librealsense float target_width, float target_height, rs2_update_progress_callback_sptr progress_callback) override; std::string get_calibration_config() const override; void set_calibration_config(const std::string& calibration_config_json_str) const override; + void set_depth_sensor( sensor_base * ds ) { _depth_sensor = ds; } private: void check_preconditions_and_set_state(); @@ -47,12 +51,14 @@ namespace librealsense std::function< void( const int count ) > progress_func, bool wait_for_final_results = true ) const; std::vector< uint8_t > get_calibration_results( float * const health = nullptr ) const; + std::shared_ptr< option > change_preset(); mutable std::vector< uint8_t > _curr_calibration; std::shared_ptr _calib_engine; calibration_mode _mode; calibration_state _state; calibration_result _result; + sensor_base * _depth_sensor; debug_interface * _debug_dev; }; } diff --git a/src/ds/d500/d500-device.cpp b/src/ds/d500/d500-device.cpp index f8386aa7a9..b0cc12ef41 100644 --- a/src/ds/d500/d500-device.cpp +++ b/src/ds/d500/d500-device.cpp @@ -438,6 +438,8 @@ namespace librealsense auto& depth_sensor = get_depth_sensor(); auto raw_depth_sensor = get_raw_depth_sensor(); + d500_auto_calibrated::set_depth_sensor( &depth_sensor ); + using namespace platform; std::string pid_hex_str, usb_type_str; diff --git a/src/ds/ds-calib-common.cpp b/src/ds/ds-calib-common.cpp index c9455a0056..514f39df82 100644 --- a/src/ds/ds-calib-common.cpp +++ b/src/ds/ds-calib-common.cpp @@ -92,6 +92,25 @@ namespace librealsense << data_sampling << " is out of range (0 - 1)." ); } + void ds_calib_common::check_tare_params( int speed, int scan_parameter, int data_sampling, + int average_step_count, int step_count, int accuracy ) + { + check_params( speed, scan_parameter, data_sampling ); + + if( average_step_count < 1 || average_step_count > 30 ) + throw invalid_value_exception( rsutils::string::from() + << "Auto calibration failed! Given value of 'number of frames to average' " + << average_step_count << " is out of range (1 - 30)." ); + if( step_count < 5 || step_count > 30 ) + throw invalid_value_exception( rsutils::string::from() + << "Auto calibration failed! Given value of 'max iteration steps' " + << step_count << " is out of range (5 - 30)." ); + if( accuracy < ACCURACY_HIGH || accuracy > ACCURACY_LOW ) + throw invalid_value_exception( rsutils::string::from() + << "Auto calibration failed! Given value of 'subpixel accuracy' " << accuracy + << " is out of range (0 - 3)." ); + } + void ds_calib_common::check_focal_length_params( int step_count, int fy_scan_range, int keep_new_value_after_sucessful_scan, diff --git a/src/ds/ds-calib-common.h b/src/ds/ds-calib-common.h index e1833dbf03..c8e9cdf921 100644 --- a/src/ds/ds-calib-common.h +++ b/src/ds/ds-calib-common.h @@ -41,6 +41,14 @@ namespace librealsense SPEED_WHITE_WALL = 4 }; + enum subpixel_accuracy + { + ACCURACY_VERY_HIGH = 0, // 0.025% + ACCURACY_HIGH = 1, // 0.05% + ACCURACY_MEDIUM = 2, // 0.1% + ACCURACY_LOW = 3 // 0.2% + }; + enum dsc_status : uint16_t { STATUS_SUCCESS = 0, @@ -56,7 +64,7 @@ namespace librealsense #pragma pack( push, 1 ) struct dsc_check_status_result { - uint16_t status; + dsc_status status; uint16_t stepCount; uint16_t stepSize; // 1/1000 of a pixel uint32_t pixelCountThreshold; // minimum number of pixels in selected bin @@ -74,6 +82,19 @@ namespace librealsense float healthCheck; uint16_t tableSize; }; + + struct TareCalibrationResult + { + dsc_status status; + uint32_t tareDepth; // Tare depth in 1/100 of depth unit + uint32_t aveDepth; // Average depth in 1/100 of depth unit + int32_t curPx; // Current Px in 1/1000000 of normalized unit + int32_t calPx; // Calibrated Px in 1/1000000 of normalized unit + float curRightRotation[9]; // Current right rotation + float calRightRotation[9]; // Calibrated right rotation + uint16_t accuracyLevel; // [0-3] (Very High/High/Medium/Low) + uint16_t iterations; // Number of iterations it took to converge + }; #pragma pack( pop ) enum data_sampling @@ -103,15 +124,44 @@ namespace librealsense SET_COEFFICIENTS = 0X19 }; + union param3 + { + struct + { + uint8_t average_step_count; + uint8_t step_count; + uint8_t accuracy; + uint8_t reserved; + }; + + uint32_t as_uint32 = 0; + }; + + union param4 + { + struct + { + uint8_t scan_parameter : 1; + uint8_t reserved : 2; + uint8_t data_sampling : 1; + }; + + uint32_t as_uint32 = 0; + }; + // Convert json to string-int pairs. static std::map< std::string, int > parse_json( const std::string & json_content ); // If map contains requested key-value pair, update value. static void update_value_if_exists( const std::map< std::string, int > & jsn, const std::string & key, int & value ); - // Check validity of common parameters. + // Check validity of common parameters. Throws on error. static void check_params( int speed, int scan_parameter, int data_sampling ); + // Checks validity of tare caliration parameters. Throws on error. + static void check_tare_params( int speed, int scan_parameter, int data_sampling, + int average_step_count, int step_count, int accuracy ); + // Checks validity of focal length caliration parameters. Throws on error. static void check_focal_length_params( int step_count, int fy_scan_range, @@ -141,6 +191,10 @@ namespace librealsense float & ratio, float & angle ); + // Checks status and throws an appropriate error. static void handle_calibration_error( int status ); + + // Changes device visual preset, returned shared_ptr will restore previous preset values when released. + //static std::shared_ptr< ds_advanced_mode_base > change_preset( ds_advanced_mode_base * advanced_mode ); }; } From 6eb56d66c27df054e56eb933238b2d52697a6887 Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Thu, 19 Sep 2024 09:44:14 +0300 Subject: [PATCH 06/75] Update installation_jetson.md --- doc/installation_jetson.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/doc/installation_jetson.md b/doc/installation_jetson.md index fe0d930847..191e1ca76b 100644 --- a/doc/installation_jetson.md +++ b/doc/installation_jetson.md @@ -41,10 +41,10 @@ In order to enable the full capabilities of RealSense devices certain modificati NVIDIA's L4T delivers an Ubuntu-based distribution with a customized kernel based on version 4.9/5.10. The way the kernel is configured and deployed is different from a desktop Ubuntu image with two notable differences being the list of kernel modules included in default configuration and the way a new image is flashed. And while it is possible to rebuild and flash a new kernel image the procedure can be perceived as challenging and shall be performed with extra caution. -This guide comes with a script that allows to modify the kernel modules with Librealsense2-related patches without replacing the kernel image. The script has been verified with **Jetson AGX Xavier™** board using L4T versions 4.6.1 (Sept 2020) and 5.0.2. Scroll to the end of the guide for details. +This guide comes with a script that allows to modify the kernel modules with Librealsense2-related patches without replacing the kernel image. The script has been verified with **Jetson AGX Xavier™** board using L4T versions 5.0.2 (Sept 2020) and 6.0. Scroll to the end of the guide for details. ### 4. Install with Debian Packages - The minimum JetPack SDK required to run the precompiled Debians is [JetPack version 4.4.1](https://developer.nvidia.com/jetpack-sdk-441-archive) ( L4T 32.4.4 , CUDA version 10.2). + The minimum JetPack SDK required to run the precompiled Debians is [JetPack version 5.0.2](https://developer.nvidia.com/jetpack-sdk-441-archive) ( L4T 35.1 , CUDA version 11.4). Note that a lower version may not work due to non compatible CUDA versions limitation. @@ -92,7 +92,7 @@ You can also double-TAB after typing `rs-` to see the full list of SDK examples. ⮕ Use the V4L Native backend by applying the kernel patching -The method was verified with **Jetson AGX Orin™** with JetPack 6.0, **Jetson AGX Xavier™** boards with JetPack **4.4**[L4T 32.4.3], **4.6.1**[L4T 32.7.1] and **5.0.2**[L4T 35.1.0]. +The method was verified with **Jetson AGX Orin™** with JetPack 6.0, **Jetson AGX Xavier™** boards with JetPack **5.0.2**[L4T 35.1.0]. For **Jetson Nano™** setup, please see the following user instructions [NVIDIA Jetson Nano with Intel RealSense Depth Camera Using ROS2 Humble | by Kabilankb | May, 2024 | Medium](https://medium.com/@kabilankb2003/nvidia-jetson-nano-with-intel-realsense-depth-camera-using-ros2-humble-c5926566a4d8) From a0a8819747da7b65e54a19d56fd9277d8486e4fe Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Thu, 19 Sep 2024 15:49:26 +0300 Subject: [PATCH 07/75] Remove wrong date --- doc/installation_jetson.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/installation_jetson.md b/doc/installation_jetson.md index 191e1ca76b..28b954c50d 100644 --- a/doc/installation_jetson.md +++ b/doc/installation_jetson.md @@ -41,7 +41,7 @@ In order to enable the full capabilities of RealSense devices certain modificati NVIDIA's L4T delivers an Ubuntu-based distribution with a customized kernel based on version 4.9/5.10. The way the kernel is configured and deployed is different from a desktop Ubuntu image with two notable differences being the list of kernel modules included in default configuration and the way a new image is flashed. And while it is possible to rebuild and flash a new kernel image the procedure can be perceived as challenging and shall be performed with extra caution. -This guide comes with a script that allows to modify the kernel modules with Librealsense2-related patches without replacing the kernel image. The script has been verified with **Jetson AGX Xavier™** board using L4T versions 5.0.2 (Sept 2020) and 6.0. Scroll to the end of the guide for details. +This guide comes with a script that allows to modify the kernel modules with Librealsense2-related patches without replacing the kernel image. The script has been verified with **Jetson AGX Xavier™** board using L4T versions 5.0.2 and 6.0. Scroll to the end of the guide for details. ### 4. Install with Debian Packages The minimum JetPack SDK required to run the precompiled Debians is [JetPack version 5.0.2](https://developer.nvidia.com/jetpack-sdk-441-archive) ( L4T 35.1 , CUDA version 11.4). From 4feb8b169b05e2379e3649ed257eb49e11f7a84f Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Mon, 23 Sep 2024 16:33:45 +0300 Subject: [PATCH 08/75] check sensors exist --- src/ds/d500/d500-color.cpp | 5 +++++ src/ds/d500/d500-device.cpp | 9 ++++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/src/ds/d500/d500-color.cpp b/src/ds/d500/d500-color.cpp index 0d49268020..a9ccf23af8 100644 --- a/src/ds/d500/d500-color.cpp +++ b/src/ds/d500/d500-color.cpp @@ -63,6 +63,11 @@ namespace librealsense std::vector color_devs_info = filter_by_mi(group.uvc_devices, 3); + if ( color_devs_info.empty() ) + { + throw backend_exception("cannot access color sensor", RS2_EXCEPTION_TYPE_BACKEND); + } + std::unique_ptr< frame_timestamp_reader > ds_timestamp_reader_backup( new ds_timestamp_reader() ); std::unique_ptr ds_timestamp_reader_metadata(new ds_timestamp_reader_from_metadata(std::move(ds_timestamp_reader_backup))); diff --git a/src/ds/d500/d500-device.cpp b/src/ds/d500/d500-device.cpp index 9c38d0aeeb..faed9c8e7c 100644 --- a/src/ds/d500/d500-device.cpp +++ b/src/ds/d500/d500-device.cpp @@ -341,7 +341,14 @@ namespace librealsense using namespace ds; std::vector> depth_devices; - for( auto & info : filter_by_mi( all_device_infos, 0 ) ) // Filter just mi=0, DEPTH + auto depth_devs_info = filter_by_mi( all_device_infos, 0 ); + + if ( depth_devs_info.empty() ) + { + throw backend_exception("cannot access depth sensor", RS2_EXCEPTION_TYPE_BACKEND); + } + + for( auto & info : depth_devs_info ) // Filter just mi=0, DEPTH depth_devices.push_back( get_backend()->create_uvc_device( info ) ); std::unique_ptr< frame_timestamp_reader > timestamp_reader_backup( new ds_timestamp_reader() ); From 5da690ac11b77363cd9a6111ff43855d5c7621ba Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Mon, 23 Sep 2024 19:12:21 +0300 Subject: [PATCH 09/75] prtect against missing color sensor d400 --- src/ds/d400/d400-device.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/ds/d400/d400-device.cpp b/src/ds/d400/d400-device.cpp index 5dfc8232fc..184c51f2e4 100644 --- a/src/ds/d400/d400-device.cpp +++ b/src/ds/d400/d400-device.cpp @@ -478,7 +478,13 @@ namespace librealsense using namespace ds; std::vector> depth_devices; - for (auto&& info : filter_by_mi(all_device_infos, 0)) // Filter just mi=0, DEPTH + auto depth_devs_info = filter_by_mi( all_device_infos, 0 ); + + if ( depth_devs_info.empty() ) + { + throw backend_exception("cannot access depth sensor", RS2_EXCEPTION_TYPE_BACKEND); + } + for (auto&& info : depth_devs_info) // Filter just mi=0, DEPTH depth_devices.push_back( get_backend()->create_uvc_device( info ) ); std::unique_ptr< frame_timestamp_reader > timestamp_reader_backup( new ds_timestamp_reader() ); From d14f0cf034b0697405a57828d4204919737df6e3 Mon Sep 17 00:00:00 2001 From: Noy-Zini Date: Tue, 24 Sep 2024 09:52:54 +0300 Subject: [PATCH 10/75] replace plugins versions withe commit hash --- .github/workflows/build-ROS2-package-CI.yaml | 4 +-- .github/workflows/buildsCI.yaml | 28 ++++++++++---------- .github/workflows/static_analysis.yaml | 6 ++--- 3 files changed, 19 insertions(+), 19 deletions(-) diff --git a/.github/workflows/build-ROS2-package-CI.yaml b/.github/workflows/build-ROS2-package-CI.yaml index ac9358194f..18d511371d 100644 --- a/.github/workflows/build-ROS2-package-CI.yaml +++ b/.github/workflows/build-ROS2-package-CI.yaml @@ -43,12 +43,12 @@ jobs: steps: - name: setup ROS environment - uses: ros-tooling/setup-ros@v0.7 + uses: ros-tooling/setup-ros@a6ce30ecca1e5dcc10ae5e6a44fe2169115bf852 with: required-ros-distributions: ${{ matrix.ros_distribution }} - name: build librealsense ROS 2 - uses: ros-tooling/action-ros-ci@v0.3 + uses: ros-tooling/action-ros-ci@0c87ffc035492b66c9afb9159ca9664fb0b513e1 with: target-ros2-distro: ${{ matrix.ros_distribution }} skip-tests: true diff --git a/.github/workflows/buildsCI.yaml b/.github/workflows/buildsCI.yaml index bc46e5bed6..ea15391206 100644 --- a/.github/workflows/buildsCI.yaml +++ b/.github/workflows/buildsCI.yaml @@ -27,7 +27,7 @@ jobs: runs-on: windows-2019 timeout-minutes: 60 steps: - - uses: actions/checkout@v4 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 - name: Enable Long Paths shell: powershell @@ -69,7 +69,7 @@ jobs: runs-on: windows-2019 timeout-minutes: 60 steps: - - uses: actions/checkout@v4 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 - name: Enable Long Paths shell: powershell @@ -111,8 +111,8 @@ jobs: runs-on: windows-2019 timeout-minutes: 60 steps: - - uses: actions/checkout@v4 - - uses: actions/setup-python@v5 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 + - uses: actions/setup-python@f677139bbe7f9c59b41e40162b753c062f5d49a3 with: python-version: '3.8.1' @@ -171,8 +171,8 @@ jobs: runs-on: windows-2019 timeout-minutes: 60 steps: - - uses: actions/checkout@v4 - - uses: actions/setup-python@v5 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 + - uses: actions/setup-python@f677139bbe7f9c59b41e40162b753c062f5d49a3 with: python-version: '3.8.1' @@ -222,8 +222,8 @@ jobs: timeout-minutes: 60 steps: - - uses: actions/checkout@v4 - - uses: actions/setup-python@v5 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 + - uses: actions/setup-python@f677139bbe7f9c59b41e40162b753c062f5d49a3 with: python-version: '3.8.1' @@ -273,7 +273,7 @@ jobs: name: ${{ matrix.name }}_ST_Py_EX_CfU timeout-minutes: 60 steps: - - uses: actions/checkout@v4 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 - name: Prebuild shell: bash @@ -300,7 +300,7 @@ jobs: runs-on: ubuntu-20.04 timeout-minutes: 60 steps: - - uses: actions/checkout@v4 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 - name: Prebuild shell: bash @@ -343,7 +343,7 @@ jobs: runs-on: ubuntu-20.04 timeout-minutes: 60 steps: - - uses: actions/checkout@v4 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 - name: Prebuild shell: bash @@ -408,7 +408,7 @@ jobs: name: ${{ matrix.name }}_SH_Py_DDS_CI timeout-minutes: 60 steps: - - uses: actions/checkout@v4 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 - name: Prebuild shell: bash @@ -449,7 +449,7 @@ jobs: timeout-minutes: 60 steps: - - uses: actions/checkout@v4 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 - name: Check_API shell: bash @@ -483,7 +483,7 @@ jobs: runs-on: ubuntu-20.04 timeout-minutes: 60 steps: - - uses: actions/checkout@v4 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 - name: Check_API shell: bash diff --git a/.github/workflows/static_analysis.yaml b/.github/workflows/static_analysis.yaml index fb299cd34a..cb88dcc8c8 100644 --- a/.github/workflows/static_analysis.yaml +++ b/.github/workflows/static_analysis.yaml @@ -14,7 +14,7 @@ jobs: timeout-minutes: 30 runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@f43a0e5ff2bd294095638e18286ca9a3d1956744 - name: Install shell: bash @@ -64,7 +64,7 @@ jobs: && echo "No diffs found in cppcheck_run.parsed.log" - name: Upload logs - uses: actions/upload-artifact@v3 + uses: actions/upload-artifact@ff15f0306b3f739f7b6fd43fb5d26cd321bd4de5 with: name: cppcheck_log path: | @@ -105,7 +105,7 @@ jobs: runs-on: ubuntu-22.04 steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@f43a0e5ff2bd294095638e18286ca9a3d1956744 - name: "Install Dependencies" run: | From a82d0c92945061d96741bd3469ae27b3ee3def3a Mon Sep 17 00:00:00 2001 From: Noy-Zini Date: Tue, 24 Sep 2024 13:09:45 +0300 Subject: [PATCH 11/75] add version in comment --- .github/workflows/build-ROS2-package-CI.yaml | 4 +-- .github/workflows/buildsCI.yaml | 28 ++++++++++---------- .github/workflows/static_analysis.yaml | 6 ++--- 3 files changed, 19 insertions(+), 19 deletions(-) diff --git a/.github/workflows/build-ROS2-package-CI.yaml b/.github/workflows/build-ROS2-package-CI.yaml index 18d511371d..7a12741251 100644 --- a/.github/workflows/build-ROS2-package-CI.yaml +++ b/.github/workflows/build-ROS2-package-CI.yaml @@ -43,12 +43,12 @@ jobs: steps: - name: setup ROS environment - uses: ros-tooling/setup-ros@a6ce30ecca1e5dcc10ae5e6a44fe2169115bf852 + uses: ros-tooling/setup-ros@a6ce30ecca1e5dcc10ae5e6a44fe2169115bf852 #v0.7 with: required-ros-distributions: ${{ matrix.ros_distribution }} - name: build librealsense ROS 2 - uses: ros-tooling/action-ros-ci@0c87ffc035492b66c9afb9159ca9664fb0b513e1 + uses: ros-tooling/action-ros-ci@0c87ffc035492b66c9afb9159ca9664fb0b513e1 #v0.3 with: target-ros2-distro: ${{ matrix.ros_distribution }} skip-tests: true diff --git a/.github/workflows/buildsCI.yaml b/.github/workflows/buildsCI.yaml index ea15391206..66427ff4d9 100644 --- a/.github/workflows/buildsCI.yaml +++ b/.github/workflows/buildsCI.yaml @@ -27,7 +27,7 @@ jobs: runs-on: windows-2019 timeout-minutes: 60 steps: - - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4 - name: Enable Long Paths shell: powershell @@ -69,7 +69,7 @@ jobs: runs-on: windows-2019 timeout-minutes: 60 steps: - - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4 - name: Enable Long Paths shell: powershell @@ -111,8 +111,8 @@ jobs: runs-on: windows-2019 timeout-minutes: 60 steps: - - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 - - uses: actions/setup-python@f677139bbe7f9c59b41e40162b753c062f5d49a3 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4 + - uses: actions/setup-python@f677139bbe7f9c59b41e40162b753c062f5d49a3 #v5 with: python-version: '3.8.1' @@ -171,8 +171,8 @@ jobs: runs-on: windows-2019 timeout-minutes: 60 steps: - - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 - - uses: actions/setup-python@f677139bbe7f9c59b41e40162b753c062f5d49a3 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4 + - uses: actions/setup-python@f677139bbe7f9c59b41e40162b753c062f5d49a3 #v5 with: python-version: '3.8.1' @@ -222,8 +222,8 @@ jobs: timeout-minutes: 60 steps: - - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 - - uses: actions/setup-python@f677139bbe7f9c59b41e40162b753c062f5d49a3 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4 + - uses: actions/setup-python@f677139bbe7f9c59b41e40162b753c062f5d49a3 #v5 with: python-version: '3.8.1' @@ -273,7 +273,7 @@ jobs: name: ${{ matrix.name }}_ST_Py_EX_CfU timeout-minutes: 60 steps: - - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4 - name: Prebuild shell: bash @@ -300,7 +300,7 @@ jobs: runs-on: ubuntu-20.04 timeout-minutes: 60 steps: - - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4 - name: Prebuild shell: bash @@ -343,7 +343,7 @@ jobs: runs-on: ubuntu-20.04 timeout-minutes: 60 steps: - - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4 - name: Prebuild shell: bash @@ -408,7 +408,7 @@ jobs: name: ${{ matrix.name }}_SH_Py_DDS_CI timeout-minutes: 60 steps: - - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4 - name: Prebuild shell: bash @@ -449,7 +449,7 @@ jobs: timeout-minutes: 60 steps: - - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4 - name: Check_API shell: bash @@ -483,7 +483,7 @@ jobs: runs-on: ubuntu-20.04 timeout-minutes: 60 steps: - - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 + - uses: actions/checkout@692973e3d937129bcbf40652eb9f2f61becf3332 #v4 - name: Check_API shell: bash diff --git a/.github/workflows/static_analysis.yaml b/.github/workflows/static_analysis.yaml index cb88dcc8c8..b5840c5ab4 100644 --- a/.github/workflows/static_analysis.yaml +++ b/.github/workflows/static_analysis.yaml @@ -14,7 +14,7 @@ jobs: timeout-minutes: 30 runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@f43a0e5ff2bd294095638e18286ca9a3d1956744 + - uses: actions/checkout@f43a0e5ff2bd294095638e18286ca9a3d1956744 #v3 - name: Install shell: bash @@ -64,7 +64,7 @@ jobs: && echo "No diffs found in cppcheck_run.parsed.log" - name: Upload logs - uses: actions/upload-artifact@ff15f0306b3f739f7b6fd43fb5d26cd321bd4de5 + uses: actions/upload-artifact@ff15f0306b3f739f7b6fd43fb5d26cd321bd4de5 #v3 with: name: cppcheck_log path: | @@ -105,7 +105,7 @@ jobs: runs-on: ubuntu-22.04 steps: - name: Checkout - uses: actions/checkout@f43a0e5ff2bd294095638e18286ca9a3d1956744 + uses: actions/checkout@f43a0e5ff2bd294095638e18286ca9a3d1956744 #v3 - name: "Install Dependencies" run: | From b96296921c2899cd15fedc8c66cdcd4b0e938e36 Mon Sep 17 00:00:00 2001 From: ohadmeir Date: Mon, 16 Sep 2024 11:16:00 +0300 Subject: [PATCH 12/75] Using _debug_dev for ETH. get_calibration_table returns full table, header included. --- src/ds/d500/d500-auto-calibration.cpp | 90 +++++++++++-------- ...d500-debug-protocol-calibration-engine.cpp | 26 ++---- src/ds/ds-calib-common.h | 4 +- 3 files changed, 62 insertions(+), 58 deletions(-) diff --git a/src/ds/d500/d500-auto-calibration.cpp b/src/ds/d500/d500-auto-calibration.cpp index b8992a5ce0..3f01c36beb 100644 --- a/src/ds/d500/d500-auto-calibration.cpp +++ b/src/ds/d500/d500-auto-calibration.cpp @@ -14,6 +14,8 @@ namespace librealsense { + static constexpr const size_t hwm_header_size = 4; + static const std::string calibration_state_strings[] = { "Idle", "In Process", @@ -92,7 +94,7 @@ namespace librealsense rs2_update_progress_callback_sptr progress_callback ) { bool is_d555 = false; - auto dev = As< device >( this ); + 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" ) is_d555 = true; @@ -169,17 +171,17 @@ namespace librealsense LOG_DEBUG("run_on_chip_calibration with parameters: speed = " << speed << " scan_parameter = " << scan_parameter << " data_sampling = " << data_sampling); ds_calib_common::param4 p4; - p4.scan_parameter = scan_parameter; - p4.data_sampling = data_sampling; + p4.field.scan_parameter = scan_parameter; + p4.field.data_sampling = data_sampling; // Begin auto-calibration auto cmd = _debug_dev->build_command( ds::AUTO_CALIB, ds_calib_common::PY_RX_CALIB_BEGIN, speed, 0, p4.as_uint32 ); - std::vector< uint8_t > res = _debug_dev->send_receive_raw_data( cmd ); // TODO - need to remove res first 4 bytes? + _debug_dev->send_receive_raw_data( cmd ); ds_calib_common::dsc_check_status_result result = get_calibration_status(timeout_ms, [progress_callback, speed](int count) { if( progress_callback ) - progress_callback->on_update_progress( count++ * ( 2.f * static_cast< int >( speed ) ) ); // Currently this number does not reflect the actual progress + progress_callback->on_update_progress( count * speed * 1.2f ); // Currently this number does not reflect the actual progress }); std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -189,7 +191,7 @@ namespace librealsense ds_calib_common::handle_calibration_error( result.status ); } - res = get_calibration_results(health); + std::vector< uint8_t > res = get_calibration_results( health ); if (progress_callback) progress_callback->on_update_progress(static_cast(100)); @@ -327,13 +329,13 @@ namespace librealsense auto p2 = static_cast< uint32_t >( ground_truth_mm ) * 100; ds_calib_common::param3 p3; - p3.average_step_count = average_step_count; - p3.step_count = step_count; - p3.accuracy = accuracy; + p3.field.average_step_count = average_step_count; + p3.field.step_count = step_count; + p3.field.accuracy = accuracy; ds_calib_common::param4 p4; - p4.scan_parameter = scan_parameter; - p4.data_sampling = data_sampling; + p4.field.scan_parameter = scan_parameter; + p4.field.data_sampling = data_sampling; // Log the current preset //auto advanced_mode = dynamic_cast< ds_advanced_mode_base * >( this ); @@ -345,7 +347,7 @@ namespace librealsense //} auto cmd = _debug_dev->build_command( ds::AUTO_CALIB, ds_calib_common::TARE_CALIB_BEGIN, p2, p3.as_uint32, p4.as_uint32 ); - _debug_dev->send_receive_raw_data( cmd ); // TODO - need to remove res first 4 bytes? + _debug_dev->send_receive_raw_data( cmd ); ds_calib_common::TareCalibrationResult result; @@ -365,12 +367,13 @@ namespace librealsense try { auto cmd = _debug_dev->build_command( ds::AUTO_CALIB, ds_calib_common::TARE_CALIB_CHECK_STATUS ); - res = _debug_dev->send_receive_raw_data( cmd ); // TODO - need to remove res first 4 bytes? - if( res.size() < sizeof( ds_calib_common::TareCalibrationResult ) ) + res = _debug_dev->send_receive_raw_data( cmd ); + if( res.size() < sizeof( ds_calib_common::TareCalibrationResult ) + hwm_header_size ) { throw std::runtime_error( "Not enough data from CALIB_STATUS!" ); } + res.erase( res.begin(), res.begin() + hwm_header_size ); // Slicing HWM command header result = *reinterpret_cast< ds_calib_common::TareCalibrationResult * >( res.data() ); done = result.status != ds_calib_common::STATUS_RESULT_NOT_READY; } @@ -433,17 +436,28 @@ namespace librealsense void d500_auto_calibrated::set_calibration_table(const std::vector& calibration) { - if (_curr_calibration.size() != sizeof(ds::table_header) && // First time setting table, only header set by get_calibration_table - _curr_calibration.size() != sizeof(ds::d500_coefficients_table)) // Table was previously set - throw std::runtime_error(rsutils::string::from() << - "Current calibration table has unexpected size " << _curr_calibration.size()); - - if (calibration.size() != sizeof(ds::d500_coefficients_table) - sizeof(ds::table_header)) - throw std::runtime_error(rsutils::string::from() << - "Setting calibration table with unexpected size" << calibration.size()); - - _curr_calibration.resize(sizeof(ds::table_header)); // Remove previously set calibration, keep header. - _curr_calibration.insert(_curr_calibration.end(), calibration.begin(), calibration.end()); + auto table_header = reinterpret_cast< const ds::table_header * >( calibration.data() ); + + size_t expected_size = sizeof( ds::d500_coefficients_table ); + if( table_header->table_type == static_cast< uint16_t >( ds::d500_calibration_table_id::rgb_calibration_id ) ) + expected_size = sizeof( ds::d500_rgb_calibration_table ); + + if( calibration.size() != expected_size ) + throw std::runtime_error( rsutils::string::from() + << "Setting calibration table with unexpected size" << calibration.size() + << " while expecting " << expected_size ); + + _curr_calibration = calibration; + + // Send updated calibration to RAM so it can be used. Will be saved only on write_calibration() + auto cmd = _debug_dev->build_command( ds::SET_HKR_CONFIG_TABLE, + static_cast< int >( ds::d500_calib_location::d500_calib_ram_memory ), + static_cast< int >( table_header->table_type ), + static_cast< int >( ds::d500_calib_type::d500_calib_dynamic ), + 0, + _curr_calibration.data(), + _curr_calibration.size() ); + _debug_dev->send_receive_raw_data( cmd ); } void d500_auto_calibrated::reset_to_factory_calibration() const @@ -472,10 +486,8 @@ namespace librealsense std::vector< uint8_t > ret; const float correction_factor = 0.5f; - auto table_data = get_calibration_table(); // Table is returned without the header - auto full_table = _curr_calibration; // During get_calibration_table header is saved in _curr_calibration - full_table.insert( full_table.end(), table_data.begin(), table_data.end() ); - auto table = reinterpret_cast< librealsense::ds::d500_coefficients_table * >( full_table.data() ); + auto calib_table = get_calibration_table(); + auto table = reinterpret_cast< librealsense::ds::d500_coefficients_table * >( calib_table.data() ); float ratio_to_apply = ds_calib_common::get_focal_length_correction_factor( left_rect_sides, right_rect_sides, @@ -501,9 +513,7 @@ namespace librealsense table->right_coefficients_table.base_instrinsics.fy *= ratio_to_apply; } - //Return data without header - table_data.assign( full_table.begin() + sizeof( ds::table_header ), full_table.end() ); - return table_data; + return calib_table; } std::vector d500_auto_calibrated::run_uv_map_calibration(rs2_frame_queue* left, rs2_frame_queue* color, rs2_frame_queue* depth, int py_px_only, @@ -579,14 +589,15 @@ namespace librealsense { // Check calibration status auto cmd = _debug_dev->build_command( ds::AUTO_CALIB, ds_calib_common::PY_RX_CALIB_CHECK_STATUS ); - auto res = _debug_dev->send_receive_raw_data( cmd ); // TODO - need to remove res first 4 bytes? - if( res.size() < sizeof( ds_calib_common::dsc_check_status_result ) ) + auto res = _debug_dev->send_receive_raw_data( cmd ); + if( res.size() < sizeof( ds_calib_common::dsc_check_status_result ) + hwm_header_size ) { if( ! ( ( retries++ ) % 5 ) ) // Add log debug once a sec LOG_DEBUG( "Not enough data from CALIB_STATUS!" ); } else { + res.erase( res.begin(), res.begin() + hwm_header_size ); // Slicing HWM command header result = *reinterpret_cast< ds_calib_common::dsc_check_status_result * >( res.data() ); done = ! wait_for_final_results || result.status != ds_calib_common::STATUS_RESULT_NOT_READY; } @@ -598,7 +609,7 @@ namespace librealsense if( progress_func ) { - progress_func( count ); + progress_func( count++ ); } now = std::chrono::high_resolution_clock::now(); @@ -617,10 +628,11 @@ namespace librealsense { // Get new calibration from the firmware auto cmd = _debug_dev->build_command( ds::AUTO_CALIB, ds_calib_common::GET_CALIBRATION_RESULT ); - auto res = _debug_dev->send_receive_raw_data( cmd ); // TODO - need to remove res first 4 bytes? - if( res.size() < sizeof( ds_calib_common::dsc_result ) ) + auto res = _debug_dev->send_receive_raw_data( cmd ); + if( res.size() < sizeof( ds_calib_common::dsc_result ) + hwm_header_size ) throw std::runtime_error( "Not enough data from CALIB_STATUS!" ); + res.erase( res.begin(), res.begin() + hwm_header_size ); // Slicing HWM command header auto * header = reinterpret_cast< ds::table_header * >( res.data() + sizeof( ds_calib_common::dsc_result ) ); if( res.size() < sizeof( ds_calib_common::dsc_result ) + sizeof( ds::table_header ) + header->table_size ) throw std::runtime_error( "Table truncated in CALIB_STATUS!" ); @@ -629,9 +641,9 @@ namespace librealsense calib.resize( sizeof( ds::table_header ) + header->table_size, 0 ); memcpy( calib.data(), header, calib.size() ); // Copy to new_calib - auto reslt = reinterpret_cast< ds_calib_common::dsc_result * >( res.data() ); + auto result = reinterpret_cast< ds_calib_common::dsc_result * >( res.data() ); if( health ) - *health = reslt->healthCheck; + *health = result->healthCheck; return calib; } diff --git a/src/ds/d500/d500-debug-protocol-calibration-engine.cpp b/src/ds/d500/d500-debug-protocol-calibration-engine.cpp index accdbf0f88..66360f824d 100644 --- a/src/ds/d500/d500-debug-protocol-calibration-engine.cpp +++ b/src/ds/d500/d500-debug-protocol-calibration-engine.cpp @@ -76,14 +76,13 @@ int8_t d500_debug_protocol_calibration_engine::get_triggered_calibration_progres std::vector d500_debug_protocol_calibration_engine::get_calibration_table(std::vector& current_calibration) const { // Getting depth calibration table. RGB table is currently not supported by auto_calibrated_interface API - std::vector< uint8_t > res; // prepare command using namespace ds; auto cmd = _dev->build_command(ds::GET_HKR_CONFIG_TABLE, - static_cast(d500_calib_location::d500_calib_flash_memory), - static_cast(d500_calibration_table_id::depth_calibration_id), - static_cast(ds::d500_calib_type::d500_calib_dynamic)); + static_cast(d500_calib_location::d500_calib_flash_memory), + static_cast(d500_calibration_table_id::depth_calibration_id), + static_cast(ds::d500_calib_type::d500_calib_dynamic)); // sending command auto calib = _dev->send_receive_raw_data(cmd); @@ -95,32 +94,25 @@ std::vector d500_debug_protocol_calibration_engine::get_calibration_tab calib.erase(calib.begin(), calib.begin() + 4); auto header = (ds::table_header*)(calib.data()); - if (calib.size() < sizeof(ds::table_header) + header->table_size) throw std::runtime_error("GET_HKR_CONFIG_TABLE response is smaller then expected table size!"); - // Backwards compatibility dictates that we will return the table without the header, but we need the header - // details like versions to later set back the table. Save it at the start of _curr_calibration. - current_calibration.assign(calib.begin(), calib.begin() + sizeof(ds::table_header)); - - res.assign(calib.begin() + sizeof(ds::table_header), calib.end()); - - return res; + return calib; } void d500_debug_protocol_calibration_engine::write_calibration(std::vector& current_calibration) const { auto table_header = reinterpret_cast(current_calibration.data()); table_header->crc32 = rsutils::number::calc_crc32(current_calibration.data() + sizeof(ds::table_header), - current_calibration.size() - sizeof(ds::table_header)); + current_calibration.size() - sizeof(ds::table_header)); // prepare command using namespace ds; auto cmd = _dev->build_command(ds::SET_HKR_CONFIG_TABLE, - static_cast(ds::d500_calib_location::d500_calib_flash_memory), - static_cast(table_header->table_type), - static_cast(ds::d500_calib_type::d500_calib_dynamic), 0, - current_calibration.data(), current_calibration.size()); + static_cast(ds::d500_calib_location::d500_calib_flash_memory), + static_cast(table_header->table_type), + static_cast(ds::d500_calib_type::d500_calib_dynamic), 0, + current_calibration.data(), current_calibration.size()); // sending command _dev->send_receive_raw_data(cmd); diff --git a/src/ds/ds-calib-common.h b/src/ds/ds-calib-common.h index c8e9cdf921..e9301ba22e 100644 --- a/src/ds/ds-calib-common.h +++ b/src/ds/ds-calib-common.h @@ -132,7 +132,7 @@ namespace librealsense uint8_t step_count; uint8_t accuracy; uint8_t reserved; - }; + } field; uint32_t as_uint32 = 0; }; @@ -144,7 +144,7 @@ namespace librealsense uint8_t scan_parameter : 1; uint8_t reserved : 2; uint8_t data_sampling : 1; - }; + } field; uint32_t as_uint32 = 0; }; From 7798bcacf0094ef3eaf4ee8fecf67e5c00d510f4 Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Wed, 25 Sep 2024 14:16:25 +0300 Subject: [PATCH 13/75] add SDLE flags for windows build --- CMake/windows_config.cmake | 28 +++++++++++++++++++++++++ include/librealsense2/hpp/rs_export.hpp | 2 +- 2 files changed, 29 insertions(+), 1 deletion(-) diff --git a/CMake/windows_config.cmake b/CMake/windows_config.cmake index 3bd808b07a..f81775a913 100644 --- a/CMake/windows_config.cmake +++ b/CMake/windows_config.cmake @@ -40,6 +40,34 @@ macro(os_set_flags) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /MP") + ############### + # According to SDLE we need to add the following flags for additional security: + # Debug & Release: + # /Gy: Enables function-level linking to reduce executable size. + # /DYNAMICBASE: Enables Address Space Layout Randomization (ASLR) to improve security. + # /GS: Enables buffer security checks to prevent buffer overflows. + + # Release only: + # /WX: Treats all warnings as errors. + # /LTCG (/GL): Enables Link Time Code Generation to improve performance. + # /sdl: Enables additional security checks. + # /NXCOMPAT: Enables Data Execution Prevention (DEP) to prevent code execution in data areas. + + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Gy /DYNAMICBASE /GS") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /Gy /DYNAMICBASE /GS") + + if(CMAKE_BUILD_TYPE STREQUAL "Debug") + message(STATUS "Configuring for Debug build") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") + else() # Release, RelWithDebInfo, or multi configuration generator is being used (aka not specifing build type, or building with VS) + message(STATUS "Configuring for Release build") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /WX /sdl") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /WX /sdl") + set(CMAKE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS} /INCREMENTAL:NO /LTCG /NXCOMPAT") # ignoring '/INCREMENTAL' due to '/LTCG' specification + endif() + + ################# + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /bigobj /wd4819") set(LRS_TRY_USE_AVX true) add_definitions(-D_UNICODE) diff --git a/include/librealsense2/hpp/rs_export.hpp b/include/librealsense2/hpp/rs_export.hpp index 6e7d097791..337d34bc61 100644 --- a/include/librealsense2/hpp/rs_export.hpp +++ b/include/librealsense2/hpp/rs_export.hpp @@ -81,7 +81,7 @@ namespace rs2 bool use_normals = get_option(OPTION_PLY_NORMALS) != 0; const auto verts = p.get_vertices(); const auto texcoords = p.get_texture_coordinates(); - const uint8_t* texture_data; + const uint8_t* texture_data = nullptr; if (use_texcoords) // texture might be on the gpu, get pointer to data before for-loop to avoid repeated access texture_data = reinterpret_cast(color.get_data()); std::vector new_verts; From bdb030497b468f4b198756d81288f1ab87949ab8 Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Wed, 25 Sep 2024 14:17:04 +0300 Subject: [PATCH 14/75] add SDLE flags for linux build --- CMake/unix_config.cmake | 38 ++++++++++++++++++++++++++++++++++++++ CMake/windows_config.cmake | 3 +++ 2 files changed, 41 insertions(+) diff --git a/CMake/unix_config.cmake b/CMake/unix_config.cmake index 711434c2ef..bf7155b908 100644 --- a/CMake/unix_config.cmake +++ b/CMake/unix_config.cmake @@ -47,6 +47,44 @@ macro(os_set_flags) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") endif() + ############### + # According to SDLE we need to add the following flags for additional security: + # Debug & Release: + # -Wformat: Checks for format string vulnerabilities. + # -Wformat-security: Ensures format strings are not vulnerable to attacks. + # -fPIC: Generates position-independent code (PIC) suitable for shared libraries. + # -fPIE: Generates position-independent executable (PIE) code. + # -pie: Links the output as a position-independent executable. + # -D_FORTIFY_SOURCE=2: Adds extra checks for buffer overflows. + # -mfunction-return=thunk: Mitigates return-oriented programming (ROP) attacks. (Added flag -fcf-protection=none to allow it) + # -mindirect-branch=thunk: Mitigates indirect branch attacks. + # -mindirect-branch-register: Uses registers for indirect branches to mitigate attacks. + # -fstack-protector: Adds stack protection to detect buffer overflows. + + # Release only + # -Werror: Treats all warnings as errors. + # -Werror=format-security: Treats format security warnings as errors. + # -z noexecstack: Marks the stack as non-executable to prevent certain types of attacks. + # -Wl,-z,relro,-z,now: Enables read-only relocations and immediate binding for security. + # -fstack-protector-strong: Provides stronger stack protection than -fstack-protector. + + # see https://readthedocs.intel.com/SecureCodingStandards/2023.Q2.0/compiler/c-cpp/ for more details + + + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wformat -Wformat-security -fPIC -fPIE -pie -D_FORTIFY_SOURCE=2 -fcf-protection=none -mfunction-return=thunk -mindirect-branch=thunk -mindirect-branch-register -fstack-protector") + #‘-mfunction-return’ and ‘-fcf-protection’ are not compatible, so specifing -fcf-protection=none + set(CMAKE_C_FLAGS "${CMAKE_CXX_FLAGS} -Werror -Werror=format-security -z noexecstack -Wl,-z,relro,-z,now -fstack-protector-strong") + + if(CMAKE_BUILD_TYPE STREQUAL "Debug") + message(STATUS "Configuring for Debug build") + else() # Release, RelWithDebInfo, or multi configuration generator is being used (aka not specifing build type, or building with VS) + message(STATUS "Configuring for Release build") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror -Werror=format-security -z noexecstack -Wl,-z,relro,-z,now -fstack-protector-strong") + set(CMAKE_C_FLAGS "${CMAKE_CXX_FLAGS} -Werror -Werror=format-security -z noexecstack -Wl,-z,relro,-z,now -fstack-protector-strong") + endif() + + ################# + if(APPLE) set(FORCE_RSUSB_BACKEND ON) endif() diff --git a/CMake/windows_config.cmake b/CMake/windows_config.cmake index f81775a913..93762b8073 100644 --- a/CMake/windows_config.cmake +++ b/CMake/windows_config.cmake @@ -53,6 +53,9 @@ macro(os_set_flags) # /sdl: Enables additional security checks. # /NXCOMPAT: Enables Data Execution Prevention (DEP) to prevent code execution in data areas. + # see https://readthedocs.intel.com/SecureCodingStandards/2023.Q2.0/compiler/c-cpp/ for more details + + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Gy /DYNAMICBASE /GS") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /Gy /DYNAMICBASE /GS") From 6035a790ab858fdd50aa32c16f6d115cb3af0dcc Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Wed, 25 Sep 2024 14:23:09 +0300 Subject: [PATCH 15/75] fix for C flags --- CMake/unix_config.cmake | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMake/unix_config.cmake b/CMake/unix_config.cmake index bf7155b908..71d846f478 100644 --- a/CMake/unix_config.cmake +++ b/CMake/unix_config.cmake @@ -73,14 +73,14 @@ macro(os_set_flags) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wformat -Wformat-security -fPIC -fPIE -pie -D_FORTIFY_SOURCE=2 -fcf-protection=none -mfunction-return=thunk -mindirect-branch=thunk -mindirect-branch-register -fstack-protector") #‘-mfunction-return’ and ‘-fcf-protection’ are not compatible, so specifing -fcf-protection=none - set(CMAKE_C_FLAGS "${CMAKE_CXX_FLAGS} -Werror -Werror=format-security -z noexecstack -Wl,-z,relro,-z,now -fstack-protector-strong") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wformat -Wformat-security -fPIC -fPIE -pie -D_FORTIFY_SOURCE=2 -fcf-protection=none -mfunction-return=thunk -mindirect-branch=thunk -mindirect-branch-register -fstack-protector") if(CMAKE_BUILD_TYPE STREQUAL "Debug") message(STATUS "Configuring for Debug build") else() # Release, RelWithDebInfo, or multi configuration generator is being used (aka not specifing build type, or building with VS) message(STATUS "Configuring for Release build") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror -Werror=format-security -z noexecstack -Wl,-z,relro,-z,now -fstack-protector-strong") - set(CMAKE_C_FLAGS "${CMAKE_CXX_FLAGS} -Werror -Werror=format-security -z noexecstack -Wl,-z,relro,-z,now -fstack-protector-strong") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Werror -Werror=format-security -z noexecstack -Wl,-z,relro,-z,now -fstack-protector-strong") endif() ################# From 86aafea15655a47580fbcb88ce99469d573e4db8 Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Wed, 25 Sep 2024 17:07:56 +0300 Subject: [PATCH 16/75] modify Jetson and fPIE flag --- CMake/unix_config.cmake | 12 +++++++++--- examples/CMakeLists.txt | 4 ++++ tools/CMakeLists.txt | 4 ++++ 3 files changed, 17 insertions(+), 3 deletions(-) diff --git a/CMake/unix_config.cmake b/CMake/unix_config.cmake index 71d846f478..9e8d9a63b4 100644 --- a/CMake/unix_config.cmake +++ b/CMake/unix_config.cmake @@ -70,10 +70,16 @@ macro(os_set_flags) # see https://readthedocs.intel.com/SecureCodingStandards/2023.Q2.0/compiler/c-cpp/ for more details + if (CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64|armv7l") # Jetson system, some flags are not recognized + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wformat -Wformat-security -fPIC -D_FORTIFY_SOURCE=2 -fstack-protector") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wformat -Wformat-security -fPIC -D_FORTIFY_SOURCE=2 -fstack-protector") + else() + #‘-mfunction-return’ and ‘-fcf-protection’ are not compatible, so specifing -fcf-protection=none + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wformat -Wformat-security -fPIC -D_FORTIFY_SOURCE=2 -fcf-protection=none -mfunction-return=thunk -mindirect-branch=thunk -mindirect-branch-register -fstack-protector") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wformat -Wformat-security -fPIC -D_FORTIFY_SOURCE=2 -fcf-protection=none -mfunction-return=thunk -mindirect-branch=thunk -mindirect-branch-register -fstack-protector") + endif() + set(CMAKE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS} -pie") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wformat -Wformat-security -fPIC -fPIE -pie -D_FORTIFY_SOURCE=2 -fcf-protection=none -mfunction-return=thunk -mindirect-branch=thunk -mindirect-branch-register -fstack-protector") - #‘-mfunction-return’ and ‘-fcf-protection’ are not compatible, so specifing -fcf-protection=none - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wformat -Wformat-security -fPIC -fPIE -pie -D_FORTIFY_SOURCE=2 -fcf-protection=none -mfunction-return=thunk -mindirect-branch=thunk -mindirect-branch-register -fstack-protector") if(CMAKE_BUILD_TYPE STREQUAL "Debug") message(STATUS "Configuring for Debug build") diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 652656e589..b8ee580d08 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -7,6 +7,8 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS 1) # View the makefile commands during build #set(CMAKE_VERBOSE_MAKEFILE on) +string(REPLACE "-fPIC" "-fPIE" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") # examples are exeutables so we want position indepandent exeutables and not libraries + set( DEPENDENCIES ${LRS_TARGET} ) if(BUILD_GRAPHICAL_EXAMPLES) include(${CMAKE_SOURCE_DIR}/CMake/opengl_config.cmake) @@ -40,3 +42,5 @@ add_subdirectory(record-playback) add_subdirectory(motion) add_subdirectory(gl) add_subdirectory(hdr) + +string(REPLACE "-fPIE" "-fPIC" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index 99807a4602..bca5cdf6f9 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -7,6 +7,8 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS 1) # View the makefile commands during build #set(CMAKE_VERBOSE_MAKEFILE on) +string(REPLACE "-fPIC" "-fPIE" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") # tools are exeutables so we want position indepandent exeutables and not libraries + list( APPEND DEPENDENCIES ${LRS_TARGET} tclap ) if(BUILD_TOOLS) @@ -45,3 +47,5 @@ if(BUILD_EXAMPLES) endif() endif() endif() + +string(REPLACE "-fPIE" "-fPIC" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") From 8d8daa9deda9e83cd58f418516f70b38da69a59f Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Thu, 26 Sep 2024 12:42:11 +0300 Subject: [PATCH 17/75] fix libcurl not compiling and other warnings --- CMake/external_libcurl.cmake | 5 +++++ CMake/unix_config.cmake | 4 ++-- CMake/windows_config.cmake | 4 ++-- src/hid/hid-device.cpp | 2 +- src/uvc/uvc-device.cpp | 6 +++--- src/uvc/uvc-streamer.cpp | 2 +- 6 files changed, 14 insertions(+), 9 deletions(-) diff --git a/CMake/external_libcurl.cmake b/CMake/external_libcurl.cmake index ac967e1cd2..2fce83548d 100644 --- a/CMake/external_libcurl.cmake +++ b/CMake/external_libcurl.cmake @@ -1,4 +1,8 @@ if(CHECK_FOR_UPDATES) + + set(FLAGS_ORIGIN "${CMAKE_C_FLAGS}") + string(REPLACE "-Werror" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") # -Werror causes libcurl build to fail + include(ExternalProject) message(STATUS "Building libcurl enabled") @@ -60,4 +64,5 @@ if(CHECK_FOR_UPDATES) endif() endif() + set(CMAKE_C_FLAGS "${FLAGS_ORIGIN}") endif() #CHECK_FOR_UPDATES diff --git a/CMake/unix_config.cmake b/CMake/unix_config.cmake index 9e8d9a63b4..dbb4cc9a10 100644 --- a/CMake/unix_config.cmake +++ b/CMake/unix_config.cmake @@ -85,8 +85,8 @@ macro(os_set_flags) message(STATUS "Configuring for Debug build") else() # Release, RelWithDebInfo, or multi configuration generator is being used (aka not specifing build type, or building with VS) message(STATUS "Configuring for Release build") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror -Werror=format-security -z noexecstack -Wl,-z,relro,-z,now -fstack-protector-strong") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Werror -Werror=format-security -z noexecstack -Wl,-z,relro,-z,now -fstack-protector-strong") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror -z noexecstack -Wl,-z,relro,-z,now -fstack-protector-strong") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Werror -z noexecstack -Wl,-z,relro,-z,now -fstack-protector-strong") endif() ################# diff --git a/CMake/windows_config.cmake b/CMake/windows_config.cmake index 93762b8073..614c7e9f96 100644 --- a/CMake/windows_config.cmake +++ b/CMake/windows_config.cmake @@ -56,8 +56,8 @@ macro(os_set_flags) # see https://readthedocs.intel.com/SecureCodingStandards/2023.Q2.0/compiler/c-cpp/ for more details - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Gy /DYNAMICBASE /GS") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /Gy /DYNAMICBASE /GS") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Gy /DYNAMICBASE /GS /wd4101") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /Gy /DYNAMICBASE /GS /wd4101") if(CMAKE_BUILD_TYPE STREQUAL "Debug") message(STATUS "Configuring for Debug build") diff --git a/src/hid/hid-device.cpp b/src/hid/hid-device.cpp index aaa3a18a40..c903016967 100644 --- a/src/hid/hid-device.cpp +++ b/src/hid/hid-device.cpp @@ -334,7 +334,7 @@ namespace librealsense //we want to change the sensitivity values only in gyro, for FW version >= 5.16 if( featureReport.reportId == REPORT_ID_GYROMETER_3D && _realsense_hid_report_actual_size == sizeof( REALSENSE_HID_REPORT ) ) - featureReport.sensitivity = sensitivity; + featureReport.sensitivity = (unsigned short)sensitivity; res = dev->control_transfer(USB_REQUEST_CODE_SET, diff --git a/src/uvc/uvc-device.cpp b/src/uvc/uvc-device.cpp index 84897e0163..765d72210b 100644 --- a/src/uvc/uvc-device.cpp +++ b/src/uvc/uvc-device.cpp @@ -177,7 +177,7 @@ namespace librealsense switch(state) { case D0: - _messenger = _usb_device->open(_info.mi); + _messenger = _usb_device->open((uint8_t)_info.mi); if (_messenger) { try{ @@ -654,7 +654,7 @@ namespace librealsense void rs_uvc_device::listen_to_interrupts() { - auto ctrl_interface = _usb_device->get_interface(_info.mi); + auto ctrl_interface = _usb_device->get_interface((uint8_t)_info.mi); if (!ctrl_interface) return; auto iep = ctrl_interface->first_endpoint(RS2_USB_ENDPOINT_DIRECTION_READ, RS2_USB_ENDPOINT_INTERRUPT); @@ -856,7 +856,7 @@ namespace librealsense req, probe ? (UVC_VS_PROBE_CONTROL << 8) : (UVC_VS_COMMIT_CONTROL << 8), ctrl->bInterfaceNumber, // When requestType is directed to an interface, the driver automatically passes the interface number in the low byte of index - buf, len, transferred, 0); + buf, (uint32_t)len, transferred, 0); } while (sts != RS2_USB_STATUS_SUCCESS && retries++ < 5); } }, [this](){ return !_messenger; }); diff --git a/src/uvc/uvc-streamer.cpp b/src/uvc/uvc-streamer.cpp index 3e1c7aa487..974aff5c52 100644 --- a/src/uvc/uvc-streamer.cpp +++ b/src/uvc/uvc-streamer.cpp @@ -28,7 +28,7 @@ namespace librealsense _action_dispatcher.start(); - _watchdog_timeout = (1000.0 / _context.profile.fps) * 10; + _watchdog_timeout = (int64_t)((1000.0 / _context.profile.fps) * 10); init(); } From 97fb66589277b363763673cee3a213ec9b7a3e67 Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Thu, 26 Sep 2024 13:23:45 +0300 Subject: [PATCH 18/75] remove additional flags from third party --- CMake/external_fastdds.cmake | 5 ++++- CMake/external_foonathan_memory.cmake | 6 ++++++ CMake/external_json.cmake | 5 +++++ CMake/external_libcurl.cmake | 7 ++++--- CMake/unix_config.cmake | 12 ++++++------ CMake/windows_config.cmake | 11 +++++------ third-party/CMakeLists.txt | 5 +++++ 7 files changed, 35 insertions(+), 16 deletions(-) diff --git a/CMake/external_fastdds.cmake b/CMake/external_fastdds.cmake index 856e0dbfc7..7cd817544a 100644 --- a/CMake/external_fastdds.cmake +++ b/CMake/external_fastdds.cmake @@ -67,7 +67,10 @@ function(get_fastdds) message(CHECK_PASS "Done") endfunction() +string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") +string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") # Trigger the FastDDS build get_fastdds() - +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") diff --git a/CMake/external_foonathan_memory.cmake b/CMake/external_foonathan_memory.cmake index f6c4938267..2bd26fe1c6 100644 --- a/CMake/external_foonathan_memory.cmake +++ b/CMake/external_foonathan_memory.cmake @@ -41,4 +41,10 @@ function(get_foonathan_memory) endfunction() +string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") +string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") + get_foonathan_memory() + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") diff --git a/CMake/external_json.cmake b/CMake/external_json.cmake index 69c84ffeec..5c55114e7a 100644 --- a/CMake/external_json.cmake +++ b/CMake/external_json.cmake @@ -46,5 +46,10 @@ function(get_nlohmann_json) endfunction() +string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") +string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") # Trigger the build get_nlohmann_json() + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") diff --git a/CMake/external_libcurl.cmake b/CMake/external_libcurl.cmake index 2fce83548d..64f57b178a 100644 --- a/CMake/external_libcurl.cmake +++ b/CMake/external_libcurl.cmake @@ -1,7 +1,7 @@ if(CHECK_FOR_UPDATES) - set(FLAGS_ORIGIN "${CMAKE_C_FLAGS}") - string(REPLACE "-Werror" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") # -Werror causes libcurl build to fail + string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") + string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") include(ExternalProject) message(STATUS "Building libcurl enabled") @@ -64,5 +64,6 @@ if(CHECK_FOR_UPDATES) endif() endif() - set(CMAKE_C_FLAGS "${FLAGS_ORIGIN}") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") endif() #CHECK_FOR_UPDATES diff --git a/CMake/unix_config.cmake b/CMake/unix_config.cmake index dbb4cc9a10..e05bfc59b7 100644 --- a/CMake/unix_config.cmake +++ b/CMake/unix_config.cmake @@ -71,12 +71,10 @@ macro(os_set_flags) # see https://readthedocs.intel.com/SecureCodingStandards/2023.Q2.0/compiler/c-cpp/ for more details if (CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64|armv7l") # Jetson system, some flags are not recognized - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wformat -Wformat-security -fPIC -D_FORTIFY_SOURCE=2 -fstack-protector") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wformat -Wformat-security -fPIC -D_FORTIFY_SOURCE=2 -fstack-protector") + set(ADDITIONAL_COMPILER_FLAGS "-Wformat -Wformat-security -fPIC -D_FORTIFY_SOURCE=2 -fstack-protector") else() #‘-mfunction-return’ and ‘-fcf-protection’ are not compatible, so specifing -fcf-protection=none - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wformat -Wformat-security -fPIC -D_FORTIFY_SOURCE=2 -fcf-protection=none -mfunction-return=thunk -mindirect-branch=thunk -mindirect-branch-register -fstack-protector") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wformat -Wformat-security -fPIC -D_FORTIFY_SOURCE=2 -fcf-protection=none -mfunction-return=thunk -mindirect-branch=thunk -mindirect-branch-register -fstack-protector") + set(ADDITIONAL_COMPILER_FLAGS "-Wformat -Wformat-security -fPIC -D_FORTIFY_SOURCE=2 -fcf-protection=none -mfunction-return=thunk -mindirect-branch=thunk -mindirect-branch-register -fstack-protector") endif() set(CMAKE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS} -pie") @@ -85,9 +83,11 @@ macro(os_set_flags) message(STATUS "Configuring for Debug build") else() # Release, RelWithDebInfo, or multi configuration generator is being used (aka not specifing build type, or building with VS) message(STATUS "Configuring for Release build") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror -z noexecstack -Wl,-z,relro,-z,now -fstack-protector-strong") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Werror -z noexecstack -Wl,-z,relro,-z,now -fstack-protector-strong") + set(ADDITIONAL_COMPILER_FLAGS "${ADDITIONAL_COMPILER_FLAGS} -Werror -z noexecstack -Wl,-z,relro,-z,now -fstack-protector-strong") endif() + + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") ################# diff --git a/CMake/windows_config.cmake b/CMake/windows_config.cmake index 614c7e9f96..97244907e2 100644 --- a/CMake/windows_config.cmake +++ b/CMake/windows_config.cmake @@ -55,20 +55,19 @@ macro(os_set_flags) # see https://readthedocs.intel.com/SecureCodingStandards/2023.Q2.0/compiler/c-cpp/ for more details - - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Gy /DYNAMICBASE /GS /wd4101") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /Gy /DYNAMICBASE /GS /wd4101") + set(ADDITIONAL_COMPILER_FLAGS "/Gy /DYNAMICBASE /GS /wd4101") if(CMAKE_BUILD_TYPE STREQUAL "Debug") message(STATUS "Configuring for Debug build") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") else() # Release, RelWithDebInfo, or multi configuration generator is being used (aka not specifing build type, or building with VS) message(STATUS "Configuring for Release build") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /WX /sdl") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /WX /sdl") + set(ADDITIONAL_COMPILER_FLAGS "${ADDITIONAL_COMPILER_FLAGS} /WX /sdl") set(CMAKE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS} /INCREMENTAL:NO /LTCG /NXCOMPAT") # ignoring '/INCREMENTAL' due to '/LTCG' specification endif() + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") + ################# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /bigobj /wd4819") diff --git a/third-party/CMakeLists.txt b/third-party/CMakeLists.txt index c70314062e..6bf02b578e 100644 --- a/third-party/CMakeLists.txt +++ b/third-party/CMakeLists.txt @@ -1,5 +1,8 @@ string(REPLACE ${PROJECT_SOURCE_DIR}/ "" _rel_path ${CMAKE_CURRENT_LIST_DIR}) +string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") +string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") + include(CMake/external_json.cmake) add_subdirectory( "${CMAKE_CURRENT_LIST_DIR}/rsutils" ) @@ -18,3 +21,5 @@ if( BUILD_WITH_DDS ) add_subdirectory( "${CMAKE_CURRENT_LIST_DIR}/realdds" ) endif() +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") From dd66948a4fbf90c1377e9f136351de5fdd539edc Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Thu, 26 Sep 2024 15:32:18 +0300 Subject: [PATCH 19/75] avoid using flags on old gcc --- CMake/unix_config.cmake | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CMake/unix_config.cmake b/CMake/unix_config.cmake index e05bfc59b7..6b9c92eb9a 100644 --- a/CMake/unix_config.cmake +++ b/CMake/unix_config.cmake @@ -70,7 +70,8 @@ macro(os_set_flags) # see https://readthedocs.intel.com/SecureCodingStandards/2023.Q2.0/compiler/c-cpp/ for more details - if (CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64|armv7l") # Jetson system, some flags are not recognized + if (CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64|armv7l" OR # Some flags are not recognized or Jetson systems or on GCC version < 9 + (CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_LESS "9.0")) # set(ADDITIONAL_COMPILER_FLAGS "-Wformat -Wformat-security -fPIC -D_FORTIFY_SOURCE=2 -fstack-protector") else() #‘-mfunction-return’ and ‘-fcf-protection’ are not compatible, so specifing -fcf-protection=none From 9cc5ad60ee1e18e80a63c31d702f552a4b2d72df Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Thu, 26 Sep 2024 15:41:42 +0300 Subject: [PATCH 20/75] disable flags on glfw --- CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 467c844b09..9e0a3f4283 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -83,6 +83,8 @@ global_target_config() include(CMake/install_config.cmake) +string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") +string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") add_subdirectory(wrappers) if ( ( BUILD_EXAMPLES OR BUILD_PC_STITCHING ) AND BUILD_GLSL_EXTENSIONS ) find_package(glfw3 3.3 QUIET) @@ -93,6 +95,8 @@ if ( ( BUILD_EXAMPLES OR BUILD_PC_STITCHING ) AND BUILD_GLSL_EXTENSIONS ) endif() add_subdirectory(src/gl) endif() +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") if(BUILD_EXAMPLES) add_subdirectory(examples) From 6cc2b954b122525e054ad0e5b1e8cc5dce08d9c7 Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Thu, 26 Sep 2024 15:47:24 +0300 Subject: [PATCH 21/75] remove some flags unrecognized on mac --- CMake/unix_config.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMake/unix_config.cmake b/CMake/unix_config.cmake index 6b9c92eb9a..603adda0fc 100644 --- a/CMake/unix_config.cmake +++ b/CMake/unix_config.cmake @@ -70,7 +70,7 @@ macro(os_set_flags) # see https://readthedocs.intel.com/SecureCodingStandards/2023.Q2.0/compiler/c-cpp/ for more details - if (CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64|armv7l" OR # Some flags are not recognized or Jetson systems or on GCC version < 9 + if (CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64|armv7l" OR APPLE OR # Some flags are not recognized or some systems / gcc versions (CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_LESS "9.0")) # set(ADDITIONAL_COMPILER_FLAGS "-Wformat -Wformat-security -fPIC -D_FORTIFY_SOURCE=2 -fstack-protector") else() From baa0dfaa1ca6b81e934fc6c3d1f32da6b518a367 Mon Sep 17 00:00:00 2001 From: ohadmeir Date: Thu, 26 Sep 2024 17:12:24 +0300 Subject: [PATCH 22/75] Fix PR#13376 comments --- common/device-model.cpp | 1 + src/ds/d400/d400-auto-calibration.cpp | 7 ------- 2 files changed, 1 insertion(+), 7 deletions(-) diff --git a/common/device-model.cpp b/common/device-model.cpp index 5818df1636..319425eda3 100644 --- a/common/device-model.cpp +++ b/common/device-model.cpp @@ -3366,6 +3366,7 @@ namespace rs2 std::shared_ptr< process_notification_model > n; if( is_d555 ) { + // D555 OHM is same as D455, using D400 calibration algorithms. manager = std::make_shared< on_chip_calib_manager >( viewer, sub, *this, dev ); n = std::make_shared< autocalib_notification_model >( "", manager, false ); } diff --git a/src/ds/d400/d400-auto-calibration.cpp b/src/ds/d400/d400-auto-calibration.cpp index a1d5310aac..b0bfd74e8e 100644 --- a/src/ds/d400/d400-auto-calibration.cpp +++ b/src/ds/d400/d400-auto-calibration.cpp @@ -47,13 +47,6 @@ namespace librealsense uint16_t tableSize; // 512 bytes }; - struct DscResultBuffer - { - uint16_t paramSize; - uint16_t status; - float healthCheck; - uint16_t tableSize; - }; #pragma pack(pop) enum class host_assistance_type From 04259b321d4a6cc0ea4951aa5cc03a9b54c57d12 Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Thu, 26 Sep 2024 17:47:20 +0300 Subject: [PATCH 23/75] try replace resize with assign to avoid warning --- src/hid-sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/hid-sensor.cpp b/src/hid-sensor.cpp index 03f831aeeb..f62ba07a8f 100644 --- a/src/hid-sensor.cpp +++ b/src/hid-sensor.cpp @@ -145,7 +145,7 @@ void hid_sensor::close() std::lock_guard< std::mutex > lock( _configure_lock ); _configured_profiles.clear(); _is_configured_stream.clear(); - _is_configured_stream.resize( RS2_STREAM_COUNT ); + _is_configured_stream.assign(RS2_STREAM_COUNT, false); } _is_opened = false; if( Is< librealsense::global_time_interface >( _owner ) ) From b17297f39c04fd44a199931434cce90c2511c277 Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Sun, 29 Sep 2024 09:44:14 +0300 Subject: [PATCH 24/75] conditionally add FORTIFY_SOURCE --- CMake/unix_config.cmake | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/CMake/unix_config.cmake b/CMake/unix_config.cmake index 603adda0fc..fdabb32a2d 100644 --- a/CMake/unix_config.cmake +++ b/CMake/unix_config.cmake @@ -72,13 +72,17 @@ macro(os_set_flags) if (CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64|armv7l" OR APPLE OR # Some flags are not recognized or some systems / gcc versions (CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_LESS "9.0")) # - set(ADDITIONAL_COMPILER_FLAGS "-Wformat -Wformat-security -fPIC -D_FORTIFY_SOURCE=2 -fstack-protector") + set(ADDITIONAL_COMPILER_FLAGS "-Wformat -Wformat-security -fPIC -fstack-protector") else() #‘-mfunction-return’ and ‘-fcf-protection’ are not compatible, so specifing -fcf-protection=none - set(ADDITIONAL_COMPILER_FLAGS "-Wformat -Wformat-security -fPIC -D_FORTIFY_SOURCE=2 -fcf-protection=none -mfunction-return=thunk -mindirect-branch=thunk -mindirect-branch-register -fstack-protector") + set(ADDITIONAL_COMPILER_FLAGS "-Wformat -Wformat-security -fPIC -fcf-protection=none -mfunction-return=thunk -mindirect-branch=thunk -mindirect-branch-register -fstack-protector") endif() set(CMAKE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS} -pie") + string(FIND "${CMAKE_CXX_FLAGS}" "-D_FORTIFY_SOURCE" _index) + if (${_index} EQUAL -1) # Define D_FORTIFY_SOURCE is undefined + set(ADDITIONAL_COMPILER_FLAGS "${ADDITIONAL_COMPILER_FLAGS} -D_FORTIFY_SOURCE=2") + endif() if(CMAKE_BUILD_TYPE STREQUAL "Debug") message(STATUS "Configuring for Debug build") From a207041369a2d4a748be45c88db4ff2da640e393 Mon Sep 17 00:00:00 2001 From: ohadmeir Date: Sun, 29 Sep 2024 11:56:29 +0300 Subject: [PATCH 25/75] Fix Tare calibration flow on DDS --- src/ds/d500/d500-auto-calibration.cpp | 33 ++++++++++++++------------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/src/ds/d500/d500-auto-calibration.cpp b/src/ds/d500/d500-auto-calibration.cpp index 3f01c36beb..d461d2be9b 100644 --- a/src/ds/d500/d500-auto-calibration.cpp +++ b/src/ds/d500/d500-auto-calibration.cpp @@ -650,29 +650,30 @@ namespace librealsense std::shared_ptr< option > d500_auto_calibrated::change_preset() { - preset old_preset_values{}; - rs2_rs400_visual_preset old_preset = { RS2_RS400_VISUAL_PRESET_DEFAULT }; - if( ! _depth_sensor ) - throw not_implemented_exception( " Depth sensor must be supplied to d500_auto_calibrated" ); + throw not_implemented_exception( "Depth sensor must be supplied to d500_auto_calibrated" ); if( _depth_sensor->supports_option( RS2_OPTION_VISUAL_PRESET ) ) { auto & opt = _depth_sensor->get_option( RS2_OPTION_VISUAL_PRESET ); - old_preset = static_cast< rs2_rs400_visual_preset >( opt.query() ); - //if( old_preset == RS2_RS400_VISUAL_PRESET_CUSTOM ) - // old_preset_values = advanced_mode->get_all(); - opt.set( RS2_RS400_VISUAL_PRESET_HIGH_ACCURACY ); + auto old_preset = opt.get_value(); + switch( opt.get_value_type() ) + { + case RS2_OPTION_TYPE_FLOAT: // USB visual preset type is float + if( old_preset == RS2_RS400_VISUAL_PRESET_CUSTOM ) + throw not_implemented_exception( "Calibration with custom visual preset is not supported" ); + opt.set_value( RS2_RS400_VISUAL_PRESET_HIGH_ACCURACY ); + break; + case RS2_OPTION_TYPE_STRING: // DDS visual preset type is a string + opt.set_value( std::string( "High Accuracy") ); + break; + default: + throw invalid_value_exception( "Unsupported option type" ); + } - std::shared_ptr< option > recover_option( &opt, [old_preset, old_preset_values]( option * opt ) + std::shared_ptr< option > recover_option( &opt, [old_preset]( option * opt ) { - if( old_preset == RS2_RS400_VISUAL_PRESET_CUSTOM ) - { - opt->set( RS2_RS400_VISUAL_PRESET_CUSTOM ); - //adv->set_all( old_preset_values ); - } - else - opt->set( static_cast< float >( old_preset ) ); + opt->set_value( old_preset ); }); return recover_option; From e66f7a7fddcf1e721613771df16b79d9216e90ee Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Sun, 29 Sep 2024 15:02:37 +0300 Subject: [PATCH 26/75] trying to ignore third party differently --- CMake/external_fastdds.cmake | 4 ---- CMake/external_foonathan_memory.cmake | 4 ---- CMake/external_json.cmake | 4 ---- CMake/external_libcurl.cmake | 5 ----- CMake/unix_config.cmake | 4 ++++ CMake/windows_config.cmake | 4 ++++ CMakeLists.txt | 4 ---- third-party/CMakeLists.txt | 5 ----- 8 files changed, 8 insertions(+), 26 deletions(-) diff --git a/CMake/external_fastdds.cmake b/CMake/external_fastdds.cmake index 7cd817544a..c3752ae41b 100644 --- a/CMake/external_fastdds.cmake +++ b/CMake/external_fastdds.cmake @@ -67,10 +67,6 @@ function(get_fastdds) message(CHECK_PASS "Done") endfunction() -string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") -string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") # Trigger the FastDDS build get_fastdds() -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") diff --git a/CMake/external_foonathan_memory.cmake b/CMake/external_foonathan_memory.cmake index 2bd26fe1c6..41c56018f6 100644 --- a/CMake/external_foonathan_memory.cmake +++ b/CMake/external_foonathan_memory.cmake @@ -41,10 +41,6 @@ function(get_foonathan_memory) endfunction() -string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") -string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") get_foonathan_memory() -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") diff --git a/CMake/external_json.cmake b/CMake/external_json.cmake index 5c55114e7a..dc76ec96cb 100644 --- a/CMake/external_json.cmake +++ b/CMake/external_json.cmake @@ -46,10 +46,6 @@ function(get_nlohmann_json) endfunction() -string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") -string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") # Trigger the build get_nlohmann_json() -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") diff --git a/CMake/external_libcurl.cmake b/CMake/external_libcurl.cmake index 64f57b178a..0f8580d4f1 100644 --- a/CMake/external_libcurl.cmake +++ b/CMake/external_libcurl.cmake @@ -1,8 +1,5 @@ if(CHECK_FOR_UPDATES) - string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") - string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") - include(ExternalProject) message(STATUS "Building libcurl enabled") @@ -64,6 +61,4 @@ if(CHECK_FOR_UPDATES) endif() endif() - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") endif() #CHECK_FOR_UPDATES diff --git a/CMake/unix_config.cmake b/CMake/unix_config.cmake index fdabb32a2d..245781ab55 100644 --- a/CMake/unix_config.cmake +++ b/CMake/unix_config.cmake @@ -93,6 +93,10 @@ macro(os_set_flags) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") + + + set_directory_properties(PROPERTIES DIRECTORY third-party/ COMPILE_OPTIONS "-w") + set_source_files_properties(third-party/*.* PROPERTIES COMPILE_OPTIONS "-w") ################# diff --git a/CMake/windows_config.cmake b/CMake/windows_config.cmake index 97244907e2..37cffdf50f 100644 --- a/CMake/windows_config.cmake +++ b/CMake/windows_config.cmake @@ -68,6 +68,10 @@ macro(os_set_flags) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") + + set_directory_properties(PROPERTIES DIRECTORY third-party/ COMPILE_OPTIONS "/W0") + set_source_files_properties(third-party/*.* PROPERTIES COMPILE_OPTIONS "/W0") + ################# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /bigobj /wd4819") diff --git a/CMakeLists.txt b/CMakeLists.txt index 9e0a3f4283..467c844b09 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -83,8 +83,6 @@ global_target_config() include(CMake/install_config.cmake) -string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") -string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") add_subdirectory(wrappers) if ( ( BUILD_EXAMPLES OR BUILD_PC_STITCHING ) AND BUILD_GLSL_EXTENSIONS ) find_package(glfw3 3.3 QUIET) @@ -95,8 +93,6 @@ if ( ( BUILD_EXAMPLES OR BUILD_PC_STITCHING ) AND BUILD_GLSL_EXTENSIONS ) endif() add_subdirectory(src/gl) endif() -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") if(BUILD_EXAMPLES) add_subdirectory(examples) diff --git a/third-party/CMakeLists.txt b/third-party/CMakeLists.txt index 6bf02b578e..c70314062e 100644 --- a/third-party/CMakeLists.txt +++ b/third-party/CMakeLists.txt @@ -1,8 +1,5 @@ string(REPLACE ${PROJECT_SOURCE_DIR}/ "" _rel_path ${CMAKE_CURRENT_LIST_DIR}) -string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") -string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") - include(CMake/external_json.cmake) add_subdirectory( "${CMAKE_CURRENT_LIST_DIR}/rsutils" ) @@ -21,5 +18,3 @@ if( BUILD_WITH_DDS ) add_subdirectory( "${CMAKE_CURRENT_LIST_DIR}/realdds" ) endif() -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") From cd82a9c27a0f69f8658207ed0b7f6722646b1f2c Mon Sep 17 00:00:00 2001 From: Eran Date: Tue, 17 Sep 2024 11:44:31 +0300 Subject: [PATCH 27/75] add FW version to rs-dds-config --- tools/dds/dds-config/rs-dds-config.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/tools/dds/dds-config/rs-dds-config.cpp b/tools/dds/dds-config/rs-dds-config.cpp index ea5fdd32bd..39bbe1d1c9 100644 --- a/tools/dds/dds-config/rs-dds-config.cpp +++ b/tools/dds/dds-config/rs-dds-config.cpp @@ -125,7 +125,11 @@ bool find_device( rs2::context const & ctx, continue; if( ! devices_looked_at.insert( sn ).second ) continue; // insert failed: device was already looked at - LOG_DEBUG( "trying " << possible_device.get_description() ); + if( possible_device.supports( RS2_CAMERA_INFO_FIRMWARE_VERSION ) ) + LOG_DEBUG( "trying " << possible_device.get_description() << ", FW version " + << possible_device.get_info( RS2_CAMERA_INFO_FIRMWARE_VERSION ) ); + else + LOG_DEBUG( "trying " << possible_device.get_description() ); config = get_eth_config( possible_device, golden ); if( device ) throw std::runtime_error( "More than one device is available; please use --serial-number" ); @@ -221,7 +225,11 @@ try throw std::runtime_error( "No device found supporting Eth" ); } - INFO( "Device: " << device.get_description() ); + if( device.supports( RS2_CAMERA_INFO_FIRMWARE_VERSION ) ) + INFO( "Device: " << device.get_description() << ", FW version " + << device.get_info( RS2_CAMERA_INFO_FIRMWARE_VERSION ) ); + else + INFO( "Device: " << device.get_description() ); eth_config requested( current ); if( golden || factory_reset_arg.isSet() || reset_arg.isSet() ) From 53dab1b53f38ce469098e68c413ae8cda91ef17d Mon Sep 17 00:00:00 2001 From: Eran Date: Tue, 17 Sep 2024 11:46:27 +0300 Subject: [PATCH 28/75] remove fixed_size_string dependencies in dds ros2 headers --- third-party/realdds/include/realdds/topics/ros2/ros2header.h | 2 +- third-party/realdds/include/realdds/topics/ros2/ros2image.h | 2 +- third-party/realdds/include/realdds/topics/ros2/ros2time.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/third-party/realdds/include/realdds/topics/ros2/ros2header.h b/third-party/realdds/include/realdds/topics/ros2/ros2header.h index a3cbcbb153..1373243f33 100644 --- a/third-party/realdds/include/realdds/topics/ros2/ros2header.h +++ b/third-party/realdds/include/realdds/topics/ros2/ros2header.h @@ -13,7 +13,7 @@ #include "ros2time.h" -#include +//#include #include #include diff --git a/third-party/realdds/include/realdds/topics/ros2/ros2image.h b/third-party/realdds/include/realdds/topics/ros2/ros2image.h index 4f1fd899ee..309d1ce130 100644 --- a/third-party/realdds/include/realdds/topics/ros2/ros2image.h +++ b/third-party/realdds/include/realdds/topics/ros2/ros2image.h @@ -13,7 +13,7 @@ #include "ros2header.h" -#include +//#include #include #include diff --git a/third-party/realdds/include/realdds/topics/ros2/ros2time.h b/third-party/realdds/include/realdds/topics/ros2/ros2time.h index d2b3306066..781a81fdcd 100644 --- a/third-party/realdds/include/realdds/topics/ros2/ros2time.h +++ b/third-party/realdds/include/realdds/topics/ros2/ros2time.h @@ -12,7 +12,7 @@ #define _FAST_DDS_GENERATED_STD_MSGS_MSG_TIME_H_ -#include +//#include #include #include From c6ffc22d4b4245820db69b52ed3aad08b9709a7f Mon Sep 17 00:00:00 2001 From: Eran Date: Tue, 17 Sep 2024 11:47:37 +0300 Subject: [PATCH 29/75] realdds::topics::image_msg contains all image header data --- .../include/realdds/topics/image-msg.h | 40 +++++++++++++++---- third-party/realdds/src/topics/image-msg.cpp | 37 ++++++----------- 2 files changed, 45 insertions(+), 32 deletions(-) diff --git a/third-party/realdds/include/realdds/topics/image-msg.h b/third-party/realdds/include/realdds/topics/image-msg.h index 5138c4c285..0184e00e8d 100644 --- a/third-party/realdds/include/realdds/topics/image-msg.h +++ b/third-party/realdds/include/realdds/topics/image-msg.h @@ -5,6 +5,8 @@ #include #include +#include + #include #include #include @@ -13,7 +15,6 @@ namespace sensor_msgs { namespace msg { class ImagePubSubType; -class Image; } // namespace msg } // namespace sensor_msgs @@ -31,6 +32,8 @@ namespace topics { class image_msg { + sensor_msgs::msg::Image _raw; + public: using type = sensor_msgs::msg::ImagePubSubType; @@ -46,8 +49,34 @@ class image_msg image_msg & operator=( image_msg && ) = default; image_msg & operator=( sensor_msgs::msg::Image && ); - bool is_valid() const { return width != -1 && height != -1; } - void invalidate() { width = -1; } + bool is_valid() const { return ! _raw.data().empty(); } + void invalidate() { _raw.data().clear(); } + + sensor_msgs::msg::Image & raw() { return _raw; } + sensor_msgs::msg::Image const & raw() const { return _raw; } + + auto width() const { return _raw.width(); } + void set_width( uint32_t w ) { _raw.width( w ); } + auto height() const { return _raw.height(); } + void set_height( uint32_t h ) { _raw.height( h ); } + auto step() const { return _raw.step(); } + void set_step( uint32_t step ) { _raw.step( step ); } + + std::string const & frame_id() const { return _raw.header().frame_id(); } + void set_frame_id( std::string new_id ) { _raw.header().frame_id( std::move( new_id ) ); } + + std::string const & encoding() const { return _raw.encoding(); } + void set_encoding( std::string new_encoding ) { _raw.encoding( std::move( new_encoding ) ); } + + bool is_bigendian() const { return _raw.is_bigendian(); } + + void set_timestamp( dds_time const & t ) + { + _raw.header().stamp().sec( t.seconds ); + _raw.header().stamp().nanosec( t.nanosec ); + } + dds_time timestamp() const { return { _raw.header().stamp().sec(), _raw.header().stamp().nanosec() }; } + static std::shared_ptr< dds_topic > create_topic( std::shared_ptr< dds_participant > const & participant, char const * topic_name ); @@ -63,11 +92,6 @@ class image_msg static bool take_next( dds_topic_reader &, image_msg * output, dds_sample * optional_sample = nullptr ); - - std::vector< uint8_t > raw_data; - int width = -1; - int height = -1; - dds_time timestamp; }; diff --git a/third-party/realdds/src/topics/image-msg.cpp b/third-party/realdds/src/topics/image-msg.cpp index d6bd28a505..44f1d8f2c3 100644 --- a/third-party/realdds/src/topics/image-msg.cpp +++ b/third-party/realdds/src/topics/image-msg.cpp @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// Copyright(c) 2022-4 Intel Corporation. All Rights Reserved. #include #include @@ -18,21 +18,14 @@ namespace topics { image_msg::image_msg( sensor_msgs::msg::Image && rhs ) + : _raw( std::move( rhs ) ) { - raw_data = std::move( rhs.data() ); - width = std::move( rhs.width() ); - height = std::move( rhs.height() ); - timestamp = dds_time( rhs.header().stamp().sec(), rhs.header().stamp().nanosec() ); } image_msg & image_msg::operator=( sensor_msgs::msg::Image && rhs ) { - raw_data = std::move( rhs.data() ); - width = std::move( rhs.width() ); - height = std::move( rhs.height() ); - timestamp = dds_time( rhs.header().stamp().sec(), rhs.header().stamp().nanosec() ); - + _raw = std::move( rhs ); return *this; } @@ -49,24 +42,20 @@ image_msg::create_topic( std::shared_ptr< dds_participant > const & participant, /*static*/ bool image_msg::take_next( dds_topic_reader & reader, image_msg * output, dds_sample * sample ) { - sensor_msgs::msg::Image raw_data; + image_msg output_; + if( ! output ) + output = &output_; // use the local copy if the user hasn't provided their own dds_sample sample_; if( ! sample ) sample = &sample_; // use the local copy if the user hasn't provided their own - auto status = reader->take_next_sample( &raw_data, sample ); - if ( status == ReturnCode_t::RETCODE_OK ) + auto status = reader->take_next_sample( &output->raw(), sample ); + if( status == ReturnCode_t::RETCODE_OK ) { - // We have data - if ( output ) - { - // Only samples for which valid_data is true should be accessed - // valid_data indicates that the instance is still ALIVE and the `take` return an - // updated sample - if( ! sample->valid_data ) - output->invalidate(); - else - *output = std::move( raw_data ); //TODO - optimize copy, use dds loans - } + // Only samples for which valid_data is true should be accessed + // valid_data indicates that the instance is still ALIVE and the `take` return an + // updated sample + if( ! sample->valid_data ) + output->invalidate(); return true; } From ad6dea8c7e893a4e76c8e8d8a9d0d0f8d51c7c52 Mon Sep 17 00:00:00 2001 From: Eran Date: Mon, 30 Sep 2024 08:23:07 +0300 Subject: [PATCH 30/75] fix rs-fw-update wait for device to reconnect --- tools/fw-update/rs-fw-update.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tools/fw-update/rs-fw-update.cpp b/tools/fw-update/rs-fw-update.cpp index 743e97a276..403a7454cb 100644 --- a/tools/fw-update/rs-fw-update.cpp +++ b/tools/fw-update/rs-fw-update.cpp @@ -525,7 +525,7 @@ try } } - update( new_fw_update_device, fw_image ); + update( new_fw_update_device, fw_image ); done = true; break; @@ -546,6 +546,7 @@ try std::cout << std::endl << "Waiting for device to reconnect..." << std::endl; std::unique_lock lk(mutex); + new_device = rs2::device(); // otherwise the wait will exit right away cv.wait_for(lk, std::chrono::seconds(WAIT_FOR_DEVICE_TIMEOUT), [&] { return !done || new_device; }); if (done) From 4d63b32ecc8c1f065490b3a520ae97a664099a9c Mon Sep 17 00:00:00 2001 From: Eran Date: Tue, 17 Sep 2024 11:48:20 +0300 Subject: [PATCH 31/75] change everything else around image_msg --- src/dds/rs-dds-sensor-proxy.cpp | 13 +++-- .../include/realdds/dds-stream-server.h | 2 +- third-party/realdds/py/pyrealdds.cpp | 51 +++++++++++-------- third-party/realdds/src/dds-stream-server.cpp | 49 ++++++++++-------- .../dds/dds-adapter/lrs-device-controller.cpp | 8 +-- unit-tests/dds/test-md-syncer.py | 1 + 6 files changed, 70 insertions(+), 54 deletions(-) diff --git a/src/dds/rs-dds-sensor-proxy.cpp b/src/dds/rs-dds-sensor-proxy.cpp index e4861c0f69..4a0d7f0c9a 100644 --- a/src/dds/rs-dds-sensor-proxy.cpp +++ b/src/dds/rs-dds-sensor-proxy.cpp @@ -306,30 +306,29 @@ void dds_sensor_proxy::handle_video_data( realdds::topics::image_msg && dds_fram data.backend_timestamp // time when the underlying backend (DDS) received it = static_cast(realdds::time_to_double( dds_sample.reception_timestamp ) * 1e3); data.timestamp // in ms - = static_cast< rs2_time_t >( realdds::time_to_double( dds_frame.timestamp ) * 1e3 ); + = static_cast< rs2_time_t >( realdds::time_to_double( dds_frame.timestamp() ) * 1e3 ); data.last_timestamp = streaming.last_timestamp.exchange( data.timestamp ); data.timestamp_domain; // from metadata, or leave default (hardware domain) data.depth_units; // from metadata data.frame_number; // filled in only once metadata is known - data.raw_size = static_cast< uint32_t >( dds_frame.raw_data.size() ); + data.raw_size = static_cast< uint32_t >( dds_frame.raw().data().size() ); auto vid_profile = dynamic_cast< video_stream_profile_interface * >( profile.get() ); if( ! vid_profile ) throw invalid_value_exception( "non-video profile provided to on_video_frame" ); - auto stride = static_cast< int >( dds_frame.height > 0 ? dds_frame.raw_data.size() / dds_frame.height - : dds_frame.raw_data.size() ); - auto bpp = dds_frame.width > 0 ? stride / dds_frame.width : stride; + auto stride = static_cast< int >( dds_frame.height() > 0 ? data.raw_size / dds_frame.height() : data.raw_size ); + auto bpp = dds_frame.width() > 0 ? stride / dds_frame.width() : stride; auto new_frame_interface = allocate_new_video_frame( vid_profile, stride, bpp, std::move( data ) ); if( ! new_frame_interface ) return; auto new_frame = static_cast< frame * >( new_frame_interface ); - new_frame->data = std::move( dds_frame.raw_data ); + new_frame->data = std::move( dds_frame.raw().data() ); if( _md_enabled ) { - streaming.syncer.enqueue_frame( dds_frame.timestamp.to_ns(), streaming.syncer.hold( new_frame ) ); + streaming.syncer.enqueue_frame( dds_frame.timestamp().to_ns(), streaming.syncer.hold( new_frame ) ); } else { diff --git a/third-party/realdds/include/realdds/dds-stream-server.h b/third-party/realdds/include/realdds/dds-stream-server.h index 564b4374ad..5ff8f0aba3 100644 --- a/third-party/realdds/include/realdds/dds-stream-server.h +++ b/third-party/realdds/include/realdds/dds-stream-server.h @@ -92,7 +92,7 @@ class dds_video_stream_server : public dds_stream_server void stop_streaming() override; image_header const & get_image_header() const { return _image_header; } - virtual void publish_image( topics::image_msg && ); + virtual void publish_image( topics::image_msg & ); private: void check_profile( std::shared_ptr< dds_stream_profile > const & ) const override; diff --git a/third-party/realdds/py/pyrealdds.cpp b/third-party/realdds/py/pyrealdds.cpp index e1d3970fb3..930cfa332f 100644 --- a/third-party/realdds/py/pyrealdds.cpp +++ b/third-party/realdds/py/pyrealdds.cpp @@ -623,22 +623,42 @@ PYBIND11_MODULE(NAME, m) { py::class_< image_msg, std::shared_ptr< image_msg > >( message, "image" ) .def( py::init<>() ) .def_static( "create_topic", &image_msg::create_topic ) - .def_readwrite( "data", &image_msg::raw_data ) - .def_readwrite( "width", &image_msg::width ) - .def_readwrite( "height", &image_msg::height ) - .def_readwrite( "timestamp", &image_msg::timestamp ) + .def_property( + "data", + []( image_msg const & self ) { return self.raw().data(); }, + []( image_msg & self, std::vector< uint8_t > bytes ) { self.raw().data( std::move( bytes ) ); } ) + .def_property( "width", &image_msg::width, &image_msg::set_width ) + .def_property( "height", &image_msg::height, &image_msg::set_height ) + .def_property( "step", &image_msg::step, &image_msg::set_step ) + .def_property( "timestamp", &image_msg::timestamp, &image_msg::set_timestamp ) + .def_property_readonly( "is_bigendian", &image_msg::is_bigendian ) + .def_property( "encoding", &image_msg::encoding, &image_msg::set_encoding ) + .def_property( "frame_id", &image_msg::frame_id, &image_msg::set_frame_id ) .def( "__bool__", &image_msg::is_valid ) .def( "__repr__", []( image_msg const & self ) { std::ostringstream os; os << "<" SNAME ".image_msg"; - if( self.width > 0 && self.height > 0 ) + if( self.is_valid() ) { - os << ' ' << self.width << 'x' << self.height; - os << 'x' << (self.raw_data.size() / (self.width * self.height)); + if( ! self.frame_id().empty() ) + os << " " << self.frame_id(); + if( ! self.encoding().empty() ) + os << " '" << self.encoding() << "'"; + os << " " << self.width() << 'x' << self.height(); + if( ! self.step() ) + os << " STEP 0"; + else if( self.step() % self.width() ) + os << " STEP " << self.step(); + else if( self.raw().data().size() % self.step() ) + os << " SIZE " << self.raw().data().size(); + //else + // os << ' ' << ( self.raw().data().size() / ( self.width() * self.height() ) ) << " Bpp"; + if( self.is_bigendian() ) + os << " BE"; } - os << " @ " << realdds::time_to_string( self.timestamp ); + os << " @ " << realdds::time_to_string( self.timestamp() ); os << ">"; return os.str(); } ) @@ -801,6 +821,8 @@ PYBIND11_MODULE(NAME, m) { .def( "is_valid", &dds_video_encoding::is_valid ) .def( "to_rs2", &dds_video_encoding::to_rs2 ) .def( "from_rs2", &dds_video_encoding::from_rs2 ) + .def( py::self == py::self ) + .def( py::self != py::self ) .def( "__bool__", &dds_video_encoding::is_valid ) .def( "__repr__", []( dds_video_encoding const & self ) { return self.to_string(); } ); video_encoding.attr( "z16" ) = dds_video_encoding( "16UC1" ); @@ -884,18 +906,7 @@ PYBIND11_MODULE(NAME, m) { []( dds_video_stream_server & self, dds_video_encoding encoding, int width, int height ) { self.start_streaming( { encoding, height, width } ); } ) - .def( "publish_image", - []( dds_video_stream_server & self, image_msg const & img ) - { - // We don't have C++ 'std::move' explicit semantics in Python, so we create a copy. - // Notice there's no copy constructor on purpose (!) so we do it manually... - image_msg img_copy; - img_copy.raw_data = img.raw_data; - img_copy.timestamp = img.timestamp; - img_copy.width = img.width; - img_copy.height = img.height; - self.publish_image( std::move( img_copy ) ); - } ); + .def( "publish_image", &dds_video_stream_server::publish_image ); using realdds::dds_depth_stream_server; py::class_< dds_depth_stream_server, std::shared_ptr< dds_depth_stream_server > >( m, "depth_stream_server", video_stream_server_base ) diff --git a/third-party/realdds/src/dds-stream-server.cpp b/third-party/realdds/src/dds-stream-server.cpp index 1c84392e16..d72b871d61 100644 --- a/third-party/realdds/src/dds-stream-server.cpp +++ b/third-party/realdds/src/dds-stream-server.cpp @@ -197,23 +197,27 @@ void dds_stream_server::close() _writer.reset(); } -void dds_video_stream_server::publish_image( topics::image_msg && image ) +void dds_video_stream_server::publish_image( topics::image_msg & image ) { if( ! is_streaming() ) - DDS_THROW( runtime_error, "stream '" + name() + "' cannot publish before start_streaming()" ); + DDS_THROW( runtime_error, "stream '" << name() << "' cannot publish before start_streaming()" ); + + if( ! image.is_valid() ) + DDS_THROW( runtime_error, "image is invalid" ); - if( image.height != _image_header.height ) + if( ! image.height() ) + image.set_height( _image_header.height ); + else if( image.height() != _image_header.height ) DDS_THROW( runtime_error, - "image height (" + std::to_string( image.height ) + ") does not match stream header (" - + std::to_string( _image_header.height ) + ")" ); - if( image.width != _image_header.width ) + "image height (" << image.height() << ") does not match stream header (" << _image_header.height << ")" ); + if( ! image.width() ) + image.set_width( _image_header.width ); + else if( image.width() != _image_header.width ) DDS_THROW( runtime_error, - "image width (" + std::to_string( image.width ) + ") does not match stream header (" - + std::to_string( _image_header.width ) + ")" ); + "image width (" << image.width() << ") does not match stream header (" << _image_header.width << ")" ); // LOG_DEBUG( "publishing a DDS video frame for topic: " << _writer->topic()->get()->get_name() ); - sensor_msgs::msg::Image raw_image; - + // "The frame_id in a message specifies the point of reference for data contained in that message." // // I.e., the frame_id in a ROS header is the ID of the sensor collecting the data, and only relevant @@ -223,22 +227,23 @@ void dds_video_stream_server::publish_image( topics::image_msg && image ) // // For us, for now, we specify the name of the sensor and the device that owns it (TODO): // - raw_image.header().frame_id() = sensor_name(); - - raw_image.header().stamp().sec() = image.timestamp.seconds; - raw_image.header().stamp().nanosec() = image.timestamp.nanosec; + if( image.frame_id().empty() ) + image.set_frame_id( sensor_name() ); - raw_image.encoding() = _image_header.encoding.to_string(); - raw_image.height() = _image_header.height; - raw_image.width() = _image_header.width; - raw_image.step() = uint32_t( image.raw_data.size() / _image_header.height ); + if( image.encoding().empty() ) + image.set_encoding( _image_header.encoding.to_string() ); + else if( dds_video_encoding( image.encoding() ) != _image_header.encoding ) + DDS_THROW( runtime_error, + "image encoding (" << image.encoding() << ") does not match stream header (" + << _image_header.encoding.to_string() << ")" ); - raw_image.is_bigendian() = false; + if( ! image.step() ) + image.set_step( uint32_t( image.raw().data().size() / image.height() ) ); - raw_image.data() = std::move( image.raw_data ); + assert( ! image.is_bigendian() ); - LOG_DEBUG( "publishing '" << name() << "' " << raw_image.encoding() << " frame @ " << time_to_string( image.timestamp ) ); - DDS_API_CALL( _writer->get()->write( &raw_image ) ); + LOG_DEBUG( "publishing '" << name() << "' " << image.encoding() << " frame @ " << time_to_string( image.timestamp() ) ); + DDS_API_CALL( _writer->get()->write( &image.raw() ) ); } diff --git a/tools/dds/dds-adapter/lrs-device-controller.cpp b/tools/dds/dds-adapter/lrs-device-controller.cpp index d8d0aa0d5f..49b2fc981f 100644 --- a/tools/dds/dds-adapter/lrs-device-controller.cpp +++ b/tools/dds/dds-adapter/lrs-device-controller.cpp @@ -733,11 +733,11 @@ lrs_device_controller::lrs_device_controller( rs2::device dev, std::shared_ptr< ( static_cast< long double >( f.get_timestamp() ) / 1e3 ); realdds::topics::image_msg image; + image.set_height( video->get_image_header().width ); + image.set_width( video->get_image_header().width ); + image.set_timestamp( timestamp ); auto data = static_cast< const uint8_t * >( f.get_data() ); - image.raw_data.assign( data, data + f.get_data_size() ); - image.height = video->get_image_header().height; - image.width = video->get_image_header().width; - image.timestamp = timestamp; + image.raw().data().assign( data, data + f.get_data_size() ); video->publish_image( std::move( image ) ); publish_frame_metadata( f, timestamp ); diff --git a/unit-tests/dds/test-md-syncer.py b/unit-tests/dds/test-md-syncer.py index c807d1ded7..6846965063 100644 --- a/unit-tests/dds/test-md-syncer.py +++ b/unit-tests/dds/test-md-syncer.py @@ -79,6 +79,7 @@ def new_syncer( on_frame_ready=on_frame_ready, on_metadata_dropped=on_metadata_d def new_image_( id, width, height, bpp, timestamp=None ): i = dds.message.image() i.width = width + i.step = width * bpp i.height = height i.data = bytearray( width * height * bpp ) i.timestamp = timestamp or time_stamp( id ) From 56893abbc53c838214ea6e9a1704b2f2eb032c28 Mon Sep 17 00:00:00 2001 From: Eran Date: Tue, 17 Sep 2024 11:49:04 +0300 Subject: [PATCH 32/75] fps.py --debug-frames; more flexible --- third-party/realdds/scripts/fps.py | 50 ++++++++++++++++++++++-------- 1 file changed, 37 insertions(+), 13 deletions(-) diff --git a/third-party/realdds/scripts/fps.py b/third-party/realdds/scripts/fps.py index c4ae1066bd..854c2bc87e 100644 --- a/third-party/realdds/scripts/fps.py +++ b/third-party/realdds/scripts/fps.py @@ -1,10 +1,11 @@ # License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2023 Intel Corporation. All Rights Reserved. +# Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. from argparse import ArgumentParser args = ArgumentParser() args.add_argument( '--debug', action='store_true', help='enable debug mode' ) args.add_argument( '--quiet', action='store_true', help='No output; just the minimum FPS as a number' ) +args.add_argument( '--debug-frames', action='store_true', help='Output frame signatures as we get them' ) args.add_argument( '--device', metavar='', required=True, help='the topic root for the device' ) def time_arg(x): t = int(x) @@ -23,14 +24,15 @@ def domain_arg(x): args.add_argument( '--color', action='store_true', help='stream Color' ) args.add_argument( '--ir1', action='store_true', help='stream IR1' ) args.add_argument( '--ir2', action='store_true', help='stream IR2' ) -args.add_argument( '--fps', metavar='5,15,30,60,90', type=int, default=30, help='Frames per second' ) +args.add_argument( '--fps', metavar='5,15,30,60,90', type=int, default=0, help='Frames per second' ) +args.add_argument( '--encoding', metavar='', default='', help='profile encoding (z16/y8/y16/yuyv/etc.); otherwise looked up per stream' ) def res_arg(x): import re m = re.fullmatch( r'(\d+)x(\d+)', x ) if not m: raise ValueError( f'--res should be WIDTHxHEIGHT' ) return [int(m.group(1)), int(m.group(2))] -args.add_argument( '--res', metavar='WxH', type=res_arg, default=[1280,720], help='Resolution as WIDTHxHEIGHT' ) +args.add_argument( '--res', metavar='WxH', type=res_arg, default=[0,0], help='Resolution as WIDTHxHEIGHT' ) args = args.parse_args() @@ -90,7 +92,8 @@ def e( *a, **kw ): n_stream_frames[s] = 0 capturing = True def on_image( stream, image, sample ): - #d( f'----> {image}') + if args.debug_frames: + print( f' {image}') global n_stream_frames, capturing if capturing: n_stream_frames[stream.name()] += 1 @@ -98,19 +101,40 @@ def on_image( stream, image, sample ): fps = args.fps width = args.res[0] height = args.res[1] -type_format = { 'depth': str(dds.video_encoding.z16), 'color': str(dds.video_encoding.yuyv), 'ir': str(dds.video_encoding.y8) } +encoding = args.encoding and vars( dds.video_encoding )[args.encoding] or None +type_format = { 'depth': dds.video_encoding.z16, 'color': dds.video_encoding.yuyv, 'ir': dds.video_encoding.y8 } + + +def find_profile( stream, w, h, encoding, fps ): + for p in stream.profiles(): + #print( f' \'{p.format()}\' {p.width()}x{p.height()} @ {p.frequency()} FPS', end='' ) + if fps and p.frequency() != fps: + #print( ' x frequency') + continue + if w and p.width() != w: + #print( ' x width') + continue + if h and p.height() != h: + #print( ' x height') + continue + if encoding and p.format() != encoding: + #print( ' x encoding') + continue + #print() + return p + print( 'Available profiles:') + for p in stream.profiles(): + print( f' \'{p.format()}\' {p.width()}x{p.height()} @ {p.frequency()} FPS' ) + raise RuntimeError( f"'{stream.name()}' profile [{encoding or 'ANY'} {w or 'ANY'}x{h or 'ANY'} @ {fps or 'ANY'} FPS] not found" ) i( device.n_streams(), 'streams available' ) stream_profiles = {} for stream in device.streams(): - #profiles = stream.profiles() if stream.name() not in streams: continue - - #stream.default_profile().to_string()[1:-1] - profile = [fps,type_format[stream.type_string()],width,height] - i( f' {stream.sensor_name()} / {stream.name()} {profile}' ) - stream_profiles[stream.name()] = profile + profile = find_profile( stream, width, height, encoding or type_format[stream.type_string()], fps ) + i( f' {stream.sensor_name()} / {stream.name()} {profile.details_to_string()}' ) + stream_profiles[stream.name()] = profile.to_json() device.send_control( { 'id': 'open-streams', 'stream-profiles': stream_profiles }, True ) @@ -125,14 +149,14 @@ def on_image( stream, image, sample ): stream.start_streaming() # Wait until we have at least one frame from each -tries = 5 +tries = 50 while tries > 0: for n in n_stream_frames.values(): if n <= 0: break else: break - time.sleep( 1 ) + time.sleep( .1 ) tries -= 1 else: raise RuntimeError( f'timed out waiting for all frames to arrive {n_stream_frames}' ) From a188c1393f222ee36ed1e9b1b346ec8b09ecaf94 Mon Sep 17 00:00:00 2001 From: Eran Date: Wed, 25 Sep 2024 13:56:19 +0300 Subject: [PATCH 33/75] add Y12I to pyrealdds.video_encoding.y12i --- third-party/realdds/py/pyrealdds.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/third-party/realdds/py/pyrealdds.cpp b/third-party/realdds/py/pyrealdds.cpp index 930cfa332f..314440496d 100644 --- a/third-party/realdds/py/pyrealdds.cpp +++ b/third-party/realdds/py/pyrealdds.cpp @@ -832,6 +832,7 @@ PYBIND11_MODULE(NAME, m) { video_encoding.attr( "yuyv" ) = dds_video_encoding( "yuv422_yuy2" ); video_encoding.attr( "uyvy" ) = dds_video_encoding( "uyvy" ); video_encoding.attr( "rgb" ) = dds_video_encoding( "rgb8" ); + video_encoding.attr( "y12i" ) = dds_video_encoding( "Y12I" ); using realdds::dds_stream_profile; py::class_< dds_stream_profile, std::shared_ptr< dds_stream_profile > > stream_profile_base( m, "stream_profile" ); From 7ed65117100271d941a6105aad73c21387fa4f6c Mon Sep 17 00:00:00 2001 From: Eran Date: Thu, 26 Sep 2024 12:44:56 +0300 Subject: [PATCH 34/75] add device-connected or -disconnected debug msgs to DDS --- src/backend-device-factory.cpp | 4 ++-- src/dds/rsdds-device-factory.cpp | 6 ++++++ 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/backend-device-factory.cpp b/src/backend-device-factory.cpp index 6157cba595..44ea5d8d0a 100644 --- a/src/backend-device-factory.cpp +++ b/src/backend-device-factory.cpp @@ -145,14 +145,14 @@ backend_device_factory::backend_device_factory( std::shared_ptr< context > const for( auto & device_removed : subtract_sets( old_list, new_list ) ) { devices_removed.push_back( device_removed ); - LOG_DEBUG( "Device disconnected: " << device_removed->get_address() ); + LOG_INFO( "Device disconnected: " << device_removed->get_address() ); } std::vector< std::shared_ptr< device_info > > devices_added; for( auto & device_added : subtract_sets( new_list, old_list ) ) { devices_added.push_back( device_added ); - LOG_DEBUG( "Device connected: " << device_added->get_address() ); + LOG_INFO( "Device connected: " << device_added->get_address() ); } if( devices_removed.size() + devices_added.size() ) diff --git a/src/dds/rsdds-device-factory.cpp b/src/dds/rsdds-device-factory.cpp index b313330faa..0338443dbb 100644 --- a/src/dds/rsdds-device-factory.cpp +++ b/src/dds/rsdds-device-factory.cpp @@ -157,9 +157,15 @@ rsdds_device_factory::rsdds_device_factory( std::shared_ptr< context > const & c std::vector< std::shared_ptr< device_info > > infos_removed; auto dev_info = std::make_shared< dds_device_info >( ctx, dev ); if( added ) + { infos_added.push_back( dev_info ); + LOG_INFO( "Device connected: " << dev_info->get_address() ); + } else + { infos_removed.push_back( dev_info ); + LOG_INFO( "Device disconnected: " << dev_info->get_address() ); + } cb( infos_removed, infos_added ); } ); } From 5f164215845ee24a6cb9c7b9c847f8ecec022ae1 Mon Sep 17 00:00:00 2001 From: Eran Date: Thu, 26 Sep 2024 12:46:34 +0300 Subject: [PATCH 35/75] remove spurious debug log messages no longer needed --- src/context.cpp | 8 ++++---- src/dds/rs-dds-device-proxy.cpp | 10 +++++----- src/dds/rs-dds-sensor-proxy.cpp | 2 +- src/environment.cpp | 2 +- src/proc/formats-converter.cpp | 4 ++-- third-party/realdds/src/dds-device-impl.cpp | 4 ++-- 6 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/context.cpp b/src/context.cpp index bcd47a95a1..b02ed8ea61 100644 --- a/src/context.cpp +++ b/src/context.cpp @@ -116,7 +116,7 @@ namespace librealsense { { for( auto & dev_info : factory->query_devices( requested_mask ) ) { - LOG_INFO( "... " << dev_info->get_address() ); + LOG_DEBUG( "... " << dev_info->get_address() ); list.push_back( dev_info ); } } @@ -124,12 +124,12 @@ namespace librealsense { { if( auto dev_info = item.second.lock() ) { - LOG_INFO( "... " << dev_info->get_address() ); + LOG_DEBUG( "... " << dev_info->get_address() ); list.push_back( dev_info ); } } - LOG_INFO( "Found " << list.size() << " RealSense devices (0x" << std::hex << requested_mask << " requested & 0x" - << get_device_mask() << " from device-mask in settings)" << std::dec ); + LOG_DEBUG( "Found " << list.size() << " RealSense devices (0x" << std::hex << requested_mask + << " requested & 0x" << get_device_mask() << " from device-mask in settings)" << std::dec ); return list; } diff --git a/src/dds/rs-dds-device-proxy.cpp b/src/dds/rs-dds-device-proxy.cpp index 0f540a6650..6628d1c884 100644 --- a/src/dds/rs-dds-device-proxy.cpp +++ b/src/dds/rs-dds-device-proxy.cpp @@ -149,7 +149,7 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const & , auto_calibrated_proxy() , _dds_dev( dev ) { - LOG_DEBUG( "=====> dds-device-proxy " << this << " created on top of dds-device " << _dds_dev.get() ); + //LOG_DEBUG( "=====> dds-device-proxy " << this << " created on top of dds-device " << _dds_dev.get() ); register_info( RS2_CAMERA_INFO_NAME, dev->device_info().name() ); register_info( RS2_CAMERA_INFO_PHYSICAL_PORT, dev->device_info().topic_root() ); register_info( RS2_CAMERA_INFO_PRODUCT_ID, "DDS" ); @@ -236,8 +236,8 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const & sensor_info.proxy->add_dds_stream( sidx, stream ); _stream_name_to_owning_sensor[stream->name()] = sensor_info.proxy; type_and_index_to_dds_stream_sidx.insert( { type_and_index, sidx } ); - LOG_DEBUG( sidx.to_string() << " " << get_string( sensor_info.type ) << " '" << stream->sensor_name() - << "' : '" << stream->name() << "' " << get_string( stream_type ) ); + //LOG_DEBUG( sidx.to_string() << " " << get_string( sensor_info.type ) << " '" << stream->sensor_name() + // << "' : '" << stream->name() << "' " << get_string( stream_type ) ); software_sensor & sensor = get_software_sensor( sensor_info.sensor_index ); auto video_stream = std::dynamic_pointer_cast< realdds::dds_video_stream >( stream ); @@ -281,7 +281,7 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const & for( auto & sensor_info : sensor_name_to_info ) { - LOG_DEBUG( sensor_info.first ); + //LOG_DEBUG( sensor_info.first ); // Set profile's ID based on the dds_stream's ID (index already set). Connect the profile to the extrinsics graph. for( auto & profile : sensor_info.second.proxy->get_stream_profiles() ) @@ -348,7 +348,7 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const & auto const dds_extr = _dds_dev->get_extrinsics( from_stream.first, to_stream.first ); if( ! dds_extr ) { - LOG_DEBUG( "missing extrinsics from " << from_stream.first << " to " << to_stream.first ); + //LOG_DEBUG( "missing extrinsics from " << from_stream.first << " to " << to_stream.first ); continue; } rs2_extrinsics extr = to_rs2_extrinsics( dds_extr ); diff --git a/src/dds/rs-dds-sensor-proxy.cpp b/src/dds/rs-dds-sensor-proxy.cpp index 4a0d7f0c9a..f4cfb7caa6 100644 --- a/src/dds/rs-dds-sensor-proxy.cpp +++ b/src/dds/rs-dds-sensor-proxy.cpp @@ -609,7 +609,7 @@ void dds_sensor_proxy::add_option( std::shared_ptr< realdds::dds_option > option if( get_option_handler( option_id ) ) throw std::runtime_error( "option '" + option->get_name() + "' already exists in sensor" ); - LOG_DEBUG( "... option -> " << option->get_name() ); + //LOG_DEBUG( "... option -> " << option->get_name() ); auto opt = std::make_shared< rs_dds_option >( option, [=]( json value ) diff --git a/src/environment.cpp b/src/environment.cpp index a01eec497a..d597c21a36 100644 --- a/src/environment.cpp +++ b/src/environment.cpp @@ -133,7 +133,7 @@ namespace librealsense } if (!invalid_ids.empty()) - LOG_INFO("Found " << invalid_ids.size() << " unreachable streams, " << std::dec << counter << " extrinsics deleted"); + LOG_DEBUG("Found " << invalid_ids.size() << " unreachable streams, " << std::dec << counter << " extrinsics deleted"); } int extrinsics_graph::find_stream_profile(const stream_interface& p, bool add_if_not_there) diff --git a/src/proc/formats-converter.cpp b/src/proc/formats-converter.cpp index 1e7ee3c2f4..baf15d7f02 100644 --- a/src/proc/formats-converter.cpp +++ b/src/proc/formats-converter.cpp @@ -97,7 +97,7 @@ stream_profiles formats_converter::get_all_possible_profiles( const stream_profi for( auto & raw_profile : raw_profiles ) { - LOG_DEBUG( "Raw profile: " << raw_profile ); + //LOG_DEBUG( "Raw profile: " << raw_profile ); for( auto & pbf : _pb_factories ) { const auto & sources = pbf->get_source_info(); @@ -129,7 +129,7 @@ stream_profiles formats_converter::get_all_possible_profiles( const stream_profi target.resolution_transform( width, height ); cloned_vsp->set_dims( width, height ); } - LOG_DEBUG( " -> " << cloned_profile ); + //LOG_DEBUG( " -> " << cloned_profile ); // Cache pbf supported profiles for efficiency in find_pbf_matching_most_profiles _pbf_supported_profiles[pbf.get()].push_back( cloned_profile ); diff --git a/third-party/realdds/src/dds-device-impl.cpp b/third-party/realdds/src/dds-device-impl.cpp index 9fc6aefe51..6dd10f1511 100644 --- a/third-party/realdds/src/dds-device-impl.cpp +++ b/third-party/realdds/src/dds-device-impl.cpp @@ -590,7 +590,7 @@ void dds_device::impl::on_device_header( json const & j, dds_sample const & samp { std::string const & from_name = ex[0].string_ref(); std::string const & to_name = ex[1].string_ref(); - LOG_DEBUG( "[" << debug_name() << "] ... got extrinsics from " << from_name << " to " << to_name ); + //LOG_DEBUG( "[" << debug_name() << "] ... got extrinsics from " << from_name << " to " << to_name ); extrinsics extr = extrinsics::from_json( ex[2] ); _extrinsics_map[std::make_pair( from_name, to_name )] = std::make_shared< extrinsics >( extr ); } @@ -700,7 +700,7 @@ void dds_device::impl::on_stream_options( json const & j, dds_sample const & sam dds_options options; for( auto & option_j : options_j ) { - LOG_DEBUG( "[" << debug_name() << "] ... " << option_j ); + //LOG_DEBUG( "[" << debug_name() << "] ... " << option_j ); auto option = dds_option::from_json( option_j ); options.push_back( option ); } From 486594207299df4994b1cd6d70bdbc60d9412e34 Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Mon, 30 Sep 2024 09:35:06 +0300 Subject: [PATCH 36/75] disable -Werror on libcurl build --- CMake/external_libcurl.cmake | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CMake/external_libcurl.cmake b/CMake/external_libcurl.cmake index 0f8580d4f1..69a068ea1a 100644 --- a/CMake/external_libcurl.cmake +++ b/CMake/external_libcurl.cmake @@ -1,5 +1,7 @@ if(CHECK_FOR_UPDATES) + string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") + string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") include(ExternalProject) message(STATUS "Building libcurl enabled") @@ -61,4 +63,6 @@ if(CHECK_FOR_UPDATES) endif() endif() + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") endif() #CHECK_FOR_UPDATES From a39b5afd8cf9ebea43d1906a2e8831f95d9aedc1 Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Mon, 30 Sep 2024 10:33:21 +0300 Subject: [PATCH 37/75] try ignore warning on release flow --- CMake/unix_config.cmake | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CMake/unix_config.cmake b/CMake/unix_config.cmake index 245781ab55..ca923fd92d 100644 --- a/CMake/unix_config.cmake +++ b/CMake/unix_config.cmake @@ -78,6 +78,8 @@ macro(os_set_flags) set(ADDITIONAL_COMPILER_FLAGS "-Wformat -Wformat-security -fPIC -fcf-protection=none -mfunction-return=thunk -mindirect-branch=thunk -mindirect-branch-register -fstack-protector") endif() set(CMAKE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS} -pie") + + set(ADDITIONAL_COMPILER_FLAGS "${ADDITIONAL_COMPILER_FLAGS} -Wno-error=stringop-overflow") string(FIND "${CMAKE_CXX_FLAGS}" "-D_FORTIFY_SOURCE" _index) if (${_index} EQUAL -1) # Define D_FORTIFY_SOURCE is undefined From 7d02b472c78b7e0a17a207a8064f86542a56f3e4 Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Mon, 30 Sep 2024 16:34:56 +0300 Subject: [PATCH 38/75] add flush after printf --- tools/fw-update/rs-fw-update.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/tools/fw-update/rs-fw-update.cpp b/tools/fw-update/rs-fw-update.cpp index 743e97a276..a8a008d274 100644 --- a/tools/fw-update/rs-fw-update.cpp +++ b/tools/fw-update/rs-fw-update.cpp @@ -109,6 +109,7 @@ void update(rs2::update_device fwu_dev, std::vector fw_image) fwu_dev.update(fw_image, [&](const float progress) { printf("\rFirmware update progress: %d[%%]", (int)(progress * 100)); + std::cout.flush(); }); } else @@ -425,6 +426,7 @@ try { flash = d.as< rs2::updatable >().create_flash_backup( [&]( const float progress ) { printf( "\rFlash backup progress: %d[%%]", (int)( progress * 100 ) ); + std::cout.flush(); } ); } else @@ -488,6 +490,7 @@ try d.as().update_unsigned( fw_image, [&]( const float progress ) { printf( "\rFirmware update progress: %d[%%]", (int)( progress * 100 ) ); + std::cout.flush(); } ); } else From 41e5f29d7ff8d64976857103c1d2140b289b7ae2 Mon Sep 17 00:00:00 2001 From: fateshelled <53618876+fateshelled@users.noreply.github.com> Date: Thu, 3 Oct 2024 23:44:37 +0900 Subject: [PATCH 39/75] NEON support `pointcloud` --- src/proc/neon/CMakeLists.txt | 1 + src/proc/neon/neon-pointcloud.cpp | 242 ++++++++++++++++++++++++++++++ src/proc/neon/neon-pointcloud.h | 45 ++++++ src/proc/pointcloud.cpp | 8 +- 4 files changed, 293 insertions(+), 3 deletions(-) create mode 100644 src/proc/neon/neon-pointcloud.cpp create mode 100644 src/proc/neon/neon-pointcloud.h diff --git a/src/proc/neon/CMakeLists.txt b/src/proc/neon/CMakeLists.txt index c344b12619..9dd1b72856 100644 --- a/src/proc/neon/CMakeLists.txt +++ b/src/proc/neon/CMakeLists.txt @@ -3,4 +3,5 @@ target_sources(${LRS_TARGET} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/image-neon.cpp" + "${CMAKE_CURRENT_LIST_DIR}/neon-pointcloud.cpp" ) diff --git a/src/proc/neon/neon-pointcloud.cpp b/src/proc/neon/neon-pointcloud.cpp new file mode 100644 index 0000000000..0ded91417a --- /dev/null +++ b/src/proc/neon/neon-pointcloud.cpp @@ -0,0 +1,242 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#include + +#include "neon-pointcloud.h" + +#include + +#if defined(__ARM_NEON) && ! defined ANDROID +#include + +namespace librealsense +{ + template + static inline void distorte_x_y( + const float32x4_t &x, const float32x4_t &y, + float32x4_t *distorted_x, float32x4_t *distorted_y, const float32x4_t(&c)[5]) + { + *distorted_x = x; + *distorted_y = y; + } + + template <> + inline void distorte_x_y( + const float32x4_t &x, const float32x4_t &y, + float32x4_t *distorted_x, float32x4_t *distorted_y, const float32x4_t(&c)[5]) + { + const auto one = vdupq_n_f32(1); + const auto two = vdupq_n_f32(2); + + // r2 = x * x + y * y + const auto r2 = vfmaq_f32(vmulq_f32(x, x), y, y); + // f = 1 + c[0] * r2 + c[1] * r2 ^ 2 + c[4] * r2 ^ 3 + // = 1 + r2 * (c[0] + r2 * (c[1] + r2 * c[4])) + const auto f = vfmaq_f32(one, r2, vfmaq_f32(c[0], r2, vfmaq_f32(c[1], r2, c[4]))); + + const auto x_f = vmulq_f32(x, f); + const auto y_f = vmulq_f32(y, f); + + // dx = x_f + 2 * c[2] * x_f * y_f + c[3] * (r2 + 2 * x_f * x_f) + // = x_f * (1 + 2 * c[2] * y_f + c[3] * 2 * x_f) + c[3] * r2 + // = x_f * (1 + 2 * (c[2] * y_f + c[3] * x_f)) + c[3] * r2 + *distorted_x = vfmaq_f32(vmulq_f32(x_f, vfmaq_f32(one, two, vfmaq_f32(vmulq_f32(c[2], y_f), c[3], x_f))), c[3], r2); + + // dy = y_f + 2 * c[3] * x_f * y_f + c[2] * (r2 + 2 * y_f * y_f) + // = y_f * (1 + 2 * c[3] * x_f + c[2] * 2 * y_f) + c[2] * r2 + // = y_f * (1 + 2 * (c[3] * x_f + c[2] * y_f)) + c[2] * r2 + *distorted_y = vfmaq_f32(vmulq_f32(y_f, vfmaq_f32(one, two, vfmaq_f32(vmulq_f32(c[3], x_f), c[2], y_f))), c[2], r2); + } + + pointcloud_neon::pointcloud_neon() : pointcloud("Pointcloud (NEON)") {} + + void pointcloud_neon::preprocess() + { + _pre_compute_map_x.resize(_depth_intrinsics->width * _depth_intrinsics->height); + _pre_compute_map_y.resize(_depth_intrinsics->width * _depth_intrinsics->height); + + for (int h = 0; h < _depth_intrinsics->height; ++h) + { + for (int w = 0; w < _depth_intrinsics->width; ++w) + { + const float pixel[] = {(float)w, (float)h}; + + float x = (pixel[0] - _depth_intrinsics->ppx) / _depth_intrinsics->fx; + float y = (pixel[1] - _depth_intrinsics->ppy) / _depth_intrinsics->fy; + + if (_depth_intrinsics->model == RS2_DISTORTION_INVERSE_BROWN_CONRADY) + { + const float r2 = x * x + y * y; + const float f = 1.0f + _depth_intrinsics->coeffs[0] * r2 + _depth_intrinsics->coeffs[1] * r2 * r2 + _depth_intrinsics->coeffs[4] * r2 * r2 * r2; + const float ux = x * f + 2.0f * _depth_intrinsics->coeffs[2] * x * y + _depth_intrinsics->coeffs[3] * (r2 + 2.0f * x * x); + const float uy = y * f + 2.0f * _depth_intrinsics->coeffs[3] * x * y + _depth_intrinsics->coeffs[2] * (r2 + 2.0f * y * y); + x = ux; + y = uy; + } + + _pre_compute_map_x[h * _depth_intrinsics->width + w] = x; + _pre_compute_map_y[h * _depth_intrinsics->width + w] = y; + } + } + } + + const float3 *pointcloud_neon::depth_to_points(rs2::points output, + const rs2_intrinsics &depth_intrinsics, + const rs2::depth_frame &depth_frame) + { + auto depth_image = (const uint16_t *)depth_frame.get_data(); + + float *pre_compute_x = _pre_compute_map_x.data(); + float *pre_compute_y = _pre_compute_map_y.data(); + + const uint32_t size = depth_intrinsics.height * depth_intrinsics.width; + + auto points = (float *)output.get_vertices(); + const auto scale = vdupq_n_f32(depth_frame.get_units()); + + for (unsigned int i = 0; i < size; i += 8) + { + const auto x0 = vld1q_f32(pre_compute_x + i); + const auto x1 = vld1q_f32(pre_compute_x + i + 4); + + const auto y0 = vld1q_f32(pre_compute_y + i); + const auto y1 = vld1q_f32(pre_compute_y + i + 4); + + const auto d = vld1q_u16(depth_image + i); + const auto depth0 = vmulq_f32(vcvtq_f32_s32((int32x4_t)vmovl_u16(vget_low_u16(d))), scale); + const auto depth1 = vmulq_f32(vcvtq_f32_s32((int32x4_t)vmovl_u16(vget_high_u16(d))), scale); + + // calculate 3D points + float32x4x3_t xyz0; + xyz0.val[0] = vmulq_f32(depth0, x0); + xyz0.val[1] = vmulq_f32(depth0, y0); + xyz0.val[2] = depth0; + vst3q_f32(&points[0], xyz0); + + float32x4x3_t xyz1; + xyz1.val[0] = vmulq_f32(depth1, x1); + xyz1.val[1] = vmulq_f32(depth1, y1); + xyz1.val[2] = depth1; + vst3q_f32(&points[12], xyz1); + + points += 24; + } + return (float3 *)output.get_vertices(); + } + + template + void pointcloud_neon::get_texture_map_neon(float2 *texture_map, + const float3 *points, + const unsigned int width, + const unsigned int height, + const rs2_intrinsics &other_intrinsics, + const rs2_extrinsics &extr, + float2 *pixels_ptr) + { + auto point = reinterpret_cast(points); + auto res = reinterpret_cast(texture_map); + auto res1 = reinterpret_cast(pixels_ptr); + + float32x4_t r[9]; + float32x4_t t[3]; + float32x4_t c[5]; + for (int i = 0; i < 9; ++i) + { + r[i] = vdupq_n_f32(extr.rotation[i]); + } + for (int i = 0; i < 3; ++i) + { + t[i] = vdupq_n_f32(extr.translation[i]); + } + for (int i = 0; i < 5; ++i) + { + c[i] = vdupq_n_f32(other_intrinsics.coeffs[i]); + } + const auto fx = vdupq_n_f32(other_intrinsics.fx); + const auto fy = vdupq_n_f32(other_intrinsics.fy); + const auto ppx = vdupq_n_f32(other_intrinsics.ppx); + const auto ppy = vdupq_n_f32(other_intrinsics.ppy); + const auto w = vdupq_n_f32(float(other_intrinsics.width)); + const auto h = vdupq_n_f32(float(other_intrinsics.height)); + const auto zero = vdupq_n_f32(0.0f); + + const uint32_t size = height * width * 3; + for (uint32_t i = 0; i < size; i+=12) + { + // load 4 points (x,y,z) + const float32x4x3_t xyz = vld3q_f32(point + i); + + // transform to other + auto p_x = vfmaq_f32(vfmaq_f32(vfmaq_f32(t[0], r[6], xyz.val[2]), r[3], xyz.val[1]), r[0], xyz.val[0]); + auto p_y = vfmaq_f32(vfmaq_f32(vfmaq_f32(t[1], r[7], xyz.val[2]), r[4], xyz.val[1]), r[1], xyz.val[0]); + auto p_z = vfmaq_f32(vfmaq_f32(vfmaq_f32(t[2], r[8], xyz.val[2]), r[5], xyz.val[1]), r[2], xyz.val[0]); + + p_x = vdivq_f32(p_x, p_z); + p_y = vdivq_f32(p_y, p_z); + + distorte_x_y(p_x, p_y, &p_x, &p_y, c); + + p_x = vfmaq_f32(ppx, p_x, fx); + p_y = vfmaq_f32(ppy, p_y, fy); + + // zero the x and y if z is zero + { + const uint32x4_t gt_zero = vcgtq_f32(p_z, zero); + p_x = vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(p_x), gt_zero)); + p_y = vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(p_y), gt_zero)); + } + + // texture_map + { + float32x4x2_t xy; + xy.val[0] = p_x; + xy.val[1] = p_y; + vst2q_f32(res1, xy); + res1 += 8; + } + + // pixels_ptr + { + float32x4x2_t xy; + xy.val[0] = vdivq_f32(p_x, w); + xy.val[1] = vdivq_f32(p_y, h); + vst2q_f32(res, xy); + res += 8; + } + } + } + + void pointcloud_neon::get_texture_map(rs2::points output, + const float3 *points, + const unsigned int width, + const unsigned int height, + const rs2_intrinsics &other_intrinsics, + const rs2_extrinsics &extr, + float2 *pixels_ptr) + { + if (other_intrinsics.model == RS2_DISTORTION_MODIFIED_BROWN_CONRADY) + { + get_texture_map_neon( + (float2 *)output.get_texture_coordinates(), + points, + width, + height, + other_intrinsics, + extr, + pixels_ptr); + } + else + { + get_texture_map_neon( + (float2 *)output.get_texture_coordinates(), + points, + width, + height, + other_intrinsics, + extr, + pixels_ptr); + } + } +} +#endif diff --git a/src/proc/neon/neon-pointcloud.h b/src/proc/neon/neon-pointcloud.h new file mode 100644 index 0000000000..46f1eb7fad --- /dev/null +++ b/src/proc/neon/neon-pointcloud.h @@ -0,0 +1,45 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#pragma once +#include "../pointcloud.h" + +namespace librealsense +{ +#if defined(__ARM_NEON) && ! defined ANDROID + class pointcloud_neon : public pointcloud + { + public: + pointcloud_neon(); + + void preprocess() override; + const float3 * depth_to_points( + rs2::points output, + const rs2_intrinsics &depth_intrinsics, + const rs2::depth_frame& depth_frame) override; + void get_texture_map( + rs2::points output, + const float3* points, + const unsigned int width, + const unsigned int height, + const rs2_intrinsics &other_intrinsics, + const rs2_extrinsics& extr, + float2* pixels_ptr) override; + + private: + template + void get_texture_map_neon(float2 * texture_map, + const float3 * points, + const unsigned int width, + const unsigned int height, + const rs2_intrinsics & other_intrinsics, + const rs2_extrinsics & extr, + float2 * pixels_ptr); + + std::vector _pre_compute_map_x; + std::vector _pre_compute_map_y; + + void pre_compute_x_y_map(); + }; +#endif +} diff --git a/src/proc/pointcloud.cpp b/src/proc/pointcloud.cpp index 2efbca1217..5b5db2724a 100644 --- a/src/proc/pointcloud.cpp +++ b/src/proc/pointcloud.cpp @@ -22,6 +22,8 @@ #ifdef __SSSE3__ #include "proc/sse/sse-pointcloud.h" #endif +#include "proc/neon/neon-pointcloud.h" + namespace librealsense { @@ -395,13 +397,13 @@ namespace librealsense { #ifdef RS2_USE_CUDA return std::make_shared(); - #else - #ifdef __SSSE3__ + #elif defined(__SSSE3__) return std::make_shared(); + #elif defined(__ARM_NEON) && ! defined ANDROID + return std::make_shared(); #else return std::make_shared(); #endif - #endif } bool pointcloud::run__occlusion_filter(const rs2_extrinsics& extr) From eeaa880a1a4553aab92aedbdcaf724f374e3ad08 Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Sun, 6 Oct 2024 12:06:04 +0300 Subject: [PATCH 40/75] Add a drowning detection example pointer --- examples/readme.md | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/readme.md b/examples/readme.md index 7e0d9bfe43..8b918f2943 100644 --- a/examples/readme.md +++ b/examples/readme.md @@ -57,4 +57,5 @@ For a detailed explanations and API documentation see our [Documentation](../doc 17. [Raspberry Pi Handheld 3D Scanner](https://eleccelerator.com/pi-handheld-3d-scanner/) by [Frank Zhao](https://github.com/frank26080115) 18. [Erwhi Hedgehog](https://gbr1.github.io/erwhi_hedgehog.html) by [Giovanni Bruno](https://github.com/gbr1) - Open-Source Hardware and Software design featuring Intel RealSense, UP-Squared and Intel Movidius technologies 19. [Archery Tower Defence Game](https://github.com/saadisaadi1/Handshot-Tower-Defence) by [Saadi Saadi](https://github.com/saadisaadi1) and [Ali Haj](mailto:alihaj00@outlook.com) - This game uses RealSense camera to control in-game movements. The game was developed as a one semester students project for the Technion Institute of Technology +20. [Real-Time Drowning Detection](https://github.com/talaSaba/intelRealsenseProject) by [Tala Saba](https://github.com/talaSaba) and [TODO](TODO) - This project is designed to detect potential drowning incidents in real-time using a Intel RealSense camera. The game was developed as a one semester students project for the Technion Institute of Technology From 7c4ed546fde5f25312c5080e8e077d0de9123943 Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Sun, 6 Oct 2024 13:56:57 +0300 Subject: [PATCH 41/75] Update names --- examples/readme.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/readme.md b/examples/readme.md index 8b918f2943..6162e70b64 100644 --- a/examples/readme.md +++ b/examples/readme.md @@ -57,5 +57,5 @@ For a detailed explanations and API documentation see our [Documentation](../doc 17. [Raspberry Pi Handheld 3D Scanner](https://eleccelerator.com/pi-handheld-3d-scanner/) by [Frank Zhao](https://github.com/frank26080115) 18. [Erwhi Hedgehog](https://gbr1.github.io/erwhi_hedgehog.html) by [Giovanni Bruno](https://github.com/gbr1) - Open-Source Hardware and Software design featuring Intel RealSense, UP-Squared and Intel Movidius technologies 19. [Archery Tower Defence Game](https://github.com/saadisaadi1/Handshot-Tower-Defence) by [Saadi Saadi](https://github.com/saadisaadi1) and [Ali Haj](mailto:alihaj00@outlook.com) - This game uses RealSense camera to control in-game movements. The game was developed as a one semester students project for the Technion Institute of Technology -20. [Real-Time Drowning Detection](https://github.com/talaSaba/intelRealsenseProject) by [Tala Saba](https://github.com/talaSaba) and [TODO](TODO) - This project is designed to detect potential drowning incidents in real-time using a Intel RealSense camera. The game was developed as a one semester students project for the Technion Institute of Technology +20. [Real-Time Drowning Detection](https://github.com/talaSaba/intelRealsenseProject) by [Tala Saba](https://github.com/talaSaba) and [Raneen Assy](https://github.com/raneen28) - This project is designed to detect a potential drowning incident in real-time using a Intel RealSense camera. The game was developed as a one semester students project for the Technion Institute of Technology From 0acabd3711ac7b02e610f8e88063ecb6e48b0487 Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Mon, 30 Sep 2024 11:04:01 +0300 Subject: [PATCH 42/75] fix C# warning --- CMake/external_fastdds.cmake | 1 + CMake/external_foonathan_memory.cmake | 2 -- CMake/external_json.cmake | 1 - CMake/windows_config.cmake | 1 - wrappers/csharp/Intel.RealSense/Devices/AutoCalibratedDevice.cs | 2 +- 5 files changed, 2 insertions(+), 5 deletions(-) diff --git a/CMake/external_fastdds.cmake b/CMake/external_fastdds.cmake index c3752ae41b..856e0dbfc7 100644 --- a/CMake/external_fastdds.cmake +++ b/CMake/external_fastdds.cmake @@ -70,3 +70,4 @@ endfunction() # Trigger the FastDDS build get_fastdds() + diff --git a/CMake/external_foonathan_memory.cmake b/CMake/external_foonathan_memory.cmake index 41c56018f6..f6c4938267 100644 --- a/CMake/external_foonathan_memory.cmake +++ b/CMake/external_foonathan_memory.cmake @@ -41,6 +41,4 @@ function(get_foonathan_memory) endfunction() - get_foonathan_memory() - diff --git a/CMake/external_json.cmake b/CMake/external_json.cmake index dc76ec96cb..69c84ffeec 100644 --- a/CMake/external_json.cmake +++ b/CMake/external_json.cmake @@ -48,4 +48,3 @@ endfunction() # Trigger the build get_nlohmann_json() - diff --git a/CMake/windows_config.cmake b/CMake/windows_config.cmake index 37cffdf50f..4481173b45 100644 --- a/CMake/windows_config.cmake +++ b/CMake/windows_config.cmake @@ -68,7 +68,6 @@ macro(os_set_flags) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") - set_directory_properties(PROPERTIES DIRECTORY third-party/ COMPILE_OPTIONS "/W0") set_source_files_properties(third-party/*.* PROPERTIES COMPILE_OPTIONS "/W0") diff --git a/wrappers/csharp/Intel.RealSense/Devices/AutoCalibratedDevice.cs b/wrappers/csharp/Intel.RealSense/Devices/AutoCalibratedDevice.cs index 4233d36e75..4b198cda57 100644 --- a/wrappers/csharp/Intel.RealSense/Devices/AutoCalibratedDevice.cs +++ b/wrappers/csharp/Intel.RealSense/Devices/AutoCalibratedDevice.cs @@ -11,7 +11,7 @@ public class AutoCalibratedDevice : CalibratedDevice internal AutoCalibratedDevice(IntPtr dev) : base(dev) { } - public static AutoCalibratedDevice FromDevice(Device dev) + public static new AutoCalibratedDevice FromDevice(Device dev) { object error; if (NativeMethods.rs2_is_device_extendable_to(dev.Handle, Extension.AutoCalibratedDevice, out error) == 0) From 2759a6018c0a3fed72b8d9dc70bbd484ecd369b5 Mon Sep 17 00:00:00 2001 From: Eran Date: Mon, 7 Oct 2024 10:51:08 +0300 Subject: [PATCH 43/75] fixup! fix rs-fw-update wait for device to reconnect --- tools/fw-update/rs-fw-update.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tools/fw-update/rs-fw-update.cpp b/tools/fw-update/rs-fw-update.cpp index 403a7454cb..b72fabe798 100644 --- a/tools/fw-update/rs-fw-update.cpp +++ b/tools/fw-update/rs-fw-update.cpp @@ -100,10 +100,10 @@ std::vector read_firmware_data(bool is_set, const std::string& file_pat } -void update(rs2::update_device fwu_dev, std::vector fw_image) -{ +void update( rs2::update_device fwu_dev, std::vector< uint8_t > const & fw_image ) +{ std::cout << std::endl << "Firmware update started. Please don't disconnect device!"<< std::endl << std::endl; - + if (ISATTY(FILENO(stdout))) { fwu_dev.update(fw_image, [&](const float progress) @@ -525,6 +525,7 @@ try } } + new_device = rs2::device(); // otherwise the wait will exit right away update( new_fw_update_device, fw_image ); done = true; @@ -546,7 +547,6 @@ try std::cout << std::endl << "Waiting for device to reconnect..." << std::endl; std::unique_lock lk(mutex); - new_device = rs2::device(); // otherwise the wait will exit right away cv.wait_for(lk, std::chrono::seconds(WAIT_FOR_DEVICE_TIMEOUT), [&] { return !done || new_device; }); if (done) From 7d02d8c7f8048369e2cccdae11d5e5bf7104ef74 Mon Sep 17 00:00:00 2001 From: Eran Date: Tue, 1 Oct 2024 09:55:54 +0300 Subject: [PATCH 44/75] move fourcc to rsutils --- src/CMakeLists.txt | 1 - src/ds/d400/d400-color.cpp | 9 +-- src/ds/d400/d400-device.cpp | 62 ++++++++++--------- src/ds/d400/d400-motion.cpp | 17 ++--- src/ds/d500/d500-color.cpp | 34 +++++----- src/ds/d500/d500-device.cpp | 5 +- src/ds/ds-motion-common.cpp | 17 ++--- src/fourcc.h | 23 ------- src/hid-sensor.cpp | 14 +++-- src/mf/mf-uvc.cpp | 13 ++-- src/platform-camera.cpp | 22 ++++--- .../rsutils/include/rsutils/type/fourcc.h | 46 ++++++++++++++ third-party/rsutils/src/fourcc.cpp | 23 +++++++ 13 files changed, 175 insertions(+), 111 deletions(-) delete mode 100644 src/fourcc.h create mode 100644 third-party/rsutils/include/rsutils/type/fourcc.h create mode 100644 third-party/rsutils/src/fourcc.cpp diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 1b75328682..78695b58fc 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -135,7 +135,6 @@ target_sources(${LRS_TARGET} "${CMAKE_CURRENT_LIST_DIR}/environment.h" "${CMAKE_CURRENT_LIST_DIR}/firmware-version.h" "${CMAKE_CURRENT_LIST_DIR}/float3.h" - "${CMAKE_CURRENT_LIST_DIR}/fourcc.h" "${CMAKE_CURRENT_LIST_DIR}/log.h" "${CMAKE_CURRENT_LIST_DIR}/error-handling.h" "${CMAKE_CURRENT_LIST_DIR}/firmware_logger_device.h" diff --git a/src/ds/d400/d400-color.cpp b/src/ds/d400/d400-color.cpp index 671c2c6fe4..14f3f090da 100644 --- a/src/ds/d400/d400-color.cpp +++ b/src/ds/d400/d400-color.cpp @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2016 Intel Corporation. All Rights Reserved. +// Copyright(c) 2016-24 Intel Corporation. All Rights Reserved. #include #include "metadata.h" @@ -11,23 +11,24 @@ #include "d400-info.h" #include #include -#include #include #include #include +#include +using rs_fourcc = rsutils::type::fourcc; namespace librealsense { - std::map d400_color_fourcc_to_rs2_format = { + std::map d400_color_fourcc_to_rs2_format = { {rs_fourcc('Y','U','Y','2'), RS2_FORMAT_YUYV}, {rs_fourcc('Y','U','Y','V'), RS2_FORMAT_YUYV}, {rs_fourcc('U','Y','V','Y'), RS2_FORMAT_UYVY}, {rs_fourcc('M','J','P','G'), RS2_FORMAT_MJPEG}, {rs_fourcc('B','Y','R','2'), RS2_FORMAT_RAW16} }; - std::map d400_color_fourcc_to_rs2_stream = { + std::map d400_color_fourcc_to_rs2_stream = { {rs_fourcc('Y','U','Y','2'), RS2_STREAM_COLOR}, {rs_fourcc('Y','U','Y','V'), RS2_STREAM_COLOR}, {rs_fourcc('U','Y','V','Y'), RS2_STREAM_COLOR}, diff --git a/src/ds/d400/d400-device.cpp b/src/ds/d400/d400-device.cpp index 184c51f2e4..f78f914566 100644 --- a/src/ds/d400/d400-device.cpp +++ b/src/ds/d400/d400-device.cpp @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2016 Intel Corporation. All Rights Reserved. +// Copyright(c) 2016-24 Intel Corporation. All Rights Reserved. #include #include @@ -19,7 +19,6 @@ #include "d400-color.h" #include "d400-nonmonochrome.h" #include -#include #include #include @@ -37,6 +36,9 @@ #include #include +#include +using rsutils::type::fourcc; + #include #include #include @@ -52,36 +54,36 @@ constexpr bool hw_mon_over_xu = false; namespace librealsense { - std::map d400_depth_fourcc_to_rs2_format = { - {rs_fourcc('Y','U','Y','2'), RS2_FORMAT_YUYV}, - {rs_fourcc('Y','U','Y','V'), RS2_FORMAT_YUYV}, - {rs_fourcc('U','Y','V','Y'), RS2_FORMAT_UYVY}, - {rs_fourcc('G','R','E','Y'), RS2_FORMAT_Y8}, - {rs_fourcc('Y','8','I',' '), RS2_FORMAT_Y8I}, - {rs_fourcc('W','1','0',' '), RS2_FORMAT_W10}, - {rs_fourcc('Y','1','6',' '), RS2_FORMAT_Y16}, - {rs_fourcc('Y','1','2','I'), RS2_FORMAT_Y12I}, - {rs_fourcc('Z','1','6',' '), RS2_FORMAT_Z16}, - {rs_fourcc('Z','1','6','H'), RS2_FORMAT_Z16H}, - {rs_fourcc('R','G','B','2'), RS2_FORMAT_BGR8}, - {rs_fourcc('M','J','P','G'), RS2_FORMAT_MJPEG}, - {rs_fourcc('B','Y','R','2'), RS2_FORMAT_RAW16} + std::map d400_depth_fourcc_to_rs2_format = { + {fourcc('Y','U','Y','2'), RS2_FORMAT_YUYV}, + {fourcc('Y','U','Y','V'), RS2_FORMAT_YUYV}, + {fourcc('U','Y','V','Y'), RS2_FORMAT_UYVY}, + {fourcc('G','R','E','Y'), RS2_FORMAT_Y8}, + {fourcc('Y','8','I',' '), RS2_FORMAT_Y8I}, + {fourcc('W','1','0',' '), RS2_FORMAT_W10}, + {fourcc('Y','1','6',' '), RS2_FORMAT_Y16}, + {fourcc('Y','1','2','I'), RS2_FORMAT_Y12I}, + {fourcc('Z','1','6',' '), RS2_FORMAT_Z16}, + {fourcc('Z','1','6','H'), RS2_FORMAT_Z16H}, + {fourcc('R','G','B','2'), RS2_FORMAT_BGR8}, + {fourcc('M','J','P','G'), RS2_FORMAT_MJPEG}, + {fourcc('B','Y','R','2'), RS2_FORMAT_RAW16} }; - std::map d400_depth_fourcc_to_rs2_stream = { - {rs_fourcc('Y','U','Y','2'), RS2_STREAM_COLOR}, - {rs_fourcc('Y','U','Y','V'), RS2_STREAM_COLOR}, - {rs_fourcc('U','Y','V','Y'), RS2_STREAM_INFRARED}, - {rs_fourcc('G','R','E','Y'), RS2_STREAM_INFRARED}, - {rs_fourcc('Y','8','I',' '), RS2_STREAM_INFRARED}, - {rs_fourcc('W','1','0',' '), RS2_STREAM_INFRARED}, - {rs_fourcc('Y','1','6',' '), RS2_STREAM_INFRARED}, - {rs_fourcc('Y','1','2','I'), RS2_STREAM_INFRARED}, - {rs_fourcc('R','G','B','2'), RS2_STREAM_INFRARED}, - {rs_fourcc('Z','1','6',' '), RS2_STREAM_DEPTH}, - {rs_fourcc('Z','1','6','H'), RS2_STREAM_DEPTH}, - {rs_fourcc('B','Y','R','2'), RS2_STREAM_COLOR}, - {rs_fourcc('M','J','P','G'), RS2_STREAM_COLOR} + std::map d400_depth_fourcc_to_rs2_stream = { + {fourcc('Y','U','Y','2'), RS2_STREAM_COLOR}, + {fourcc('Y','U','Y','V'), RS2_STREAM_COLOR}, + {fourcc('U','Y','V','Y'), RS2_STREAM_INFRARED}, + {fourcc('G','R','E','Y'), RS2_STREAM_INFRARED}, + {fourcc('Y','8','I',' '), RS2_STREAM_INFRARED}, + {fourcc('W','1','0',' '), RS2_STREAM_INFRARED}, + {fourcc('Y','1','6',' '), RS2_STREAM_INFRARED}, + {fourcc('Y','1','2','I'), RS2_STREAM_INFRARED}, + {fourcc('R','G','B','2'), RS2_STREAM_INFRARED}, + {fourcc('Z','1','6',' '), RS2_STREAM_DEPTH}, + {fourcc('Z','1','6','H'), RS2_STREAM_DEPTH}, + {fourcc('B','Y','R','2'), RS2_STREAM_COLOR}, + {fourcc('M','J','P','G'), RS2_STREAM_COLOR} }; std::vector d400_device::send_receive_raw_data(const std::vector& input) diff --git a/src/ds/d400/d400-motion.cpp b/src/ds/d400/d400-motion.cpp index aa25f4bd0d..9a3a9802ee 100644 --- a/src/ds/d400/d400-motion.cpp +++ b/src/ds/d400/d400-motion.cpp @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2016 Intel Corporation. All Rights Reserved. +// Copyright(c) 2016-24 Intel Corporation. All Rights Reserved. #include "d400-motion.h" @@ -19,18 +19,21 @@ #include "stream.h" #include "proc/motion-transform.h" #include "proc/auto-exposure-processor.h" -#include #include #include -using namespace librealsense; + +#include +using rsutils::type::fourcc; + + namespace librealsense { // D457 development - const std::map motion_fourcc_to_rs2_format = { - {rs_fourcc('G','R','E','Y'), RS2_FORMAT_MOTION_XYZ32F}, + const std::map motion_fourcc_to_rs2_format = { + {fourcc('G','R','E','Y'), RS2_FORMAT_MOTION_XYZ32F}, }; - const std::map motion_fourcc_to_rs2_stream = { - {rs_fourcc('G','R','E','Y'), RS2_STREAM_ACCEL}, + const std::map motion_fourcc_to_rs2_stream = { + {fourcc('G','R','E','Y'), RS2_STREAM_ACCEL}, }; rs2_motion_device_intrinsic d400_motion_base::get_motion_intrinsics(rs2_stream stream) const diff --git a/src/ds/d500/d500-color.cpp b/src/ds/d500/d500-color.cpp index a9ccf23af8..5ae9d96512 100644 --- a/src/ds/d500/d500-color.cpp +++ b/src/ds/d500/d500-color.cpp @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// Copyright(c) 2022-4 Intel Corporation. All Rights Reserved. #include #include "metadata.h" @@ -11,29 +11,31 @@ #include "d500-info.h" #include "backend.h" #include "platform/platform-utils.h" -#include #include #include #include +#include +using rsutils::type::fourcc; + namespace librealsense { - std::map d500_color_fourcc_to_rs2_format = { - {rs_fourcc('Y','U','Y','2'), RS2_FORMAT_YUYV}, - {rs_fourcc('Y','U','Y','V'), RS2_FORMAT_YUYV}, - {rs_fourcc('U','Y','V','Y'), RS2_FORMAT_UYVY}, - {rs_fourcc('M','J','P','G'), RS2_FORMAT_MJPEG}, - {rs_fourcc('B','Y','R','2'), RS2_FORMAT_RAW16}, - {rs_fourcc('M','4','2','0'), RS2_FORMAT_M420} + std::map d500_color_fourcc_to_rs2_format = { + {fourcc('Y','U','Y','2'), RS2_FORMAT_YUYV}, + {fourcc('Y','U','Y','V'), RS2_FORMAT_YUYV}, + {fourcc('U','Y','V','Y'), RS2_FORMAT_UYVY}, + {fourcc('M','J','P','G'), RS2_FORMAT_MJPEG}, + {fourcc('B','Y','R','2'), RS2_FORMAT_RAW16}, + {fourcc('M','4','2','0'), RS2_FORMAT_M420} }; - std::map d500_color_fourcc_to_rs2_stream = { - {rs_fourcc('Y','U','Y','2'), RS2_STREAM_COLOR}, - {rs_fourcc('Y','U','Y','V'), RS2_STREAM_COLOR}, - {rs_fourcc('U','Y','V','Y'), RS2_STREAM_COLOR}, - {rs_fourcc('B','Y','R','2'), RS2_STREAM_COLOR}, - {rs_fourcc('M','J','P','G'), RS2_STREAM_COLOR}, - {rs_fourcc('M','4','2','0'), RS2_STREAM_COLOR} + std::map d500_color_fourcc_to_rs2_stream = { + {fourcc('Y','U','Y','2'), RS2_STREAM_COLOR}, + {fourcc('Y','U','Y','V'), RS2_STREAM_COLOR}, + {fourcc('U','Y','V','Y'), RS2_STREAM_COLOR}, + {fourcc('B','Y','R','2'), RS2_STREAM_COLOR}, + {fourcc('M','J','P','G'), RS2_STREAM_COLOR}, + {fourcc('M','4','2','0'), RS2_STREAM_COLOR} }; d500_color::d500_color( std::shared_ptr< const d500_info > const & dev_info, rs2_format native_format ) diff --git a/src/ds/d500/d500-device.cpp b/src/ds/d500/d500-device.cpp index 60790878cb..8342d7edf7 100644 --- a/src/ds/d500/d500-device.cpp +++ b/src/ds/d500/d500-device.cpp @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// Copyright(c) 2022-4 Intel Corporation. All Rights Reserved. #include "metadata-parser.h" #include "metadata.h" @@ -24,7 +24,8 @@ #include "proc/y8i-to-y8y8.h" #include "proc/y16i-to-y10msby10msb.h" -#include +#include +using rs_fourcc = rsutils::type::fourcc; #include #include diff --git a/src/ds/ds-motion-common.cpp b/src/ds/ds-motion-common.cpp index e91a00b84f..6706655845 100644 --- a/src/ds/ds-motion-common.cpp +++ b/src/ds/ds-motion-common.cpp @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// Copyright(c) 2022-4 Intel Corporation. All Rights Reserved. #include "ds-motion-common.h" @@ -22,22 +22,23 @@ #include "ds-options.h" #include "ds-private.h" #include -#include #include +#include +using rsutils::type::fourcc; #include namespace librealsense { using namespace ds; - const std::map fisheye_fourcc_to_rs2_format = { - {rs_fourcc('R','A','W','8'), RS2_FORMAT_RAW8}, - {rs_fourcc('G','R','E','Y'), RS2_FORMAT_RAW8}, + const std::map fisheye_fourcc_to_rs2_format = { + {fourcc('R','A','W','8'), RS2_FORMAT_RAW8}, + {fourcc('G','R','E','Y'), RS2_FORMAT_RAW8}, }; - const std::map fisheye_fourcc_to_rs2_stream = { - {rs_fourcc('R','A','W','8'), RS2_STREAM_FISHEYE}, - {rs_fourcc('G','R','E','Y'), RS2_STREAM_FISHEYE}, + const std::map fisheye_fourcc_to_rs2_stream = { + {fourcc('R','A','W','8'), RS2_STREAM_FISHEYE}, + {fourcc('G','R','E','Y'), RS2_STREAM_FISHEYE}, }; void fisheye_auto_exposure_roi_method::set(const region_of_interest& roi) diff --git a/src/fourcc.h b/src/fourcc.h deleted file mode 100644 index f3403fe9c8..0000000000 --- a/src/fourcc.h +++ /dev/null @@ -1,23 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2023 Intel Corporation. All Rights Reserved. -#pragma once - -#include -#include - - -namespace librealsense { - - -template< typename T > -uint32_t rs_fourcc( const T a, const T b, const T c, const T d ) -{ - static_assert( ( std::is_integral< T >::value ), "rs_fourcc supports integral built-in types only" ); - return ( ( static_cast< uint32_t >( a ) << 24 ) - | ( static_cast< uint32_t >( b ) << 16 ) - | ( static_cast< uint32_t >( c ) << 8 ) - | ( static_cast< uint32_t >( d ) << 0 ) ); -} - - -} // namespace librealsense diff --git a/src/hid-sensor.cpp b/src/hid-sensor.cpp index 03f831aeeb..44dd4a2778 100644 --- a/src/hid-sensor.cpp +++ b/src/hid-sensor.cpp @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2023 Intel Corporation. All Rights Reserved. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. #include "hid-sensor.h" #include "device.h" @@ -7,18 +7,20 @@ #include "global_timestamp_reader.h" #include "metadata.h" #include "platform/stream-profile-impl.h" -#include "fourcc.h" #include #include +#include +using rsutils::type::fourcc; + namespace librealsense { -static const std::map< rs2_stream, uint32_t > stream_and_fourcc - = { { RS2_STREAM_GYRO, rs_fourcc( 'G', 'Y', 'R', 'O' ) }, - { RS2_STREAM_ACCEL, rs_fourcc( 'A', 'C', 'C', 'L' ) }, - { RS2_STREAM_GPIO, rs_fourcc( 'G', 'P', 'I', 'O' ) } }; +static const std::map< rs2_stream, fourcc::value_type > stream_and_fourcc + = { { RS2_STREAM_GYRO, fourcc( 'G', 'Y', 'R', 'O' ) }, + { RS2_STREAM_ACCEL, fourcc( 'A', 'C', 'C', 'L' ) }, + { RS2_STREAM_GPIO, fourcc( 'G', 'P', 'I', 'O' ) } }; /*For gyro sensitivity - FW gets 0 for 61 millidegree/s/LSB resolution 0.1 for 30.5 millidegree/s/LSB diff --git a/src/mf/mf-uvc.cpp b/src/mf/mf-uvc.cpp index 0b5e053a8f..305cfab412 100644 --- a/src/mf/mf-uvc.cpp +++ b/src/mf/mf-uvc.cpp @@ -30,6 +30,8 @@ The library will be compiled without the metadata support!\n") #include // monotonic_to_realtime #include +#include +using rsutils::type::fourcc; #include "Shlwapi.h" #include @@ -970,10 +972,13 @@ namespace librealsense throw std::runtime_error("Device must be powered to query supported profiles!"); std::vector results; - foreach_profile([&results](const mf_profile& mfp, CComPtr media_type, bool& quit) - { - results.push_back(mfp.profile); - }); + foreach_profile( + [&results]( const mf_profile & mfp, CComPtr< IMFMediaType > media_type, bool & quit ) + { + //LOG_DEBUG( mfp.profile.width << 'x' << mfp.profile.height << ' ' << fourcc( mfp.profile.format ) + // << " @ " << mfp.profile.fps << " Hz" ); + results.push_back( mfp.profile ); + } ); return results; } diff --git a/src/platform-camera.cpp b/src/platform-camera.cpp index fe66c246cd..e53ffb7cdf 100644 --- a/src/platform-camera.cpp +++ b/src/platform-camera.cpp @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2023 Intel Corporation. All Rights Reserved. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. #include "platform-camera.h" #include "ds/ds-timestamp.h" @@ -8,23 +8,25 @@ #include "proc/color-formats-converter.h" #include "backend.h" #include "platform/platform-utils.h" -#include #include +#include +using rsutils::type::fourcc; + namespace librealsense { namespace { -const std::map< uint32_t, rs2_format > platform_color_fourcc_to_rs2_format = { - { rs_fourcc( 'Y', 'U', 'Y', '2' ), RS2_FORMAT_YUYV }, - { rs_fourcc( 'Y', 'U', 'Y', 'V' ), RS2_FORMAT_YUYV }, - { rs_fourcc( 'M', 'J', 'P', 'G' ), RS2_FORMAT_MJPEG }, +const std::map< fourcc::value_type, rs2_format > platform_color_fourcc_to_rs2_format = { + { fourcc( 'Y', 'U', 'Y', '2' ), RS2_FORMAT_YUYV }, + { fourcc( 'Y', 'U', 'Y', 'V' ), RS2_FORMAT_YUYV }, + { fourcc( 'M', 'J', 'P', 'G' ), RS2_FORMAT_MJPEG }, }; -const std::map< uint32_t, rs2_stream > platform_color_fourcc_to_rs2_stream = { - { rs_fourcc( 'Y', 'U', 'Y', '2' ), RS2_STREAM_COLOR }, - { rs_fourcc( 'Y', 'U', 'Y', 'V' ), RS2_STREAM_COLOR }, - { rs_fourcc( 'M', 'J', 'P', 'G' ), RS2_STREAM_COLOR }, +const std::map< fourcc::value_type, rs2_stream > platform_color_fourcc_to_rs2_stream = { + { fourcc( 'Y', 'U', 'Y', '2' ), RS2_STREAM_COLOR }, + { fourcc( 'Y', 'U', 'Y', 'V' ), RS2_STREAM_COLOR }, + { fourcc( 'M', 'J', 'P', 'G' ), RS2_STREAM_COLOR }, }; diff --git a/third-party/rsutils/include/rsutils/type/fourcc.h b/third-party/rsutils/include/rsutils/type/fourcc.h new file mode 100644 index 0000000000..ed90b1f74b --- /dev/null +++ b/third-party/rsutils/include/rsutils/type/fourcc.h @@ -0,0 +1,46 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. +#pragma once + +#include +#include + + +namespace rsutils { +namespace type { + + +class fourcc +{ +public: + using value_type = uint32_t; + +private: + value_type _4cc; + +public: + template< typename T > + fourcc( const T a, const T b, const T c, const T d ) + : _4cc( ( static_cast< uint32_t >( a ) << 24 ) | ( static_cast< uint32_t >( b ) << 16 ) + | ( static_cast< uint32_t >( c ) << 8 ) | ( static_cast< uint32_t >( d ) << 0 ) ) + { + } + explicit fourcc( value_type fcc ) + : _4cc( fcc ) + { + } + fourcc( fourcc const & other ) = default; + + value_type get() const noexcept { return _4cc; } + operator value_type() const noexcept { return get(); } + + bool operator==( fourcc const & other ) const noexcept { return get() == other.get(); } + bool operator!=( fourcc const & other ) const noexcept { return ! operator==( other ); } +}; + + +std::ostream & operator<<( std::ostream &, fourcc const & ); + + +} // namespace type +} // namespace rsutils diff --git a/third-party/rsutils/src/fourcc.cpp b/third-party/rsutils/src/fourcc.cpp new file mode 100644 index 0000000000..0d53a74731 --- /dev/null +++ b/third-party/rsutils/src/fourcc.cpp @@ -0,0 +1,23 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#include +#include + + +namespace rsutils { +namespace type { + + +std::ostream & operator<<( std::ostream & os, fourcc const & fcc ) +{ + os << char( fcc.get() >> 24 ); + os << char( fcc.get() >> 16 ); + os << char( fcc.get() >> 8 ); + os << char( fcc.get() ); + return os; +} + + +} // namespace number +} // namespace rsutils From 54c3b1def1ee4a5d01c63d70bd9e31537828243b Mon Sep 17 00:00:00 2001 From: Eran Date: Mon, 7 Oct 2024 09:00:49 +0300 Subject: [PATCH 45/75] add backend_stream_profile::to_stream() --- src/dds/rs-dds-sensor-proxy.cpp | 4 +- src/ds/d400/d400-factory.cpp | 3 +- src/hid-sensor.cpp | 1 + src/media/ros/ros_reader.cpp | 3 +- src/media/ros/ros_reader.h | 3 +- src/platform/stream-profile-impl.h | 15 ++- src/proc/formats-converter.cpp | 2 +- src/proc/synthetic-stream.cpp | 3 +- src/proc/y8i-to-y8y8.cpp | 5 +- src/sensor.cpp | 4 +- src/stream.cpp | 196 ++++++++++++++++------------- src/stream.h | 14 +-- src/uvc-sensor.cpp | 3 +- 13 files changed, 149 insertions(+), 107 deletions(-) diff --git a/src/dds/rs-dds-sensor-proxy.cpp b/src/dds/rs-dds-sensor-proxy.cpp index f4cfb7caa6..4fc99588d0 100644 --- a/src/dds/rs-dds-sensor-proxy.cpp +++ b/src/dds/rs-dds-sensor-proxy.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include @@ -250,9 +251,8 @@ void dds_sensor_proxy::open( const stream_profiles & profiles ) { auto & sp = source_profiles[i]; sid_index sidx( sp->get_unique_id(), sp->get_stream_index() ); - if( Is< video_stream_profile >( sp ) ) + if( auto const vsp = As< video_stream_profile >( sp ) ) { - const auto && vsp = As< video_stream_profile >( source_profiles[i] ); auto video_profile = find_profile( sidx, realdds::dds_video_stream_profile( sp->get_framerate(), diff --git a/src/ds/d400/d400-factory.cpp b/src/ds/d400/d400-factory.cpp index 56428ff9c8..44867a2e97 100644 --- a/src/ds/d400/d400-factory.cpp +++ b/src/ds/d400/d400-factory.cpp @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2016 Intel Corporation. All Rights Reserved. +// Copyright(c) 2016-24 Intel Corporation. All Rights Reserved. #include #include @@ -10,6 +10,7 @@ #include "device.h" #include "image.h" #include "metadata-parser.h" +#include "context.h" #include diff --git a/src/hid-sensor.cpp b/src/hid-sensor.cpp index 44dd4a2778..dd4559a3a2 100644 --- a/src/hid-sensor.cpp +++ b/src/hid-sensor.cpp @@ -4,6 +4,7 @@ #include "hid-sensor.h" #include "device.h" #include "stream.h" +#include "image.h" #include "global_timestamp_reader.h" #include "metadata.h" #include "platform/stream-profile-impl.h" diff --git a/src/media/ros/ros_reader.cpp b/src/media/ros/ros_reader.cpp index 03e1c93084..ce5df08463 100644 --- a/src/media/ros/ros_reader.cpp +++ b/src/media/ros/ros_reader.cpp @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2019 Intel Corporation. All Rights Reserved. +// Copyright(c) 2019-24 Intel Corporation. All Rights Reserved. #include "ros_reader.h" #include "ds/ds-device-common.h" @@ -10,6 +10,7 @@ #include #include #include +#include #include #include diff --git a/src/media/ros/ros_reader.h b/src/media/ros/ros_reader.h index 00ad56e0cd..e71f5bba07 100644 --- a/src/media/ros/ros_reader.h +++ b/src/media/ros/ros_reader.h @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2017 Intel Corporation. All Rights Reserved. +// Copyright(c) 2017-24 Intel Corporation. All Rights Reserved. #pragma once @@ -14,6 +14,7 @@ namespace librealsense { using namespace device_serializer; + class context; class frame_source; class info_container; class options_interface; diff --git a/src/platform/stream-profile-impl.h b/src/platform/stream-profile-impl.h index a0fd77a1e8..953d326ea4 100644 --- a/src/platform/stream-profile-impl.h +++ b/src/platform/stream-profile-impl.h @@ -1,8 +1,10 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2023 Intel Corporation. All Rights Reserved. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. #pragma once #include "stream-profile.h" +#include +#include namespace librealsense { @@ -23,7 +25,16 @@ class stream_profile_impl : public super { } - virtual stream_profile const & get_backend_profile() const override { return _backend_stream_profile; } + stream_profile const & get_backend_profile() const override { return _backend_stream_profile; } + + void to_stream( std::ostream & os ) const override + { + os << " ["; + os << _backend_stream_profile.width << 'x' << _backend_stream_profile.height; + os << ' ' << rsutils::type::fourcc( _backend_stream_profile.format ); + os << " @ " << _backend_stream_profile.fps << " Hz"; + os << "]"; + } }; diff --git a/src/proc/formats-converter.cpp b/src/proc/formats-converter.cpp index baf15d7f02..2c874a40aa 100644 --- a/src/proc/formats-converter.cpp +++ b/src/proc/formats-converter.cpp @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2023 Intel Corporation. All Rights Reserved. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. #include "proc/formats-converter.h" #include "stream.h" diff --git a/src/proc/synthetic-stream.cpp b/src/proc/synthetic-stream.cpp index 179337d457..c49beb62b7 100644 --- a/src/proc/synthetic-stream.cpp +++ b/src/proc/synthetic-stream.cpp @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2017 Intel Corporation. All Rights Reserved. +// Copyright(c) 2017-24 Intel Corporation. All Rights Reserved. #include "proc/synthetic-stream.h" @@ -12,6 +12,7 @@ #include "option.h" #include "stream.h" #include "types.h" +#include #include #include diff --git a/src/proc/y8i-to-y8y8.cpp b/src/proc/y8i-to-y8y8.cpp index c268aa1bc8..92f36f5791 100644 --- a/src/proc/y8i-to-y8y8.cpp +++ b/src/proc/y8i-to-y8y8.cpp @@ -1,9 +1,10 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2019 Intel Corporation. All Rights Reserved. +// Copyright(c) 2019-24 Intel Corporation. All Rights Reserved. #include "y8i-to-y8y8.h" -#include "stream.h" +#include +#include #ifdef RS2_USE_CUDA #include "cuda/cuda-conversion.cuh" diff --git a/src/sensor.cpp b/src/sensor.cpp index d551ea25f7..f051cb2af4 100644 --- a/src/sensor.cpp +++ b/src/sensor.cpp @@ -1,11 +1,13 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2015 Intel Corporation. All Rights Reserved. +// Copyright(c) 2015-24 Intel Corporation. All Rights Reserved. #include "uvc-sensor.h" #include "source.h" #include "device.h" #include "stream.h" +#include "context.h" +#include "image.h" #include "proc/synthetic-stream.h" #include "proc/decimation-filter.h" #include "global_timestamp_reader.h" diff --git a/src/stream.cpp b/src/stream.cpp index 9dcb4d5bca..31fde0bb58 100644 --- a/src/stream.cpp +++ b/src/stream.cpp @@ -1,111 +1,137 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2015 Intel Corporation. All Rights Reserved. +// Copyright(c) 2015-24 Intel Corporation. All Rights Reserved. #include "stream.h" +#include -namespace librealsense + +namespace librealsense { + + +stream::stream( rs2_stream stream_type, int index ) + : _index( index ) + , _type( stream_type ) { - stream::stream(rs2_stream stream_type, int index) - : _index(index), _type(stream_type) - { - _uid = environment::get_instance().generate_stream_id(); - } + _uid = environment::get_instance().generate_stream_id(); +} - int stream::get_stream_index() const - { - return _index; - } +int stream::get_stream_index() const +{ + return _index; +} - void stream::set_stream_index(int index) - { - _index = index; - } +void stream::set_stream_index( int index ) +{ + _index = index; +} - rs2_stream stream::get_stream_type() const - { - return _type; - } +rs2_stream stream::get_stream_type() const +{ + return _type; +} - void stream::set_stream_type(rs2_stream stream) - { - _type = stream; - } +void stream::set_stream_type( rs2_stream stream ) +{ + _type = stream; +} - stream_profile_base::stream_profile_base() - { - _c_ptr = &_c_wrapper; - _c_wrapper.profile = this; - _c_wrapper.clone = nullptr; - } +stream_profile_base::stream_profile_base() +{ + _c_ptr = &_c_wrapper; + _c_wrapper.profile = this; + _c_wrapper.clone = nullptr; +} - int stream_profile_base::get_stream_index() const - { - return _index; - } +int stream_profile_base::get_stream_index() const +{ + return _index; +} - void stream_profile_base::set_stream_index(int index) - { - _index = index; - } +void stream_profile_base::set_stream_index( int index ) +{ + _index = index; +} - rs2_stream stream_profile_base::get_stream_type() const - { - return _type; - } +rs2_stream stream_profile_base::get_stream_type() const +{ + return _type; +} - void stream_profile_base::set_stream_type(rs2_stream stream) - { - _type = stream; - } +void stream_profile_base::set_stream_type( rs2_stream stream ) +{ + _type = stream; +} - rs2_format stream_profile_base::get_format() const - { - return _format; - } +rs2_format stream_profile_base::get_format() const +{ + return _format; +} - void stream_profile_base::set_format(rs2_format format) - { - _format = format; - } +void stream_profile_base::set_format( rs2_format format ) +{ + _format = format; +} - uint32_t stream_profile_base::get_framerate() const - { - return _framerate; - } +uint32_t stream_profile_base::get_framerate() const +{ + return _framerate; +} - void stream_profile_base::set_framerate(uint32_t val) - { - _framerate = val; - } +void stream_profile_base::set_framerate( uint32_t val ) +{ + _framerate = val; +} - int stream_profile_base::get_tag() const - { - return _tag; - } +int stream_profile_base::get_tag() const +{ + return _tag; +} - void stream_profile_base::tag_profile(int tag) - { - _tag = tag; - } +void stream_profile_base::tag_profile( int tag ) +{ + _tag = tag; +} - rs2_stream_profile* stream_profile_base::get_c_wrapper() const - { - return _c_ptr; - } - void stream_profile_base::set_c_wrapper(rs2_stream_profile* wrapper) - { - _c_ptr = wrapper; - } - void stream_profile_base::create_snapshot(std::shared_ptr& snapshot) const - { - auto ptr = std::const_pointer_cast(shared_from_this()); - snapshot = std::dynamic_pointer_cast(ptr); - } - void stream_profile_base::enable_recording(std::function record_action) +rs2_stream_profile * stream_profile_base::get_c_wrapper() const +{ + return _c_ptr; +} + + +void stream_profile_base::set_c_wrapper( rs2_stream_profile * wrapper ) +{ + _c_ptr = wrapper; +} + + +void stream_profile_base::create_snapshot( std::shared_ptr< stream_profile_interface > & snapshot ) const +{ + auto ptr = std::const_pointer_cast< stream_interface >( shared_from_this() ); + snapshot = std::dynamic_pointer_cast< stream_profile_interface >( ptr ); +} + + +void stream_profile_base::enable_recording( std::function< void( const stream_profile_interface & ) > record_action ) +{ + // TODO: implement or remove inheritance from recordable + throw not_implemented_exception( __FUNCTION__ ); +} + + +std::ostream & operator<<( std::ostream & os, const stream_profiles & profiles ) +{ + bool first = true; + for( auto & p : profiles ) { - //TODO: implement or remove inheritance from recordable - throw not_implemented_exception(__FUNCTION__); + if( first ) + first = false; + else + os << ", "; + os << p; } + return os; } + +} // namespace librealsense diff --git a/src/stream.h b/src/stream.h index ad06f56065..d1f82b45e4 100644 --- a/src/stream.h +++ b/src/stream.h @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2015 Intel Corporation. All Rights Reserved. +// Copyright(c) 2015-24 Intel Corporation. All Rights Reserved. #pragma once #include "core/stream-interface.h" @@ -8,16 +8,10 @@ #include "core/motion.h" #include "core/stream-profile.h" #include "core/tagged-profile.h" -#include "context.h" -#include "image.h" +#include "librealsense-exception.h" #include "environment.h" -namespace librealsense -{ - class stream_profile_interface; -} - struct rs2_stream_profile { librealsense::stream_profile_interface * profile; @@ -58,6 +52,8 @@ namespace librealsense throw not_implemented_exception( "not a backend profile!" ); } + virtual void to_stream( std::ostream & ) const {} + virtual ~backend_stream_profile() = default; }; @@ -136,7 +132,7 @@ namespace librealsense auto id = environment::get_instance().generate_stream_id(); res->set_unique_id( id ); - LOG_DEBUG( "video_stream_profile::clone, id= " << id ); + //LOG_DEBUG( "video_stream_profile::clone, id= " << id ); res->set_dims(get_width(), get_height()); std::function int_func = _calc_intrinsics; res->set_intrinsics([int_func]() { return int_func(); }); diff --git a/src/uvc-sensor.cpp b/src/uvc-sensor.cpp index 847647f736..fab565af0f 100644 --- a/src/uvc-sensor.cpp +++ b/src/uvc-sensor.cpp @@ -1,9 +1,10 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2023 Intel Corporation. All Rights Reserved. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. #include "uvc-sensor.h" #include "device.h" #include "stream.h" +#include "image.h" #include "global_timestamp_reader.h" #include "core/video-frame.h" #include "core/notification.h" From d014218423ecd44f41c72f32527d19b707af46c9 Mon Sep 17 00:00:00 2001 From: Eran Date: Mon, 7 Oct 2024 08:54:05 +0300 Subject: [PATCH 46/75] fix problems with type-index mapping --- src/core/stream-profile-interface.h | 14 ++----- src/dds/rs-dds-device-proxy.cpp | 63 +++++++++++++++-------------- src/proc/formats-converter.cpp | 27 ++++++++++--- src/proc/formats-converter.h | 2 + 4 files changed, 61 insertions(+), 45 deletions(-) diff --git a/src/core/stream-profile-interface.h b/src/core/stream-profile-interface.h index 9745e5733d..e60c0ad251 100644 --- a/src/core/stream-profile-interface.h +++ b/src/core/stream-profile-interface.h @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2023 Intel Corporation. All Rights Reserved. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. #pragma once #include "stream-interface.h" @@ -9,7 +9,7 @@ #include #include #include -#include +#include namespace librealsense { @@ -38,14 +38,8 @@ class stream_profile_interface using stream_profiles = std::vector< std::shared_ptr< stream_profile_interface > >; -inline std::ostream & operator<<( std::ostream & os, const stream_profiles & profiles ) -{ - for( auto & p : profiles ) - { - os << rs2_format_to_string( p->get_format() ) << " " << rs2_stream_to_string( p->get_stream_type() ) << ", "; - } - return os; -} +std::ostream & operator<<( std::ostream &, const std::shared_ptr< stream_profile_interface > & ); +std::ostream & operator<<( std::ostream &, const stream_profiles & ); } // namespace librealsense diff --git a/src/dds/rs-dds-device-proxy.cpp b/src/dds/rs-dds-device-proxy.cpp index 6628d1c884..10503106f5 100644 --- a/src/dds/rs-dds-device-proxy.cpp +++ b/src/dds/rs-dds-device-proxy.cpp @@ -176,6 +176,12 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const & std::shared_ptr< dds_sensor_proxy > proxy; int sensor_index = 0; rs2_stream type = RS2_STREAM_ANY; + // dds_streams bear stream type and index information, we add it to a dds_sensor_proxy mapped by a newly generated + // unique ID. After the sensor initialization we get all the "final" profiles from formats-converter with type and + // index but without IDs. We need to find the dds_stream that each profile was created from so we create a map from + // type and index to dds_stream ID and index, because the dds_sensor_proxy holds a map from sidx to dds_stream. We + // need both the ID from that map key and the stream itself (for intrinsics information) + std::map< sid_index, sid_index > type_and_index_to_dds_stream_sidx; }; std::map< std::string, sensor_info > sensor_name_to_info; @@ -185,7 +191,7 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const & { auto & sensor = sensor_name_to_info[stream->sensor_name()]; if( stream->type_string() == "depth" - || stream->type_string() == "infrared" ) + || stream->type_string() == "ir" ) { // If there's depth or infrared, it is a depth sensor regardless of what else is in there // E.g., the D405 has a color stream in the depth sensor @@ -208,13 +214,6 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const & } } ); // End foreach_stream lambda - // dds_streams bear stream type and index information, we add it to a dds_sensor_proxy mapped by a newly generated - // unique ID. After the sensor initialization we get all the "final" profiles from formats-converter with type and - // index but without IDs. We need to find the dds_stream that each profile was created from so we create a map from - // type and index to dds_stream ID and index, because the dds_sensor_proxy holds a map from sidx to dds_stream. We - // need both the ID from that map key and the stream itself (for intrinsics information) - std::map< sid_index, sid_index > type_and_index_to_dds_stream_sidx; - _dds_dev->foreach_stream( [&]( std::shared_ptr< realdds::dds_stream > const & stream ) { @@ -235,9 +234,14 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const & = std::make_shared< librealsense::stream >( stream_type, sidx.index ); sensor_info.proxy->add_dds_stream( sidx, stream ); _stream_name_to_owning_sensor[stream->name()] = sensor_info.proxy; - type_and_index_to_dds_stream_sidx.insert( { type_and_index, sidx } ); - //LOG_DEBUG( sidx.to_string() << " " << get_string( sensor_info.type ) << " '" << stream->sensor_name() - // << "' : '" << stream->name() << "' " << get_string( stream_type ) ); + if( ! sensor_info.type_and_index_to_dds_stream_sidx.insert( { type_and_index, sidx } ).second ) + LOG_ERROR( "Failed to insert '" << stream->sensor_name() << "' " << type_and_index.to_string() << " " + << get_string( sensor_info.type ) << " -> " << get_string( stream_type ) + << " " << sidx.to_string() << " '" << stream->name() << "' mapping" ); + //else + // LOG_DEBUG( "'" << stream->sensor_name() << "' " << type_and_index.to_string() << " " + // << get_string( sensor_info.type ) << " -> " << get_string( stream_type ) << " " + // << sidx.to_string() << " '" << stream->name() << "'" ); software_sensor & sensor = get_software_sensor( sensor_info.sensor_index ); auto video_stream = std::dynamic_pointer_cast< realdds::dds_video_stream >( stream ); @@ -279,36 +283,35 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< const device_info > const & } } ); // End foreach_stream lambda - for( auto & sensor_info : sensor_name_to_info ) + for( auto & name_info : sensor_name_to_info ) { + auto & sensor_name = name_info.first; + auto & sensor_info = name_info.second; + auto & sensor_proxy = sensor_info.proxy; //LOG_DEBUG( sensor_info.first ); // Set profile's ID based on the dds_stream's ID (index already set). Connect the profile to the extrinsics graph. - for( auto & profile : sensor_info.second.proxy->get_stream_profiles() ) + // The get_stream_profiles() call will initialize the profiles (calling dds_sensor_proxy::init_stream_profiles()) + for( auto & profile : sensor_proxy->get_stream_profiles() ) { - //if( auto p = std::dynamic_pointer_cast< librealsense::video_stream_profile_interface >( profile ) ) - //{ - // LOG_DEBUG( " " << get_string( p->get_stream_type() ) << ' ' << p->get_stream_index() << ' ' - // << get_string( p->get_format() ) << ' ' << p->get_width() << 'x' << p->get_height() - // << " @ " << p->get_framerate() ); - //} - //else if( auto p = std::dynamic_pointer_cast( profile ) ) - //{ - // LOG_DEBUG( " " << get_string( p->get_stream_type() ) << ' ' << p->get_stream_index() << ' ' - // << get_string( p->get_format() ) << " @ " << p->get_framerate() ); - //} - sid_index type_and_index( profile->get_stream_type(), profile->get_stream_index() ); - - auto & streams = sensor_info.second.proxy->streams(); - - sid_index sidx = type_and_index_to_dds_stream_sidx.at( type_and_index ); + auto & source_profiles = sensor_proxy->_formats_converter.get_source_profiles_from_target( profile ); + if( source_profiles.size() != 1 ) + LOG_ERROR( "More than one source profile available for [" << profile << "]: " << source_profiles ); + auto source_profile = source_profiles[0]; + + sid_index type_and_index( source_profile->get_stream_type(), source_profile->get_stream_index() ); + sid_index sidx = sensor_info.type_and_index_to_dds_stream_sidx.at( type_and_index ); + + auto & streams = sensor_proxy->streams(); auto stream_iter = streams.find( sidx ); if( stream_iter == streams.end() ) { - LOG_DEBUG( " no dds stream" ); + LOG_ERROR( "No dds stream " << sidx.to_string() << " found for '" << sensor_name << "' " << profile + << " -> " << source_profile << " " << type_and_index.to_string() ); continue; } + //LOG_DEBUG( " " << profile << " -> " << source_profile << " " << type_and_index.to_string() ); profile->set_unique_id( sidx.sid ); // Was lost on clone // NOTE: the 'initialization_done' call above creates target profiles from the raw profiles we supplied it. diff --git a/src/proc/formats-converter.cpp b/src/proc/formats-converter.cpp index 2c874a40aa..4d66c80e76 100644 --- a/src/proc/formats-converter.cpp +++ b/src/proc/formats-converter.cpp @@ -6,6 +6,7 @@ #include #include +#include #include namespace librealsense @@ -73,15 +74,18 @@ std::ostream & operator<<( std::ostream & os, const std::shared_ptr< stream_prof { if( profile ) { - os << "(" << rs2_stream_to_string( profile->get_stream_type() ) << ")"; - os << " " << rs2_format_to_string( profile->get_format() ); - os << " " << profile->get_stream_index(); + os << rs2_stream_to_string( profile->get_stream_type() ); + if( auto stream_index = profile->get_stream_index() ) + os << " " << stream_index; if( auto vsp = As< video_stream_profile, stream_profile_interface >( profile ) ) { os << " " << vsp->get_width(); os << "x" << vsp->get_height(); } - os << " @ " << profile->get_framerate(); + os << " " << rs2_format_to_string( profile->get_format() ); + os << " @ " << profile->get_framerate() << " Hz"; + if( auto bsp = std::dynamic_pointer_cast< backend_stream_profile >( profile ) ) + bsp->to_stream( os ); } return os; @@ -222,6 +226,7 @@ bool formats_converter::is_profile_in_list( const std::shared_ptr< stream_profil // Not passing const & because we modify from_profiles, would otherwise need to create a copy void formats_converter::prepare_to_convert( stream_profiles from_profiles ) { + LOG_DEBUG( "Requested: " << from_profiles ); clear_active_cache(); // Add missing data to target profiles (was not available during get_all_possible_target_profiles) @@ -264,10 +269,22 @@ void formats_converter::prepare_to_convert( stream_profiles from_profiles ) } } const stream_profiles & print_current_resolved_reqs = { current_resolved_reqs.begin(), current_resolved_reqs.end() }; - LOG_INFO( "Request: " << from_profiles_of_best_match << "\nResolved to: " << print_current_resolved_reqs ); + LOG_DEBUG( "Resolved to: " << print_current_resolved_reqs ); } } + +stream_profiles const & formats_converter::get_source_profiles_from_target( + std::shared_ptr< stream_profile_interface > const & target_profile ) const +{ + auto it = _target_profiles_to_raw_profiles.find( to_profile( target_profile.get() ) ); + if( it == _target_profiles_to_raw_profiles.end() ) + throw invalid_value_exception( rsutils::string::from() + << "target profile [" << target_profile << "] not found" ); + return it->second; +} + + void formats_converter::update_target_profiles_data( const stream_profiles & from_profiles ) { for( auto & from_profile : from_profiles ) diff --git a/src/proc/formats-converter.h b/src/proc/formats-converter.h index 64318d791a..ef5b73cd0b 100644 --- a/src/proc/formats-converter.h +++ b/src/proc/formats-converter.h @@ -43,6 +43,8 @@ namespace librealsense rs2_frame_callback_sptr get_frames_callback() const { return _converted_frames_callback; } void convert_frame( frame_holder & f ); + stream_profiles const & get_source_profiles_from_target( std::shared_ptr< stream_profile_interface > const & target_profile ) const; + protected: void clear_active_cache(); void update_target_profiles_data( const stream_profiles & from_profiles ); From e8f433a29a604c1e2007fa67d37722c4615a028c Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Tue, 8 Oct 2024 15:52:09 +0530 Subject: [PATCH 47/75] Bug fix: Removing old driver module files and update modinfo --- scripts/patch-utils-hwe.sh | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/scripts/patch-utils-hwe.sh b/scripts/patch-utils-hwe.sh index e96d89a39d..c10cd9242e 100644 --- a/scripts/patch-utils-hwe.sh +++ b/scripts/patch-utils-hwe.sh @@ -234,10 +234,10 @@ function try_module_insert { # backup the existing module (if available) for recovery if [ -f ${tgt_ko}.zst ]; then - sudo cp ${tgt_ko}.zst ${tgt_ko}.zst.bckup + sudo mv ${tgt_ko}.zst ${tgt_ko}.zst.bckup elif [ -f ${tgt_ko} ]; then - sudo cp ${tgt_ko} ${tgt_ko}.bckup + sudo mv ${tgt_ko} ${tgt_ko}.bckup else backup_available=0 fi @@ -245,6 +245,9 @@ function try_module_insert { # copy the patched module to target location sudo cp ${src_ko} ${tgt_ko} + # Run 'depmod' command, so that, it will update the 'modinfo' if incase filename or other details got changed. + sudo depmod + # try to load the new module modprobe_failed=0 printf "\e[32m\tApplying the patched module ... \e[0m" @@ -259,9 +262,9 @@ function try_module_insert { then if [ -f ${tgt_ko}.zst.bckup ]; then - sudo cp ${tgt_ko}.zst.bckup ${tgt_ko}.zst + sudo mv ${tgt_ko}.zst.bckup ${tgt_ko}.zst else - sudo cp ${tgt_ko}.bckup ${tgt_ko} + sudo mv ${tgt_ko}.bckup ${tgt_ko} fi sudo modprobe ${module_name} printf "\e[34mThe original \e[33m %s \e[34m module was reloaded\n\e[0m" ${module_name} From 0eb30161cbd8a48de413500090c7af4ecb669494 Mon Sep 17 00:00:00 2001 From: Noy-Zini Date: Tue, 8 Oct 2024 13:24:13 +0300 Subject: [PATCH 48/75] remove py3.8 support --- wrappers/python/readme.md | 2 +- wrappers/python/setup.py | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/wrappers/python/readme.md b/wrappers/python/readme.md index d59453a44e..1e328ff851 100644 --- a/wrappers/python/readme.md +++ b/wrappers/python/readme.md @@ -20,7 +20,7 @@ Package is available at https://pypi.python.org/pypi/pyrealsense2 To install the package, run: > `pip install pyrealsense2` -Windows users can install the RealSense SDK 2.0 from the release tab to get pre-compiled binaries of the wrapper, for both x86 and x64 architectures. (Python versions 3.8, 3.9, 3.10, 3.11, 3.12 are supported). +Windows users can install the RealSense SDK 2.0 from the release tab to get pre-compiled binaries of the wrapper, for both x86 and x64 architectures. (Python versions 3.9, 3.10, 3.11, 3.12 are supported). > **Note:** > Python 3.7 distributables can be found for pyrealsense2 versions <= 2.55.1 diff --git a/wrappers/python/setup.py b/wrappers/python/setup.py index 2905ece22a..187d2acdc6 100644 --- a/wrappers/python/setup.py +++ b/wrappers/python/setup.py @@ -57,7 +57,6 @@ def __len__(self): 'Operating System :: Unix', 'Programming Language :: Python', 'Programming Language :: Python :: 3', - 'Programming Language :: Python :: 3.8', 'Programming Language :: Python :: 3.9', 'Programming Language :: Python :: 3.10', 'Programming Language :: Python :: 3.11', From ce22ba87ba1cce37b62902c6017d4c35eba4a52c Mon Sep 17 00:00:00 2001 From: Eran Date: Thu, 10 Oct 2024 12:21:38 +0300 Subject: [PATCH 49/75] fix 'cannot bind non-const lvalue reference' error in lrs-device-controller --- tools/dds/dds-adapter/lrs-device-controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/dds/dds-adapter/lrs-device-controller.cpp b/tools/dds/dds-adapter/lrs-device-controller.cpp index 49b2fc981f..18a566499b 100644 --- a/tools/dds/dds-adapter/lrs-device-controller.cpp +++ b/tools/dds/dds-adapter/lrs-device-controller.cpp @@ -738,7 +738,7 @@ lrs_device_controller::lrs_device_controller( rs2::device dev, std::shared_ptr< image.set_timestamp( timestamp ); auto data = static_cast< const uint8_t * >( f.get_data() ); image.raw().data().assign( data, data + f.get_data_size() ); - video->publish_image( std::move( image ) ); + video->publish_image( image ); publish_frame_metadata( f, timestamp ); } ); From dcba20817377139938e2d1f80d88399d39e6d98d Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Thu, 10 Oct 2024 12:36:50 +0300 Subject: [PATCH 50/75] ignore rsutils on third-party folder, fix typo and static_cast --- CMake/external_libcurl.cmake | 8 +-- CMake/unix_config.cmake | 87 +++++++++++++----------------- CMake/windows_config.cmake | 22 ++++---- examples/CMakeLists.txt | 2 +- src/hid/hid-device.cpp | 2 +- src/uvc/uvc-device.cpp | 6 +-- src/uvc/uvc-streamer.cpp | 2 +- third-party/CMakeLists.txt | 7 +++ third-party/rsutils/CMakeLists.txt | 1 + tools/CMakeLists.txt | 2 +- 10 files changed, 69 insertions(+), 70 deletions(-) diff --git a/CMake/external_libcurl.cmake b/CMake/external_libcurl.cmake index 69a068ea1a..36750d47fb 100644 --- a/CMake/external_libcurl.cmake +++ b/CMake/external_libcurl.cmake @@ -1,7 +1,7 @@ if(CHECK_FOR_UPDATES) - string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") - string(REPLACE "${ADDITIONAL_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") + string(REPLACE "${SECURITY_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") # remove flags + string(REPLACE "${SECURITY_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") include(ExternalProject) message(STATUS "Building libcurl enabled") @@ -63,6 +63,6 @@ if(CHECK_FOR_UPDATES) endif() endif() - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${SECURITY_COMPILER_FLAGS}") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${SECURITY_COMPILER_FLAGS}") endif() #CHECK_FOR_UPDATES diff --git a/CMake/unix_config.cmake b/CMake/unix_config.cmake index ca923fd92d..bc88fcb408 100644 --- a/CMake/unix_config.cmake +++ b/CMake/unix_config.cmake @@ -47,60 +47,49 @@ macro(os_set_flags) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") endif() - ############### - # According to SDLE we need to add the following flags for additional security: - # Debug & Release: - # -Wformat: Checks for format string vulnerabilities. - # -Wformat-security: Ensures format strings are not vulnerable to attacks. - # -fPIC: Generates position-independent code (PIC) suitable for shared libraries. - # -fPIE: Generates position-independent executable (PIE) code. - # -pie: Links the output as a position-independent executable. - # -D_FORTIFY_SOURCE=2: Adds extra checks for buffer overflows. - # -mfunction-return=thunk: Mitigates return-oriented programming (ROP) attacks. (Added flag -fcf-protection=none to allow it) - # -mindirect-branch=thunk: Mitigates indirect branch attacks. - # -mindirect-branch-register: Uses registers for indirect branches to mitigate attacks. - # -fstack-protector: Adds stack protection to detect buffer overflows. + + if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + # Due to security reasons we need to add the following flags for additional security: + # Debug & Release: + # -Wformat: Checks for format string vulnerabilities. + # -Wformat-security: Ensures format strings are not vulnerable to attacks. + # -fPIC: Generates position-independent code during the compilation phase. + # -fPIE: Generates position-independent executables during the compilation phase. + # -D_FORTIFY_SOURCE=2: Adds extra checks for buffer overflows. + # -fstack-protector: Adds stack protection to detect buffer overflows. - # Release only - # -Werror: Treats all warnings as errors. - # -Werror=format-security: Treats format security warnings as errors. - # -z noexecstack: Marks the stack as non-executable to prevent certain types of attacks. - # -Wl,-z,relro,-z,now: Enables read-only relocations and immediate binding for security. - # -fstack-protector-strong: Provides stronger stack protection than -fstack-protector. - - # see https://readthedocs.intel.com/SecureCodingStandards/2023.Q2.0/compiler/c-cpp/ for more details + # Release only + # -Werror: Treats all warnings as errors. + # -Werror=format-security: Treats format security warnings as errors. + # -z noexecstack: Marks the stack as non-executable to prevent certain types of attacks. + # -Wl,-z,relro,-z,now: Enables read-only relocations and immediate binding for security. + # -fstack-protector-strong: Provides stronger stack protection than -fstack-protector. + + # Linker flags + # -pie: Produces position-independent executables during the linking phase. + + # see https://readthedocs.intel.com/SecureCodingStandards/2023.Q2.0/compiler/c-cpp/ for more details - if (CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64|armv7l" OR APPLE OR # Some flags are not recognized or some systems / gcc versions - (CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_LESS "9.0")) # - set(ADDITIONAL_COMPILER_FLAGS "-Wformat -Wformat-security -fPIC -fstack-protector") - else() - #‘-mfunction-return’ and ‘-fcf-protection’ are not compatible, so specifing -fcf-protection=none - set(ADDITIONAL_COMPILER_FLAGS "-Wformat -Wformat-security -fPIC -fcf-protection=none -mfunction-return=thunk -mindirect-branch=thunk -mindirect-branch-register -fstack-protector") - endif() - set(CMAKE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS} -pie") - - set(ADDITIONAL_COMPILER_FLAGS "${ADDITIONAL_COMPILER_FLAGS} -Wno-error=stringop-overflow") + set(SECURITY_COMPILER_FLAGS "-Wformat -Wformat-security -fPIC -fstack-protector -Wno-error=stringop-overflow") - string(FIND "${CMAKE_CXX_FLAGS}" "-D_FORTIFY_SOURCE" _index) - if (${_index} EQUAL -1) # Define D_FORTIFY_SOURCE is undefined - set(ADDITIONAL_COMPILER_FLAGS "${ADDITIONAL_COMPILER_FLAGS} -D_FORTIFY_SOURCE=2") - endif() + string(FIND "${CMAKE_CXX_FLAGS}" "-D_FORTIFY_SOURCE" _index) + if (${_index} EQUAL -1) # Define D_FORTIFY_SOURCE if undefined + set(SECURITY_COMPILER_FLAGS "${SECURITY_COMPILER_FLAGS} -D_FORTIFY_SOURCE=2") + endif() - if(CMAKE_BUILD_TYPE STREQUAL "Debug") - message(STATUS "Configuring for Debug build") - else() # Release, RelWithDebInfo, or multi configuration generator is being used (aka not specifing build type, or building with VS) - message(STATUS "Configuring for Release build") - set(ADDITIONAL_COMPILER_FLAGS "${ADDITIONAL_COMPILER_FLAGS} -Werror -z noexecstack -Wl,-z,relro,-z,now -fstack-protector-strong") - endif() - - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") - - - set_directory_properties(PROPERTIES DIRECTORY third-party/ COMPILE_OPTIONS "-w") - set_source_files_properties(third-party/*.* PROPERTIES COMPILE_OPTIONS "-w") + if(CMAKE_BUILD_TYPE STREQUAL "Debug") + message(STATUS "Configuring for Debug build") + else() # Release, RelWithDebInfo, or multi configuration generator is being used (aka not specifing build type, or building with VS) + message(STATUS "Configuring for Release build") + set(SECURITY_COMPILER_FLAGS "${SECURITY_COMPILER_FLAGS} -Werror -z noexecstack -Wl,-z,relro,-z,now -fstack-protector-strong") + endif() + + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${SECURITY_COMPILER_FLAGS}") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${SECURITY_COMPILER_FLAGS}") + + set(CMAKE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS} -pie") - ################# + endif() if(APPLE) set(FORCE_RSUSB_BACKEND ON) diff --git a/CMake/windows_config.cmake b/CMake/windows_config.cmake index 4481173b45..4b94d06133 100644 --- a/CMake/windows_config.cmake +++ b/CMake/windows_config.cmake @@ -41,7 +41,7 @@ macro(os_set_flags) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /MP") ############### - # According to SDLE we need to add the following flags for additional security: + # Due to security reasons we need to add the following flags for additional security: # Debug & Release: # /Gy: Enables function-level linking to reduce executable size. # /DYNAMICBASE: Enables Address Space Layout Randomization (ASLR) to improve security. @@ -49,27 +49,29 @@ macro(os_set_flags) # Release only: # /WX: Treats all warnings as errors. - # /LTCG (/GL): Enables Link Time Code Generation to improve performance. # /sdl: Enables additional security checks. + + # Release only linker flags: + # /LTCG (/GL): Enables Link Time Code Generation to improve performance. # /NXCOMPAT: Enables Data Execution Prevention (DEP) to prevent code execution in data areas. # see https://readthedocs.intel.com/SecureCodingStandards/2023.Q2.0/compiler/c-cpp/ for more details - set(ADDITIONAL_COMPILER_FLAGS "/Gy /DYNAMICBASE /GS /wd4101") + set(SECURITY_COMPILER_FLAGS "/Gy /DYNAMICBASE /GS /wd4101") if(CMAKE_BUILD_TYPE STREQUAL "Debug") message(STATUS "Configuring for Debug build") else() # Release, RelWithDebInfo, or multi configuration generator is being used (aka not specifing build type, or building with VS) message(STATUS "Configuring for Release build") - set(ADDITIONAL_COMPILER_FLAGS "${ADDITIONAL_COMPILER_FLAGS} /WX /sdl") - set(CMAKE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS} /INCREMENTAL:NO /LTCG /NXCOMPAT") # ignoring '/INCREMENTAL' due to '/LTCG' specification + set(SECURITY_COMPILER_FLAGS "${SECURITY_COMPILER_FLAGS} /WX /sdl") endif() - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ADDITIONAL_COMPILER_FLAGS}") - - set_directory_properties(PROPERTIES DIRECTORY third-party/ COMPILE_OPTIONS "/W0") - set_source_files_properties(third-party/*.* PROPERTIES COMPILE_OPTIONS "/W0") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${SECURITY_COMPILER_FLAGS}") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${SECURITY_COMPILER_FLAGS}") + + if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") + set(CMAKE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS} /INCREMENTAL:NO /LTCG /NXCOMPAT") # ignoring '/INCREMENTAL' due to '/LTCG' specification + endif() ################# diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index b8ee580d08..aaf3c1d369 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -7,7 +7,7 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS 1) # View the makefile commands during build #set(CMAKE_VERBOSE_MAKEFILE on) -string(REPLACE "-fPIC" "-fPIE" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") # examples are exeutables so we want position indepandent exeutables and not libraries +string(REPLACE "-fPIC" "-fPIE" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") # examples are executables so we want position indepandent executables and not libraries set( DEPENDENCIES ${LRS_TARGET} ) if(BUILD_GRAPHICAL_EXAMPLES) diff --git a/src/hid/hid-device.cpp b/src/hid/hid-device.cpp index c903016967..01d79ad3fd 100644 --- a/src/hid/hid-device.cpp +++ b/src/hid/hid-device.cpp @@ -334,7 +334,7 @@ namespace librealsense //we want to change the sensitivity values only in gyro, for FW version >= 5.16 if( featureReport.reportId == REPORT_ID_GYROMETER_3D && _realsense_hid_report_actual_size == sizeof( REALSENSE_HID_REPORT ) ) - featureReport.sensitivity = (unsigned short)sensitivity; + featureReport.sensitivity = static_cast(sensitivity); res = dev->control_transfer(USB_REQUEST_CODE_SET, diff --git a/src/uvc/uvc-device.cpp b/src/uvc/uvc-device.cpp index 765d72210b..a23615ae74 100644 --- a/src/uvc/uvc-device.cpp +++ b/src/uvc/uvc-device.cpp @@ -177,7 +177,7 @@ namespace librealsense switch(state) { case D0: - _messenger = _usb_device->open((uint8_t)_info.mi); + _messenger = _usb_device->open(static_cast(_info.mi)); if (_messenger) { try{ @@ -654,7 +654,7 @@ namespace librealsense void rs_uvc_device::listen_to_interrupts() { - auto ctrl_interface = _usb_device->get_interface((uint8_t)_info.mi); + auto ctrl_interface = _usb_device->get_interface(static_cast(_info.mi)); if (!ctrl_interface) return; auto iep = ctrl_interface->first_endpoint(RS2_USB_ENDPOINT_DIRECTION_READ, RS2_USB_ENDPOINT_INTERRUPT); @@ -856,7 +856,7 @@ namespace librealsense req, probe ? (UVC_VS_PROBE_CONTROL << 8) : (UVC_VS_COMMIT_CONTROL << 8), ctrl->bInterfaceNumber, // When requestType is directed to an interface, the driver automatically passes the interface number in the low byte of index - buf, (uint32_t)len, transferred, 0); + buf, static_cast(len), transferred, 0); } while (sts != RS2_USB_STATUS_SUCCESS && retries++ < 5); } }, [this](){ return !_messenger; }); diff --git a/src/uvc/uvc-streamer.cpp b/src/uvc/uvc-streamer.cpp index 974aff5c52..01ccf9998e 100644 --- a/src/uvc/uvc-streamer.cpp +++ b/src/uvc/uvc-streamer.cpp @@ -28,7 +28,7 @@ namespace librealsense _action_dispatcher.start(); - _watchdog_timeout = (int64_t)((1000.0 / _context.profile.fps) * 10); + _watchdog_timeout = static_cast(((1000.0 / _context.profile.fps) * 10)); init(); } diff --git a/third-party/CMakeLists.txt b/third-party/CMakeLists.txt index c70314062e..9eb1a680b8 100644 --- a/third-party/CMakeLists.txt +++ b/third-party/CMakeLists.txt @@ -1,5 +1,12 @@ string(REPLACE ${PROJECT_SOURCE_DIR}/ "" _rel_path ${CMAKE_CURRENT_LIST_DIR}) +# ignore warnings on third party files +if (MSVC) + add_compile_options(/W0) +elseif (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + add_compile_options(-w) +endif() + include(CMake/external_json.cmake) add_subdirectory( "${CMAKE_CURRENT_LIST_DIR}/rsutils" ) diff --git a/third-party/rsutils/CMakeLists.txt b/third-party/rsutils/CMakeLists.txt index b0fc310497..386a8c1ba9 100644 --- a/third-party/rsutils/CMakeLists.txt +++ b/third-party/rsutils/CMakeLists.txt @@ -3,6 +3,7 @@ cmake_minimum_required(VERSION 3.8.0) # source_group(TREE) project( rsutils ) +set_directory_properties(PROPERTIES COMPILE_FLAGS "") # unignore warnings on rsutils - remove flag applied at third-party folder add_library( ${PROJECT_NAME} STATIC "" ) # We cannot directly interface with nlohmann_json (doesn't work on bionic) #target_link_libraries( ${PROJECT_NAME} PUBLIC nlohmann_json ) diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index bca5cdf6f9..d758cd01c4 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -7,7 +7,7 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS 1) # View the makefile commands during build #set(CMAKE_VERBOSE_MAKEFILE on) -string(REPLACE "-fPIC" "-fPIE" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") # tools are exeutables so we want position indepandent exeutables and not libraries +string(REPLACE "-fPIC" "-fPIE" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") # tools are executables so we want position indepandent executables and not libraries list( APPEND DEPENDENCIES ${LRS_TARGET} tclap ) From ef0e8e0ff012eb9b5b320f38116f3f9ef9365921 Mon Sep 17 00:00:00 2001 From: fateshelled <53618876+fateshelled@users.noreply.github.com> Date: Sun, 13 Oct 2024 22:43:48 +0900 Subject: [PATCH 51/75] NEON support `align` --- src/proc/align.cpp | 3 + src/proc/neon/CMakeLists.txt | 1 + src/proc/neon/neon-align.cpp | 469 ++++++++++++++++++++++++++++++ src/proc/neon/neon-align.h | 87 ++++++ src/proc/neon/neon-pointcloud.cpp | 2 +- 5 files changed, 561 insertions(+), 1 deletion(-) create mode 100644 src/proc/neon/neon-align.cpp create mode 100644 src/proc/neon/neon-align.h diff --git a/src/proc/align.cpp b/src/proc/align.cpp index 8f818307c3..a1910290cd 100644 --- a/src/proc/align.cpp +++ b/src/proc/align.cpp @@ -16,6 +16,7 @@ #elif defined(__SSSE3__) #include "proc/sse/sse-align.h" #endif +#include "proc/neon/neon-align.h" namespace librealsense { @@ -27,6 +28,8 @@ namespace librealsense return std::make_shared(align_to); #elif defined(__SSSE3__) return std::make_shared(align_to); + #elif defined(__ARM_NEON) && ! defined(ANDROID) + return std::make_shared(align_to); #else return std::make_shared(align_to); #endif diff --git a/src/proc/neon/CMakeLists.txt b/src/proc/neon/CMakeLists.txt index 9dd1b72856..5a4732369c 100644 --- a/src/proc/neon/CMakeLists.txt +++ b/src/proc/neon/CMakeLists.txt @@ -4,4 +4,5 @@ target_sources(${LRS_TARGET} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/image-neon.cpp" "${CMAKE_CURRENT_LIST_DIR}/neon-pointcloud.cpp" + "${CMAKE_CURRENT_LIST_DIR}/neon-align.cpp" ) diff --git a/src/proc/neon/neon-align.cpp b/src/proc/neon/neon-align.cpp new file mode 100644 index 0000000000..121a1c4466 --- /dev/null +++ b/src/proc/neon/neon-align.cpp @@ -0,0 +1,469 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#include "neon-align.h" + +#if defined(__ARM_NEON) && ! defined ANDROID + +#include +namespace librealsense +{ + template + struct bytes + { + uint8_t b[N]; + }; + + static inline bool is_special_resolution(const rs2_intrinsics &depth, const rs2_intrinsics &to) + { + if ((depth.width == 640 && depth.height == 240 && to.width == 320 && to.height == 180) || + (depth.width == 640 && depth.height == 480 && to.width == 640 && to.height == 360)) + return true; + return false; + } + + template + static inline void distorte_x_y( + const float32x4_t &x, const float32x4_t &y, + float32x4_t *distorted_x, float32x4_t *distorted_y, const float32x4_t(&c)[5]) + { + *distorted_x = x; + *distorted_y = y; + } + + template <> + inline void distorte_x_y( + const float32x4_t &x, const float32x4_t &y, + float32x4_t *distorted_x, float32x4_t *distorted_y, const float32x4_t(&c)[5]) + { + const auto one = vdupq_n_f32(1); + const auto two = vdupq_n_f32(2); + + // r2 = x * x + y * y + const auto r2 = vaddq_f32(vmulq_f32(x, x), vmulq_f32(y, y)); + // f = 1 + c[0] * r2 + c[1] * r2 ^ 2 + c[4] * r2 ^ 3 + // = 1 + (c[0] + (c[1] + c[4] * r2) * r2) * r2 + const auto f = vfmaq_f32(one, r2, vfmaq_f32(c[0], r2, vfmaq_f32(c[1], r2, c[4]))); + + const auto x_f = vmulq_f32(x, f); + const auto y_f = vmulq_f32(y, f); + + // dx = x_f + 2 * c[2] * x_f * y_f + c[3] * (r2 + 2 * x_f * x_f) + // = x_f * (1 + 2 * c[2] * y_f + c[3] * 2 * x_f) + c[3] * r2 + // = x_f * (1 + 2 * (c[2] * y_f + c[3] * x_f)) + c[3] * r2 + *distorted_x = vfmaq_f32(vmulq_f32(x_f, vfmaq_f32(one, two, vfmaq_f32(vmulq_f32(c[2], y_f), c[3], x_f))), c[3], r2); + + // dy = y_f + 2 * c[3] * x_f * y_f + c[2] * (r2 + 2 * y_f * y_f) + // = y_f * (1 + 2 * c[3] * x_f + c[2] * 2 * y_f) + c[2] * r2 + // = y_f * (1 + 2 * (c[3] * x_f + c[2] * y_f)) + c[2] * r2 + *distorted_y = vfmaq_f32(vmulq_f32(y_f, vfmaq_f32(one, two, vfmaq_f32(vmulq_f32(c[3], x_f), c[2], y_f))), c[2], r2); + } + + template + static inline void get_texture_map_neon(const uint16_t *depth, + float depth_scale, + const unsigned int size, + const float *pre_compute_x, const float *pre_compute_y, + uint8_t *pixels_ptr_int, + const rs2_intrinsics &to, + const rs2_extrinsics &from_to_other) + { + auto res = reinterpret_cast(pixels_ptr_int); + + float32x4_t r[9]; + float32x4_t t[3]; + float32x4_t c[5]; + for (int i = 0; i < 9; ++i) + { + r[i] = vdupq_n_f32(from_to_other.rotation[i]); + } + for (int i = 0; i < 3; ++i) + { + t[i] = vdupq_n_f32(from_to_other.translation[i]); + } + for (int i = 0; i < 5; ++i) + { + c[i] = vdupq_n_f32(to.coeffs[i]); + } + const auto zero = vdupq_n_f32(0.0f); + const auto half = vdupq_n_f32(0.5f); + const auto fx = vdupq_n_f32(to.fx); + const auto fy = vdupq_n_f32(to.fy); + const auto ppx = vdupq_n_f32(to.ppx); + const auto ppy = vdupq_n_f32(to.ppy); + const auto scale = vdupq_n_f32(depth_scale); + + for (unsigned int i = 0; i < size; i += 8) + { + const auto x0 = vld1q_f32(pre_compute_x + i); + const auto x1 = vld1q_f32(pre_compute_x + i + 4); + + const auto y0 = vld1q_f32(pre_compute_y + i); + const auto y1 = vld1q_f32(pre_compute_y + i + 4); + + const auto d = vld1q_u16(depth + i); + const auto depth0 = vmulq_f32(vcvtq_f32_s32((int32x4_t)vmovl_u16(vget_low_u16(d))), scale); + const auto depth1 = vmulq_f32(vcvtq_f32_s32((int32x4_t)vmovl_u16(vget_high_u16(d))), scale); + + // calculate 3D point + // rs2_deproject_pixel_to_point + const auto p0x = vmulq_f32(depth0, x0); + const auto p0y = vmulq_f32(depth0, y0); + + const auto p1x = vmulq_f32(depth1, x1); + const auto p1y = vmulq_f32(depth1, y1); + + // transform to other + // rs2_transform_point_to_point + auto p_x0 = vfmaq_f32(vfmaq_f32(vfmaq_f32(t[0], r[6], depth0), r[3], p0y), r[0], p0x); + auto p_y0 = vfmaq_f32(vfmaq_f32(vfmaq_f32(t[1], r[7], depth0), r[4], p0y), r[1], p0x); + auto p_z0 = vfmaq_f32(vfmaq_f32(vfmaq_f32(t[2], r[8], depth0), r[5], p0y), r[2], p0x); + + auto p_x1 = vfmaq_f32(vfmaq_f32(vfmaq_f32(t[0], r[6], depth1), r[3], p1y), r[0], p1x); + auto p_y1 = vfmaq_f32(vfmaq_f32(vfmaq_f32(t[1], r[7], depth1), r[4], p1y), r[1], p1x); + auto p_z1 = vfmaq_f32(vfmaq_f32(vfmaq_f32(t[2], r[8], depth1), r[5], p1y), r[2], p1x); + + // rs2_project_point_to_pixel + { + p_x0 = vdivq_f32(p_x0, p_z0); + p_y0 = vdivq_f32(p_y0, p_z0); + + p_x1 = vdivq_f32(p_x1, p_z1); + p_y1 = vdivq_f32(p_y1, p_z1); + + distorte_x_y(p_x0, p_y0, &p_x0, &p_y0, c); + distorte_x_y(p_x1, p_y1, &p_x1, &p_y1, c); + + p_x0 = vfmaq_f32(ppx, p_x0, fx); + p_y0 = vfmaq_f32(ppy, p_y0, fy); + + p_x1 = vfmaq_f32(ppx, p_x1, fx); + p_y1 = vfmaq_f32(ppy, p_y1, fy); + } + + // float to int32 + { + // zero the x and y if z is zero + const uint32x4_t gt_zero = vcgtq_f32(depth0, zero); + + const auto u = vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(vaddq_f32(p_x0, half)), gt_zero)); + const auto v = vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(vaddq_f32(p_y0, half)), gt_zero)); + + int32x4x2_t res_int; + res_int.val[0] = vcvtq_s32_f32(u); + res_int.val[1] = vcvtq_s32_f32(v); + vst2q_s32(res, res_int); + res += 8; + } + + { + // zero the x and y if z is zero + const uint32x4_t gt_zero = vcgtq_f32(depth1, zero); + + const auto u = vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(vaddq_f32(p_x1, half)), gt_zero)); + const auto v = vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(vaddq_f32(p_y1, half)), gt_zero)); + + int32x4x2_t res_int; + res_int.val[0] = vcvtq_s32_f32(u); + res_int.val[1] = vcvtq_s32_f32(v); + vst2q_s32(res, res_int); + res += 8; + } + } + } + + align_neon_helper::align_neon_helper(const rs2_intrinsics &from, float depth_scale) + : _depth(from), + _depth_scale(depth_scale), + _pixel_top_left_int(from.width * from.height), + _pixel_bottom_right_int(from.width * from.height) + { + } + + void align_neon_helper::pre_compute_x_y_map_corners() + { + pre_compute_x_y_map(_pre_compute_map_x_top_left, _pre_compute_map_y_top_left, -0.5f); + pre_compute_x_y_map(_pre_compute_map_x_bottom_right, _pre_compute_map_y_bottom_right, 0.5f); + } + + void align_neon_helper::pre_compute_x_y_map(std::vector &pre_compute_map_x, + std::vector &pre_compute_map_y, + float offset) + { + pre_compute_map_x.resize(_depth.width * _depth.height); + pre_compute_map_y.resize(_depth.width * _depth.height); + + for (int h = 0; h < _depth.height; ++h) + { + for (int w = 0; w < _depth.width; ++w) + { + const float pixel[] = {(float)w + offset, (float)h + offset}; + + float x = (pixel[0] - _depth.ppx) / _depth.fx; + float y = (pixel[1] - _depth.ppy) / _depth.fy; + + if (_depth.model == RS2_DISTORTION_INVERSE_BROWN_CONRADY) + { + const float r2 = x * x + y * y; + const float f = 1.0f + (_depth.coeffs[0] + (_depth.coeffs[1] + _depth.coeffs[4] * r2) * r2) * r2; + const float ux = x * f + 2.0f * _depth.coeffs[2] * x * y + _depth.coeffs[3] * (r2 + 2.0f * x * x); + const float uy = y * f + 2.0f * _depth.coeffs[3] * x * y + _depth.coeffs[2] * (r2 + 2.0f * y * y); + x = ux; + y = uy; + } + + pre_compute_map_x[h * _depth.width + w] = x; + pre_compute_map_y[h * _depth.width + w] = y; + } + } + } + + void align_neon_helper::align_depth_to_other( + const uint16_t *z_pixels, uint16_t *dest, int bpp, const rs2_intrinsics &depth, + const rs2_intrinsics &to, const rs2_extrinsics &from_to_other) + { + switch (to.model) + { + case RS2_DISTORTION_MODIFIED_BROWN_CONRADY: + align_depth_to_other_neon(z_pixels, dest, depth, to, from_to_other); + break; + default: + align_depth_to_other_neon(z_pixels, dest, depth, to, from_to_other); + break; + } + } + + inline void align_neon_helper::move_depth_to_other( + const uint16_t *z_pixels, uint16_t *dest, const rs2_intrinsics &to, + const std::vector &pixel_top_left_int, + const std::vector &pixel_bottom_right_int) + { + for (int y = 0; y < _depth.height; ++y) + { + for (int x = 0; x < _depth.width; ++x) + { + const auto depth_pixel_index = y * _depth.width + x; + // Skip over depth pixels with the value of zero, we have no depth data so we will not write anything into our aligned images + if (z_pixels[depth_pixel_index]) + { + for (int other_y = pixel_top_left_int[depth_pixel_index].y; other_y <= pixel_bottom_right_int[depth_pixel_index].y; ++other_y) + { + if (other_y < 0 || other_y >= to.height) + continue; + + const auto y_width = other_y * to.width; + for (int other_x = pixel_top_left_int[depth_pixel_index].x; other_x <= pixel_bottom_right_int[depth_pixel_index].x; ++other_x) + { + if (other_x < 0 || other_x >= to.width) + continue; + const auto other_ind = y_width + other_x; + + dest[other_ind] = dest[other_ind] ? std::min(dest[other_ind], z_pixels[depth_pixel_index]) : z_pixels[depth_pixel_index]; + } + } + } + } + } + } + + void align_neon_helper::align_other_to_depth( + const uint16_t *z_pixels, const uint8_t *source, uint8_t *dest, + int bpp, const rs2_intrinsics &to, const rs2_extrinsics &from_to_other) + { + switch (to.model) + { + case RS2_DISTORTION_MODIFIED_BROWN_CONRADY: + case RS2_DISTORTION_INVERSE_BROWN_CONRADY: + align_other_to_depth_neon(z_pixels, source, dest, bpp, to, from_to_other); + break; + default: + align_other_to_depth_neon(z_pixels, source, dest, bpp, to, from_to_other); + break; + } + } + + template + inline void align_neon_helper::align_depth_to_other_neon( + const uint16_t *z_pixels, uint16_t *dest, const rs2_intrinsics &depth, + const rs2_intrinsics &to, const rs2_extrinsics &from_to_other) + { + // Map the top-left corner of the depth pixel onto the other image + get_texture_map_neon( + z_pixels, _depth_scale, _depth.height * _depth.width, + _pre_compute_map_x_top_left.data(), _pre_compute_map_y_top_left.data(), + (uint8_t *)_pixel_top_left_int.data(), to, from_to_other); + + float fov[2]; + rs2_fov(&depth, fov); + float2 pixels_per_angle_depth = {(float)depth.width / fov[0], (float)depth.height / fov[1]}; + + rs2_fov(&to, fov); + float2 pixels_per_angle_target = {(float)to.width / fov[0], (float)to.height / fov[1]}; + + if (pixels_per_angle_depth.x < pixels_per_angle_target.x || pixels_per_angle_depth.y < pixels_per_angle_target.y || is_special_resolution(depth, to)) + { + // Map the bottom-right corner of the depth pixel onto the other image + get_texture_map_neon( + z_pixels, _depth_scale, _depth.height * _depth.width, + _pre_compute_map_x_bottom_right.data(), _pre_compute_map_y_bottom_right.data(), + (uint8_t *)_pixel_bottom_right_int.data(), to, from_to_other); + + move_depth_to_other(z_pixels, dest, to, _pixel_top_left_int, _pixel_bottom_right_int); + } + else + { + move_depth_to_other(z_pixels, dest, to, _pixel_top_left_int, _pixel_top_left_int); + } + } + + template + inline void align_neon_helper::align_other_to_depth_neon( + const uint16_t *z_pixels, const uint8_t *source, uint8_t *dest, + int bpp, const rs2_intrinsics &to, const rs2_extrinsics &from_to_other) + { + // Map the top-left corner of the depth pixel onto the other image + get_texture_map_neon( + z_pixels, _depth_scale, _depth.height * _depth.width, + _pre_compute_map_x_top_left.data(), _pre_compute_map_y_top_left.data(), + (uint8_t *)_pixel_top_left_int.data(), to, from_to_other); + + std::vector &bottom_right = _pixel_top_left_int; + if (to.height < _depth.height && to.width < _depth.width) + { + // Map the bottom-right corner of the depth pixel onto the other image + get_texture_map_neon( + z_pixels, _depth_scale, _depth.height * _depth.width, + _pre_compute_map_x_bottom_right.data(), _pre_compute_map_y_bottom_right.data(), + (uint8_t *)_pixel_bottom_right_int.data(), to, from_to_other); + + bottom_right = _pixel_bottom_right_int; + } + + switch (bpp) + { + case 1: + move_other_to_depth( + z_pixels, + reinterpret_cast *>(source), reinterpret_cast *>(dest), + to, _pixel_top_left_int, bottom_right); + break; + case 2: + move_other_to_depth( + z_pixels, + reinterpret_cast *>(source), reinterpret_cast *>(dest), + to, _pixel_top_left_int, bottom_right); + break; + case 3: + move_other_to_depth( + z_pixels, + reinterpret_cast *>(source), reinterpret_cast *>(dest), + to, _pixel_top_left_int, bottom_right); + break; + case 4: + move_other_to_depth( + z_pixels, + reinterpret_cast *>(source), reinterpret_cast *>(dest), + to, _pixel_top_left_int, bottom_right); + break; + default: + break; + } + } + + template + void align_neon_helper::move_other_to_depth( + const uint16_t *z_pixels, const T *source, T *dest, + const rs2_intrinsics &to, + const std::vector &pixel_top_left_int, + const std::vector &pixel_bottom_right_int) + { + // Iterate over the pixels of the depth image + for (int y = 0; y < _depth.height; ++y) + { + for (int x = 0; x < _depth.width; ++x) + { + const auto depth_pixel_index = y * _depth.width + x; + // Skip over depth pixels with the value of zero, we have no depth data so we will not write anything into our aligned images + if (z_pixels[depth_pixel_index]) + { + for (int other_y = pixel_top_left_int[depth_pixel_index].y; other_y <= pixel_bottom_right_int[depth_pixel_index].y; ++other_y) + { + if (other_y < 0 || other_y >= to.height) + continue; + + const auto y_width = other_y * to.width; + for (int other_x = pixel_top_left_int[depth_pixel_index].x; other_x <= pixel_bottom_right_int[depth_pixel_index].x; ++other_x) + { + if (other_x < 0 || other_x >= to.width) + continue; + const auto other_ind = y_width + other_x; + + dest[depth_pixel_index] = source[other_ind]; + } + } + } + } + } + } + + void align_neon::reset_cache(rs2_stream from, rs2_stream to) + { + _neon_helper = nullptr; + } + + void align_neon::align_z_to_other( + rs2::video_frame &aligned, const rs2::video_frame &depth, + const rs2::video_stream_profile &other_profile, float z_scale) + { + uint8_t *aligned_data = reinterpret_cast(const_cast(aligned.get_data())); + auto aligned_profile = aligned.get_profile().as(); + memset(aligned_data, 0, aligned_profile.height() * aligned_profile.width() * aligned.get_bytes_per_pixel()); + + auto depth_profile = depth.get_profile().as(); + + auto z_intrin = depth_profile.get_intrinsics(); + auto other_intrin = other_profile.get_intrinsics(); + auto z_to_other = depth_profile.get_extrinsics_to(other_profile); + + auto z_pixels = reinterpret_cast(depth.get_data()); + + if (_neon_helper == nullptr) + { + _neon_helper = std::make_shared(z_intrin, z_scale); + _neon_helper->pre_compute_x_y_map_corners(); + } + _neon_helper->align_depth_to_other( + z_pixels, reinterpret_cast(aligned_data), 2, + z_intrin, other_intrin, z_to_other); + } + + void align_neon::align_other_to_z( + rs2::video_frame &aligned, const rs2::video_frame &depth, + const rs2::video_frame &other, float z_scale) + { + uint8_t *aligned_data = reinterpret_cast(const_cast(aligned.get_data())); + auto aligned_profile = aligned.get_profile().as(); + memset(aligned_data, 0, aligned_profile.height() * aligned_profile.width() * aligned.get_bytes_per_pixel()); + + auto depth_profile = depth.get_profile().as(); + auto other_profile = other.get_profile().as(); + + auto z_intrin = depth_profile.get_intrinsics(); + auto other_intrin = other_profile.get_intrinsics(); + auto z_to_other = depth_profile.get_extrinsics_to(other_profile); + + auto z_pixels = reinterpret_cast(depth.get_data()); + auto other_pixels = reinterpret_cast(other.get_data()); + + if (_neon_helper == nullptr) + { + _neon_helper = std::make_shared(z_intrin, z_scale); + _neon_helper->pre_compute_x_y_map_corners(); + } + + _neon_helper->align_other_to_depth( + z_pixels, other_pixels, aligned_data, other.get_bytes_per_pixel(), + other_intrin, z_to_other); + } +} // namespace librealsense +#endif diff --git a/src/proc/neon/neon-align.h b/src/proc/neon/neon-align.h new file mode 100644 index 0000000000..41160db9e2 --- /dev/null +++ b/src/proc/neon/neon-align.h @@ -0,0 +1,87 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "proc/align.h" +#include + +namespace librealsense +{ +#ifndef ANDROID +#if defined(__ARM_NEON) + class align_neon_helper + { + public: + align_neon_helper(const rs2_intrinsics& from, float depth_scale); + + inline void align_depth_to_other(const uint16_t* z_pixels, + uint16_t* dest, int bpp, + const rs2_intrinsics& depth, + const rs2_intrinsics& to, + const rs2_extrinsics& from_to_other); + + inline void align_other_to_depth(const uint16_t* z_pixels, + const uint8_t * source, + uint8_t* dest, int bpp, const rs2_intrinsics& to, + const rs2_extrinsics& from_to_other); + + void pre_compute_x_y_map_corners(); + + private: + const rs2_intrinsics _depth; + float _depth_scale; + + std::vector _pre_compute_map_x_top_left; + std::vector _pre_compute_map_y_top_left; + std::vector _pre_compute_map_x_bottom_right; + std::vector _pre_compute_map_y_bottom_right; + + std::vector _pixel_top_left_int; + std::vector _pixel_bottom_right_int; + + void pre_compute_x_y_map(std::vector& pre_compute_map_x, + std::vector& pre_compute_map_y, + float offset = 0); + + template + inline void align_depth_to_other_neon(const uint16_t* z_pixels, + uint16_t* dest, const rs2_intrinsics& depth, + const rs2_intrinsics& to, + const rs2_extrinsics& from_to_other); + + template + inline void align_other_to_depth_neon(const uint16_t* z_pixels, + const uint8_t * source, + uint8_t* dest, int bpp, const rs2_intrinsics& to, + const rs2_extrinsics& from_to_other); + + inline void move_depth_to_other(const uint16_t* z_pixels, + uint16_t* dest, const rs2_intrinsics& to, + const std::vector& pixel_top_left_int, + const std::vector& pixel_bottom_right_int); + + template + inline void move_other_to_depth(const uint16_t* z_pixels, + const T* source, + T* dest, const rs2_intrinsics& to, + const std::vector& pixel_top_left_int, + const std::vector& pixel_bottom_right_int); + }; + + class align_neon : public align + { + public: + align_neon(rs2_stream align_to) : align(align_to, "Align (NEON)") {} + protected: + void reset_cache(rs2_stream from, rs2_stream to) override; + + void align_z_to_other(rs2::video_frame& aligned, const rs2::video_frame& depth, const rs2::video_stream_profile& other_profile, float z_scale) override; + + void align_other_to_z(rs2::video_frame& aligned, const rs2::video_frame& depth, const rs2::video_frame& other, float z_scale) override; + private: + std::shared_ptr _neon_helper; + }; +#endif +#endif +} // namespace librealsense diff --git a/src/proc/neon/neon-pointcloud.cpp b/src/proc/neon/neon-pointcloud.cpp index 0ded91417a..76effcbd4f 100644 --- a/src/proc/neon/neon-pointcloud.cpp +++ b/src/proc/neon/neon-pointcloud.cpp @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2017 Intel Corporation. All Rights Reserved. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. #include From 4d0e9d5e7707ee37cb2f36aa7ff20eea802d6d2e Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Mon, 14 Oct 2024 16:47:14 +0300 Subject: [PATCH 52/75] update libusb --- CMake/external_libusb.cmake | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/CMake/external_libusb.cmake b/CMake/external_libusb.cmake index 7d32981932..2d452d2b56 100644 --- a/CMake/external_libusb.cmake +++ b/CMake/external_libusb.cmake @@ -3,14 +3,10 @@ include(ExternalProject) ExternalProject_Add( libusb - # Work-around for libusb master broken on Nov 26' 2020 with introduction of v1.0.24 - # the issue has been reported in https://github.com/libusb/libusb/issues/812 - GIT_REPOSITORY "https://github.com/ev-mp/libusb.git" - GIT_TAG "2a7372db54094a406a755f0b8548b614ba8c78ec" # "v1.0.22" + Mac get_device_list hang fix + GIT_REPOSITORY "https://github.com/libusb/libusb-cmake.git" + GIT_TAG "v1.0.27-1" # "v1.0.27-1" - UPDATE_COMMAND ${CMAKE_COMMAND} -E copy_if_different - ${CMAKE_CURRENT_SOURCE_DIR}/third-party/libusb/CMakeLists.txt - ${CMAKE_CURRENT_BINARY_DIR}/third-party/libusb/CMakeLists.txt + UPDATE_COMMAND "" PATCH_COMMAND "" SOURCE_DIR "third-party/libusb/" From c02549112d599df893ce35c7b054999fa90a2505 Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Mon, 14 Oct 2024 17:13:56 +0300 Subject: [PATCH 53/75] remove --- CMake/external_libusb.cmake | 7 ++++--- .../libusb/{CMakeLists.txt => CMakeLists-remove.txt} | 0 2 files changed, 4 insertions(+), 3 deletions(-) rename third-party/libusb/{CMakeLists.txt => CMakeLists-remove.txt} (100%) diff --git a/CMake/external_libusb.cmake b/CMake/external_libusb.cmake index 2d452d2b56..eb6d7076ac 100644 --- a/CMake/external_libusb.cmake +++ b/CMake/external_libusb.cmake @@ -2,7 +2,7 @@ include(ExternalProject) ExternalProject_Add( libusb - + PREFIX libusb GIT_REPOSITORY "https://github.com/libusb/libusb-cmake.git" GIT_TAG "v1.0.27-1" # "v1.0.27-1" @@ -16,14 +16,15 @@ ExternalProject_Add( -DANDROID_ABI=${ANDROID_ABI} -DANDROID_STL=${ANDROID_STL} -DCMAKE_INSTALL_PREFIX=${CMAKE_CURRENT_BINARY_DIR}/libusb_install + -DLIBUSB_INSTALL_TARGETS=ON --no-warn-unused-cli TEST_COMMAND "" - BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/libusb_install/lib/${CMAKE_STATIC_LIBRARY_PREFIX}usb${CMAKE_STATIC_LIBRARY_SUFFIX} + #BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/libusb_install/lib/${CMAKE_STATIC_LIBRARY_PREFIX}usb${CMAKE_STATIC_LIBRARY_SUFFIX} ) add_library(usb INTERFACE) target_include_directories(usb INTERFACE $) -target_link_libraries(usb INTERFACE ${CMAKE_CURRENT_BINARY_DIR}/libusb_install/lib/${CMAKE_STATIC_LIBRARY_PREFIX}usb${CMAKE_STATIC_LIBRARY_SUFFIX}) +target_link_libraries(usb INTERFACE ${CMAKE_CURRENT_BINARY_DIR}/libusb_install/lib/${CMAKE_STATIC_LIBRARY_PREFIX}libusb-1.0${CMAKE_STATIC_LIBRARY_SUFFIX}) set(USE_EXTERNAL_USB ON) # INTERFACE libraries can't have real deps, so targets that link with usb need to also depend on libusb set_target_properties( libusb PROPERTIES FOLDER "3rd Party") diff --git a/third-party/libusb/CMakeLists.txt b/third-party/libusb/CMakeLists-remove.txt similarity index 100% rename from third-party/libusb/CMakeLists.txt rename to third-party/libusb/CMakeLists-remove.txt From bdcfb3303aab4b77020ae23207f4fc65b0289804 Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Mon, 14 Oct 2024 22:34:07 +0300 Subject: [PATCH 54/75] add shared --- CMake/external_libusb.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/CMake/external_libusb.cmake b/CMake/external_libusb.cmake index eb6d7076ac..5ec1bb8b12 100644 --- a/CMake/external_libusb.cmake +++ b/CMake/external_libusb.cmake @@ -15,6 +15,7 @@ ExternalProject_Add( -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} -DANDROID_ABI=${ANDROID_ABI} -DANDROID_STL=${ANDROID_STL} + -DBUILD_SHARED_LIBS=OFF -DCMAKE_INSTALL_PREFIX=${CMAKE_CURRENT_BINARY_DIR}/libusb_install -DLIBUSB_INSTALL_TARGETS=ON --no-warn-unused-cli From 3eb293b2b6826cd3dd6070b5a5167a61ff348e9f Mon Sep 17 00:00:00 2001 From: Eran Date: Tue, 15 Oct 2024 12:05:56 +0300 Subject: [PATCH 55/75] add heartbeat-period and default to half reply timeout --- third-party/realdds/src/dds-device-impl.cpp | 5 ++++- third-party/realdds/src/dds-topic-writer.cpp | 2 ++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/third-party/realdds/src/dds-device-impl.cpp b/third-party/realdds/src/dds-device-impl.cpp index 6dd10f1511..a79d99237e 100644 --- a/third-party/realdds/src/dds-device-impl.cpp +++ b/third-party/realdds/src/dds-device-impl.cpp @@ -85,7 +85,7 @@ dds_device::impl::impl( std::shared_ptr< dds_participant > const & participant, , _subscriber( std::make_shared< dds_subscriber >( participant ) ) , _device_settings( device_settings( participant ) ) , _reply_timeout_ms( - _device_settings.nested( "control", "reply-timeout-ms" ).default_value< size_t >( 2000 ) ) + _device_settings.nested( "control", "reply-timeout-ms" ).default_value< size_t >( 2500 ) ) { create_control_writer(); create_notifications_reader(); @@ -564,6 +564,9 @@ void dds_device::impl::create_control_writer() _control_writer = std::make_shared< dds_topic_writer >( topic ); dds_topic_writer::qos wqos( eprosima::fastdds::dds::RELIABLE_RELIABILITY_QOS ); wqos.history().depth = 10; // default is 1 + // If our reply timeout is less than the heartbeat period, we could lose the control message! + // So we set a short heartbeat time at half the reply timeout... + wqos.reliable_writer_qos().times.heartbeatPeriod = _reply_timeout_ms / 2000.; _control_writer->override_qos_from_json( wqos, _device_settings.nested( "control" ) ); _control_writer->run( wqos ); } diff --git a/third-party/realdds/src/dds-topic-writer.cpp b/third-party/realdds/src/dds-topic-writer.cpp index 7051b44f38..11f9ee8cd5 100644 --- a/third-party/realdds/src/dds-topic-writer.cpp +++ b/third-party/realdds/src/dds-topic-writer.cpp @@ -92,6 +92,8 @@ void dds_topic_writer::override_qos_from_json( qos & wqos, rsutils::json const & // Default values should be set before we're called: // All we do here is override those - if specified! override_reliability_qos_from_json( wqos.reliability(), qos_settings.nested( "reliability" ) ); + if( wqos.reliability().kind == eprosima::fastdds::dds::RELIABLE_RELIABILITY_QOS ) + qos_settings.nested( "heartbeat-period" ).get_ex( wqos.reliable_writer_qos().times.heartbeatPeriod ); override_durability_qos_from_json( wqos.durability(), qos_settings.nested( "durability" ) ); override_history_qos_from_json( wqos.history(), qos_settings.nested( "history" ) ); override_liveliness_qos_from_json( wqos.liveliness(), qos_settings.nested( "liveliness" ) ); From 24c4dcc135fdc7d1a1f098dd1b61b900856d6736 Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Mon, 14 Oct 2024 10:47:34 +0300 Subject: [PATCH 56/75] fix compile options --- CMake/external_fastdds.cmake | 5 +++++ CMakeLists.txt | 6 ++++++ third-party/CMakeLists.txt | 15 +++++++-------- third-party/rsutils/CMakeLists.txt | 1 - third-party/rsutils/src/json.cpp | 4 ++-- 5 files changed, 20 insertions(+), 11 deletions(-) diff --git a/CMake/external_fastdds.cmake b/CMake/external_fastdds.cmake index 856e0dbfc7..00f8d7b8b9 100644 --- a/CMake/external_fastdds.cmake +++ b/CMake/external_fastdds.cmake @@ -60,6 +60,11 @@ function(get_fastdds) add_library(dds INTERFACE) target_link_libraries( dds INTERFACE fastcdr fastrtps ) + if (MSVC) + target_compile_options( dds INTERFACE "/W0" ) + elseif (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + target_compile_options( dds INTERFACE "-w" ) + endif() add_definitions(-DBUILD_WITH_DDS) diff --git a/CMakeLists.txt b/CMakeLists.txt index 467c844b09..3f9fd95030 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,11 +61,17 @@ target_link_libraries( ${LRS_TARGET} PUBLIC rsutils ) if(BUILD_WITH_DDS) if (CMAKE_SYSTEM MATCHES "Windows" OR CMAKE_SYSTEM MATCHES "Linux") + string(REPLACE "${SECURITY_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") # remove security flags + string(REPLACE "${SECURITY_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") + message(STATUS "Building with FastDDS") include(CMake/external_foonathan_memory.cmake) include(CMake/external_fastdds.cmake) target_link_libraries( ${LRS_TARGET} PRIVATE realdds ) + + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${SECURITY_COMPILER_FLAGS}") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${SECURITY_COMPILER_FLAGS}") else() MESSAGE(STATUS "Turning off `BUILD_WITH_DDS` as it's only supported on Windows & Linux and not on ${CMAKE_SYSTEM}") diff --git a/third-party/CMakeLists.txt b/third-party/CMakeLists.txt index 9eb1a680b8..47164ed9a4 100644 --- a/third-party/CMakeLists.txt +++ b/third-party/CMakeLists.txt @@ -1,16 +1,12 @@ string(REPLACE ${PROJECT_SOURCE_DIR}/ "" _rel_path ${CMAKE_CURRENT_LIST_DIR}) -# ignore warnings on third party files -if (MSVC) - add_compile_options(/W0) -elseif (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") - add_compile_options(-w) -endif() - -include(CMake/external_json.cmake) add_subdirectory( "${CMAKE_CURRENT_LIST_DIR}/rsutils" ) +string(REPLACE "${SECURITY_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") # remove security flags +string(REPLACE "${SECURITY_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") + +include(CMake/external_json.cmake) # Add additional include directories to allow file to include rosbag headers include(${_rel_path}/realsense-file/config.cmake) @@ -25,3 +21,6 @@ if( BUILD_WITH_DDS ) add_subdirectory( "${CMAKE_CURRENT_LIST_DIR}/realdds" ) endif() +# restore flags +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${SECURITY_COMPILER_FLAGS}") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${SECURITY_COMPILER_FLAGS}") diff --git a/third-party/rsutils/CMakeLists.txt b/third-party/rsutils/CMakeLists.txt index 386a8c1ba9..b0fc310497 100644 --- a/third-party/rsutils/CMakeLists.txt +++ b/third-party/rsutils/CMakeLists.txt @@ -3,7 +3,6 @@ cmake_minimum_required(VERSION 3.8.0) # source_group(TREE) project( rsutils ) -set_directory_properties(PROPERTIES COMPILE_FLAGS "") # unignore warnings on rsutils - remove flag applied at third-party folder add_library( ${PROJECT_NAME} STATIC "" ) # We cannot directly interface with nlohmann_json (doesn't work on bionic) #target_link_libraries( ${PROJECT_NAME} PUBLIC nlohmann_json ) diff --git a/third-party/rsutils/src/json.cpp b/third-party/rsutils/src/json.cpp index 81653ada1d..6428bc7003 100644 --- a/third-party/rsutils/src/json.cpp +++ b/third-party/rsutils/src/json.cpp @@ -411,7 +411,7 @@ class serializer { dump( *i, pretty_print_width, ensure_ascii, indent_step, new_indent ); _o.put( ',' ); - if( need_to_indent || pretty_print_width && _line_width > pretty_print_width ) + if( need_to_indent || (pretty_print_width && _line_width > pretty_print_width )) { newline(); _o.write( _indent_string.c_str(), new_indent ); @@ -1100,7 +1100,7 @@ class serializer } }; - JSON_ASSERT(byte < utf8d.size()); + JSON_ASSERT(static_cast(byte) < utf8d.size()); const std::uint8_t type = utf8d[byte]; codep = (state != UTF8_ACCEPT) From 262cc9afc1f4d2db01f3eab169918984ea59087b Mon Sep 17 00:00:00 2001 From: Yu Meng Date: Thu, 17 Oct 2024 17:23:35 +0800 Subject: [PATCH 57/75] fix build for android test --- .../intel/realsense/librealsense/ExampleInstrumentedTest.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wrappers/android/librealsense/src/androidTest/java/com/intel/realsense/librealsense/ExampleInstrumentedTest.java b/wrappers/android/librealsense/src/androidTest/java/com/intel/realsense/librealsense/ExampleInstrumentedTest.java index 454e7e94cb..4c2fade39f 100644 --- a/wrappers/android/librealsense/src/androidTest/java/com/intel/realsense/librealsense/ExampleInstrumentedTest.java +++ b/wrappers/android/librealsense/src/androidTest/java/com/intel/realsense/librealsense/ExampleInstrumentedTest.java @@ -19,7 +19,7 @@ public class ExampleInstrumentedTest { @Test public void useAppContext() { // RsContext of the app under test. - Context appContext = InstrumentationRegistry.getTargetContext(); + Context appContext = InstrumentationRegistry.getInstrumentation().getContext(); assertEquals("com.intel.realsense.librealsense", appContext.getPackageName()); } From 13bd86daa50925ff9e0e058883a77cdc8d13681f Mon Sep 17 00:00:00 2001 From: Yu Meng Date: Thu, 17 Oct 2024 17:24:36 +0800 Subject: [PATCH 58/75] fix android build. upgrade AGP to 8.1 for Android Studio 2024.2.1 --- wrappers/android/build.gradle | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wrappers/android/build.gradle b/wrappers/android/build.gradle index 7704849724..a8443897ee 100644 --- a/wrappers/android/build.gradle +++ b/wrappers/android/build.gradle @@ -8,8 +8,8 @@ plugins { * see Applying external plugins with same version to subprojects. */ - id 'com.android.application' version '8.0.1' apply false - id 'com.android.library' version '8.0.1' apply false + id 'com.android.application' version '8.1.4' apply false + id 'com.android.library' version '8.1.4' apply false id 'org.jetbrains.kotlin.android' version '1.8.20' apply false } From f5f50a7414cf1d51a79319ffae0f197fecbc3a60 Mon Sep 17 00:00:00 2001 From: Eran Date: Tue, 8 Oct 2024 13:06:04 +0300 Subject: [PATCH 59/75] pyrealdds.image_msg.data as a memoryview; fps.py --save-frames --- third-party/realdds/py/pyrealdds.cpp | 2 +- third-party/realdds/scripts/fps.py | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/third-party/realdds/py/pyrealdds.cpp b/third-party/realdds/py/pyrealdds.cpp index 314440496d..61e3fad4a4 100644 --- a/third-party/realdds/py/pyrealdds.cpp +++ b/third-party/realdds/py/pyrealdds.cpp @@ -625,7 +625,7 @@ PYBIND11_MODULE(NAME, m) { .def_static( "create_topic", &image_msg::create_topic ) .def_property( "data", - []( image_msg const & self ) { return self.raw().data(); }, + []( image_msg const & self ) { return py::memoryview::from_memory( self.raw().data().data(), self.raw().data().size() ); }, []( image_msg & self, std::vector< uint8_t > bytes ) { self.raw().data( std::move( bytes ) ); } ) .def_property( "width", &image_msg::width, &image_msg::set_width ) .def_property( "height", &image_msg::height, &image_msg::set_height ) diff --git a/third-party/realdds/scripts/fps.py b/third-party/realdds/scripts/fps.py index 854c2bc87e..9a87cf2b74 100644 --- a/third-party/realdds/scripts/fps.py +++ b/third-party/realdds/scripts/fps.py @@ -6,6 +6,7 @@ args.add_argument( '--debug', action='store_true', help='enable debug mode' ) args.add_argument( '--quiet', action='store_true', help='No output; just the minimum FPS as a number' ) args.add_argument( '--debug-frames', action='store_true', help='Output frame signatures as we get them' ) +args.add_argument( '--save-frames', action='store_true', help='Save each image data on disk, as _' ) args.add_argument( '--device', metavar='', required=True, help='the topic root for the device' ) def time_arg(x): t = int(x) @@ -97,6 +98,10 @@ def on_image( stream, image, sample ): global n_stream_frames, capturing if capturing: n_stream_frames[stream.name()] += 1 + if args.save_frames: + filename = f'{stream.name().lower()}_{image.frame_id or n_stream_frames[stream.name()]}' + with open( filename, 'wb' ) as f: + f.write( image.data ) fps = args.fps width = args.res[0] From 4a16924f8603c9a143de6df3a4bb570f04cd0c6f Mon Sep 17 00:00:00 2001 From: ohadmeir Date: Sun, 20 Oct 2024 11:02:43 +0300 Subject: [PATCH 60/75] D555 on USB should start with Default visual preset --- src/ds/d500/d500-factory.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/ds/d500/d500-factory.cpp b/src/ds/d500/d500-factory.cpp index e411212a6d..cea6c6a3ca 100644 --- a/src/ds/d500/d500-factory.cpp +++ b/src/ds/d500/d500-factory.cpp @@ -76,6 +76,10 @@ class d555_device depth_sensor.register_option( RS2_OPTION_THERMAL_COMPENSATION, std::make_shared< thermal_compensation >( _thermal_monitor, thermal_compensation_toggle ) ); + + // We usually use "Custom" visual preset becasue we don't know what is the current setting. + // When connected by Ethernet D555 does not support "Custom" so we set here to "Default" to match. + depth_sensor.get_option( RS2_OPTION_VISUAL_PRESET ).set( RS2_RS400_VISUAL_PRESET_DEFAULT ); } ); // group_multiple_fw_calls } From 011ffaec3eeda5d32b4f5ac3290ad3689fa83816 Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Sun, 20 Oct 2024 13:42:44 +0300 Subject: [PATCH 61/75] make hwmon_response product line dependent --- src/ds/d400/d400-device.cpp | 6 +- src/ds/d400/d400-device.h | 1 + src/ds/d400/d400-private.h | 154 ++++++++++++++++++++ src/ds/d500/d500-device.cpp | 4 +- src/ds/d500/d500-device.h | 2 + src/ds/d500/d500-private.h | 102 ++++++++++++++ src/ds/d500/hw_monitor_extended_buffers.h | 4 +- src/ds/ds-options.cpp | 13 +- src/ds/ds-options.h | 3 +- src/hdr-config.cpp | 9 +- src/hw-monitor.cpp | 8 +- src/hw-monitor.h | 162 ++-------------------- 12 files changed, 290 insertions(+), 178 deletions(-) diff --git a/src/ds/d400/d400-device.cpp b/src/ds/d400/d400-device.cpp index f78f914566..be105ee927 100644 --- a/src/ds/d400/d400-device.cpp +++ b/src/ds/d400/d400-device.cpp @@ -552,7 +552,7 @@ namespace librealsense _hw_monitor = std::make_shared( std::make_shared( std::make_shared( *raw_sensor, depth_xu, DS5_HWMONITOR ), - raw_sensor ) ); + raw_sensor ), hw_monitor_response); } else { @@ -560,7 +560,7 @@ namespace librealsense _hw_monitor = std::make_shared< hw_monitor >( std::make_shared< locked_transfer >( get_backend()->create_usb_device( group.usb_devices.front() ), - raw_sensor ) ); + raw_sensor ), hw_monitor_response); } set_hw_monitor_for_auto_calib(_hw_monitor); @@ -841,7 +841,7 @@ namespace librealsense if ((_fw_version >= firmware_version("5.11.3.0")) && ((_device_capabilities & mask) == mask)) { bool is_fw_version_using_id = (_fw_version >= firmware_version("5.12.8.100")); - auto alternating_emitter_opt = std::make_shared(*_hw_monitor, is_fw_version_using_id); + auto alternating_emitter_opt = std::make_shared(*_hw_monitor, is_fw_version_using_id, ds::d400_hwmon_response_handler::d400_hwmon_response_names::hwm_NoDataToReturn); auto emitter_always_on_opt = std::make_shared( _hw_monitor, ds::LASERONCONST, ds::LASERONCONST ); if ((_fw_version >= firmware_version("5.12.1.0")) && ((_device_capabilities & ds_caps::CAP_GLOBAL_SHUTTER) == ds_caps::CAP_GLOBAL_SHUTTER)) diff --git a/src/ds/d400/d400-device.h b/src/ds/d400/d400-device.h index 0816b54d03..0b773bc5f8 100644 --- a/src/ds/d400/d400-device.h +++ b/src/ds/d400/d400-device.h @@ -94,6 +94,7 @@ namespace librealsense friend class d400_depth_sensor; std::shared_ptr _hw_monitor; + std::shared_ptr hw_monitor_response = std::make_shared(); firmware_version _fw_version; firmware_version _recommended_fw_version; ds::ds_caps _device_capabilities; diff --git a/src/ds/d400/d400-private.h b/src/ds/d400/d400-private.h index 6b2f78c8ff..73022bb470 100644 --- a/src/ds/d400/d400-private.h +++ b/src/ds/d400/d400-private.h @@ -341,5 +341,159 @@ namespace librealsense { isp_boot_data_upload_failed, "ISP boot data upload failure" }, }; + class d400_hwmon_response_handler : public base_hwmon_response_handler + { + public: + enum d400_hwmon_response_names : int32_t + { + hwm_Success = 0, + hwm_WrongCommand = -1, + hwm_StartNGEndAddr = -2, + hwm_AddressSpaceNotAligned = -3, + hwm_AddressSpaceTooSmall = -4, + hwm_ReadOnly = -5, + hwm_WrongParameter = -6, + hwm_HWNotReady = -7, + hwm_I2CAccessFailed = -8, + hwm_NoExpectedUserAction = -9, + hwm_IntegrityError = -10, + hwm_NullOrZeroSizeString = -11, + hwm_GPIOPinNumberInvalid = -12, + hwm_GPIOPinDirectionInvalid = -13, + hwm_IllegalAddress = -14, + hwm_IllegalSize = -15, + hwm_ParamsTableNotValid = -16, + hwm_ParamsTableIdNotValid = -17, + hwm_ParamsTableWrongExistingSize = -18, + hwm_WrongCRC = -19, + hwm_NotAuthorisedFlashWrite = -20, + hwm_NoDataToReturn = -21, + hwm_SpiReadFailed = -22, + hwm_SpiWriteFailed = -23, + hwm_SpiEraseSectorFailed = -24, + hwm_TableIsEmpty = -25, + hwm_I2cSeqDelay = -26, + hwm_CommandIsLocked = -27, + hwm_CalibrationWrongTableId = -28, + hwm_ValueOutOfRange = -29, + hwm_InvalidDepthFormat = -30, + hwm_DepthFlowError = -31, + hwm_Timeout = -32, + hwm_NotSafeCheckFailed = -33, + hwm_FlashRegionIsLocked = -34, + hwm_SummingEventTimeout = -35, + hwm_SDSCorrupted = -36, + hwm_SDSVerifyFailed = -37, + hwm_IllegalHwState = -38, + hwm_RealtekNotLoaded = -39, + hwm_WakeUpDeviceNotSupported = -40, + hwm_ResourceBusy = -41, + hwm_MaxErrorValue = -42, + hwm_PwmNotSupported = -43, + hwm_PwmStereoModuleNotConnected = -44, + hwm_UvcStreamInvalidStreamRequest = -45, + hwm_UvcControlManualExposureInvalid = -46, + hwm_UvcControlManualGainInvalid = -47, + hwm_EyesafetyPayloadFailure = -48, + hwm_ProjectorTestFailed = -49, + hwm_ThreadModifyFailed = -50, + hwm_HotLaserPwrReduce = -51, // reported to error depth XU control + hwm_HotLaserDisable = -52, // reported to error depth XU control + hwm_FlagBLaserDisable = -53, // reported to error depth XU control + hwm_NoStateChange = -54, + hwm_EEPROMIsLocked = -55, + hwm_OEMIdWrong = -56, + hwm_RealtekNotUpdated = -57, + hwm_FunctionNotSupported = -58, + hwm_IspNotImplemented = -59, + hwm_IspNotSupported = -60, + hwm_IspNotPermited = -61, + hwm_IspNotExists = -62, + hwm_IspFail = -63, + hwm_Unknown = -64, + hwm_LastError = hwm_Unknown - 1, + }; + + // Elaborate HW Monitor response + std::map< hwmon_response, std::string> hwmon_response_report = { + { d400_hwmon_response_names::hwm_Success, "Success" }, + { d400_hwmon_response_names::hwm_WrongCommand, "Invalid Command" }, + { d400_hwmon_response_names::hwm_StartNGEndAddr, "Start NG End Address" }, + { d400_hwmon_response_names::hwm_AddressSpaceNotAligned, "Address space not aligned" }, + { d400_hwmon_response_names::hwm_AddressSpaceTooSmall, "Address space too small" }, + { d400_hwmon_response_names::hwm_ReadOnly, "Read-only" }, + { d400_hwmon_response_names::hwm_WrongParameter, "Invalid parameter" }, + { d400_hwmon_response_names::hwm_HWNotReady, "HW not ready" }, + { d400_hwmon_response_names::hwm_I2CAccessFailed, "I2C access failed" }, + { d400_hwmon_response_names::hwm_NoExpectedUserAction, "No expected user action" }, + { d400_hwmon_response_names::hwm_IntegrityError, "Integrity error" }, + { d400_hwmon_response_names::hwm_NullOrZeroSizeString, "Null or zero size string" }, + { d400_hwmon_response_names::hwm_GPIOPinNumberInvalid, "GPIOP in number invalid" }, + { d400_hwmon_response_names::hwm_GPIOPinDirectionInvalid, "GPIOP in direction invalid" }, + { d400_hwmon_response_names::hwm_IllegalAddress, "Illegal address" }, + { d400_hwmon_response_names::hwm_IllegalSize, "Illegal size" }, + { d400_hwmon_response_names::hwm_ParamsTableNotValid, "Params table not valid" }, + { d400_hwmon_response_names::hwm_ParamsTableIdNotValid, "Params table id not valid" }, + { d400_hwmon_response_names::hwm_ParamsTableWrongExistingSize, "Params rable wrong existing size" }, + { d400_hwmon_response_names::hwm_WrongCRC, "Invalid CRC" }, + { d400_hwmon_response_names::hwm_NotAuthorisedFlashWrite, "Not authorised flash write" }, + { d400_hwmon_response_names::hwm_NoDataToReturn, "No data to return" }, + { d400_hwmon_response_names::hwm_SpiReadFailed, "Spi read failed" }, + { d400_hwmon_response_names::hwm_SpiWriteFailed, "Spi write failed" }, + { d400_hwmon_response_names::hwm_SpiEraseSectorFailed, "Spi erase sector failed" }, + { d400_hwmon_response_names::hwm_TableIsEmpty, "Table is empty" }, + { d400_hwmon_response_names::hwm_I2cSeqDelay, "I2c seq delay" }, + { d400_hwmon_response_names::hwm_CommandIsLocked, "Command is locked" }, + { d400_hwmon_response_names::hwm_CalibrationWrongTableId, "Calibration invalid table id" }, + { d400_hwmon_response_names::hwm_ValueOutOfRange, "Value out of range" }, + { d400_hwmon_response_names::hwm_InvalidDepthFormat, "Invalid depth format" }, + { d400_hwmon_response_names::hwm_DepthFlowError, "Depth flow error" }, + { d400_hwmon_response_names::hwm_Timeout, "Timeout" }, + { d400_hwmon_response_names::hwm_NotSafeCheckFailed, "Not safe check failed" }, + { d400_hwmon_response_names::hwm_FlashRegionIsLocked, "Flash region is locked" }, + { d400_hwmon_response_names::hwm_SummingEventTimeout, "Summing event timeout" }, + { d400_hwmon_response_names::hwm_SDSCorrupted, "SDS corrupted" }, + { d400_hwmon_response_names::hwm_SDSVerifyFailed, "SDS verification failed" }, + { d400_hwmon_response_names::hwm_IllegalHwState, "Illegal HW state" }, + { d400_hwmon_response_names::hwm_RealtekNotLoaded, "Realtek not loaded" }, + { d400_hwmon_response_names::hwm_WakeUpDeviceNotSupported, "Wake up device not supported" }, + { d400_hwmon_response_names::hwm_ResourceBusy, "Resource busy" }, + { d400_hwmon_response_names::hwm_MaxErrorValue, "Max error value" }, + { d400_hwmon_response_names::hwm_PwmNotSupported, "Pwm not supported" }, + { d400_hwmon_response_names::hwm_PwmStereoModuleNotConnected, "Pwm stereo module not connected" }, + { d400_hwmon_response_names::hwm_UvcStreamInvalidStreamRequest,"Uvc stream invalid stream request" }, + { d400_hwmon_response_names::hwm_UvcControlManualExposureInvalid,"Uvc control manual exposure invalid" }, + { d400_hwmon_response_names::hwm_UvcControlManualGainInvalid, "Uvc control manual gain invalid" }, + { d400_hwmon_response_names::hwm_EyesafetyPayloadFailure, "Eyesafety payload failure" }, + { d400_hwmon_response_names::hwm_ProjectorTestFailed, "Projector test failed" }, + { d400_hwmon_response_names::hwm_ThreadModifyFailed, "Thread modify failed" }, + { d400_hwmon_response_names::hwm_HotLaserPwrReduce, "Hot laser pwr reduce" }, + { d400_hwmon_response_names::hwm_HotLaserDisable, "Hot laser disable" }, + { d400_hwmon_response_names::hwm_FlagBLaserDisable, "FlagB laser disable" }, + { d400_hwmon_response_names::hwm_NoStateChange, "No state change" }, + { d400_hwmon_response_names::hwm_EEPROMIsLocked, "EEPROM is locked" }, + { d400_hwmon_response_names::hwm_EEPROMIsLocked, "EEPROM Is locked" }, + { d400_hwmon_response_names::hwm_OEMIdWrong, "OEM invalid id" }, + { d400_hwmon_response_names::hwm_RealtekNotUpdated, "Realtek not updated" }, + { d400_hwmon_response_names::hwm_FunctionNotSupported, "Function not supported" }, + { d400_hwmon_response_names::hwm_IspNotImplemented, "Isp not implemented" }, + { d400_hwmon_response_names::hwm_IspNotSupported, "Isp not supported" }, + { d400_hwmon_response_names::hwm_IspNotPermited, "Isp not permited" }, + { d400_hwmon_response_names::hwm_IspNotExists, "Isp not present" }, + { d400_hwmon_response_names::hwm_IspFail, "Isp fail" }, + { d400_hwmon_response_names::hwm_Unknown, "Unresolved error" }, + { d400_hwmon_response_names::hwm_LastError, "Last error" }, + }; + + inline virtual std::string hwmon_error2str(hwmon_response opcode) const override { + if (hwmon_response_report.find(opcode) != hwmon_response_report.end()) + return hwmon_response_report.at(opcode); + return {}; + } + + virtual hwmon_response hwmon_Success() const override { return d400_hwmon_response_names::hwm_Success; }; + }; + + } // namespace ds } // namespace librealsense diff --git a/src/ds/d500/d500-device.cpp b/src/ds/d500/d500-device.cpp index 8342d7edf7..208a18c729 100644 --- a/src/ds/d500/d500-device.cpp +++ b/src/ds/d500/d500-device.cpp @@ -407,13 +407,13 @@ namespace librealsense _hw_monitor = std::make_shared( std::make_shared( std::make_shared( *raw_sensor, depth_xu, DS5_HWMONITOR ), - raw_sensor)); + 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 ) ); + raw_sensor ), hw_monitor_response); } _ds_device_common = std::make_shared(this, _hw_monitor); diff --git a/src/ds/d500/d500-device.h b/src/ds/d500/d500-device.h index a0c14ea43c..2a95cc4079 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::make_shared(); + std::vector send_receive_raw_data(const std::vector& input) override; std::vector build_command(uint32_t opcode, diff --git a/src/ds/d500/d500-private.h b/src/ds/d500/d500-private.h index d960cf5e14..080f4a3de8 100644 --- a/src/ds/d500/d500-private.h +++ b/src/ds/d500/d500-private.h @@ -454,5 +454,107 @@ namespace librealsense { EHU_IDX_DSP_UP_CHECKSUM_ERR, "DSP UP CHECKSUM ERROR" }, }; + class d500_hwmon_response_handler : public base_hwmon_response_handler + { + public: + enum d500_hwmon_response_names : int32_t + { + hwm_Success = 0, + hwm_InvalidCommand = -1, + hwm_InvalidParam = -2, + hwm_HWNotReady = -3, // (different from #21) + hwm_UnauthorizedUserAction = -4, + hwm_IntegrityError = -5, + hwm_CRC_Error = -6, + hwm_GPIOPinNumberInvalid = -7, + hwm_GPIOPinDirectionInvalid = -8, + hwm_IllegalAddress = -9, + hwm_IllegalSize = -10, + hwm_ParamsTableNotValid = -11, + hwm_ParamsTableIdNotValid = -12, + hwm_ParamsTableWrongExistingSize = -13, + hwm_SpiReadFailed = -14, + hwm_SpiWriteFailed = -15, + hwm_TableIsEmpty = -16, + hwm_ValueOutOfRange = -17, + hwm_Operation_Timeout = -18, + hwm_CommandNotSupported = -19, // (inappropriate FW/SKU) + hwm_IncompleteData = -20, + hwm_SWNotReady = -21, // (mind the difference from #3) + hwm_Reserved_22 = -22, + hwm_Reserved_23 = -23, + hwm_Reserved_24 = -24, + hwm_Reserved_25 = -25, + hwm_Reserved_26 = -26, + hwm_Reserved_27 = -27, + hwm_Reserved_28 = -28, + hwm_Reserved_29 = -29, + hwm_Reserved_30 = -30, + hwm_Reserved_31 = -31, + hwm_Reserved_32 = -32, + hwm_Reserved_33 = -33, + hwm_Reserved_34 = -34, + hwm_Reserved_35 = -35, + hwm_Reserved_36 = -36, + hwm_Reserved_37 = -37, + hwm_Reserved_38 = -38, + hwm_Reserved_39 = -39, + hwm_LastError = hwm_Reserved_39 - 1, // if more error codes are added, this value should be updated + }; + + // Elaborate HW Monitor response + const std::map< hwmon_response, std::string> hwmon_response_report = { + { d500_hwmon_response_names::hwm_Success, "Success" }, + { d500_hwmon_response_names::hwm_InvalidCommand, "Invalid Command" }, + { d500_hwmon_response_names::hwm_InvalidParam, "Invalid Param" }, + { d500_hwmon_response_names::hwm_HWNotReady, "HW Not Ready" }, + { d500_hwmon_response_names::hwm_UnauthorizedUserAction, "Unauthorized User Action" }, + { d500_hwmon_response_names::hwm_IntegrityError, "Integrity Error" }, + { d500_hwmon_response_names::hwm_CRC_Error, "CRC Error" }, + { d500_hwmon_response_names::hwm_GPIOPinNumberInvalid, "GPIO Pin Number Invalid" }, + { d500_hwmon_response_names::hwm_GPIOPinDirectionInvalid, "GPIO Pin Direction Invalid" }, + { d500_hwmon_response_names::hwm_IllegalAddress, "Illegal Address" }, + { d500_hwmon_response_names::hwm_IllegalSize, "Illegal Size" }, + { d500_hwmon_response_names::hwm_ParamsTableNotValid, "Params Table Not Valid" }, + { d500_hwmon_response_names::hwm_ParamsTableIdNotValid, "Params Table Id Not Valid" }, + { d500_hwmon_response_names::hwm_ParamsTableWrongExistingSize, "Params Table Wrong Existing Size" }, + { d500_hwmon_response_names::hwm_SpiReadFailed, "Spi Read Failed" }, + { d500_hwmon_response_names::hwm_SpiWriteFailed, "Spi Write Failed" }, + { d500_hwmon_response_names::hwm_TableIsEmpty, "Table Is Empty" }, + { d500_hwmon_response_names::hwm_ValueOutOfRange, "Value Out Of Range" }, + { d500_hwmon_response_names::hwm_Operation_Timeout, "Operation Timeout" }, + { d500_hwmon_response_names::hwm_CommandNotSupported, "Command Not Supported" }, + { d500_hwmon_response_names::hwm_IncompleteData, "Incomplete Data" }, + { d500_hwmon_response_names::hwm_SWNotReady, "SW Not Ready" }, + { d500_hwmon_response_names::hwm_Reserved_22, "Reserved 22" }, + { d500_hwmon_response_names::hwm_Reserved_23, "Reserved 23" }, + { d500_hwmon_response_names::hwm_Reserved_24, "Reserved 24" }, + { d500_hwmon_response_names::hwm_Reserved_25, "Reserved 25" }, + { d500_hwmon_response_names::hwm_Reserved_26, "Reserved 26" }, + { d500_hwmon_response_names::hwm_Reserved_27, "Reserved 27" }, + { d500_hwmon_response_names::hwm_Reserved_28, "Reserved 28" }, + { d500_hwmon_response_names::hwm_Reserved_29, "Reserved 29" }, + { d500_hwmon_response_names::hwm_Reserved_30, "Reserved 30" }, + { d500_hwmon_response_names::hwm_Reserved_31, "Reserved 31" }, + { d500_hwmon_response_names::hwm_Reserved_32, "Reserved 32" }, + { d500_hwmon_response_names::hwm_Reserved_33, "Reserved 33" }, + { d500_hwmon_response_names::hwm_Reserved_34, "Reserved 34" }, + { d500_hwmon_response_names::hwm_Reserved_35, "Reserved 35" }, + { d500_hwmon_response_names::hwm_Reserved_36, "Reserved 36" }, + { d500_hwmon_response_names::hwm_Reserved_37, "Reserved 37" }, + { d500_hwmon_response_names::hwm_Reserved_38, "Reserved 38" }, + { d500_hwmon_response_names::hwm_Reserved_39, "Reserved 39" }, + { d500_hwmon_response_names::hwm_LastError, "Last Error"} + }; + + inline virtual std::string hwmon_error2str(hwmon_response opcode) const override { + if (hwmon_response_report.find(opcode) != hwmon_response_report.end()) + return hwmon_response_report.at(opcode); + return {}; + } + + virtual hwmon_response hwmon_Success() const override { return d500_hwmon_response_names::hwm_Success; }; + }; + } // namespace ds } // namespace librealsense \ No newline at end of file diff --git a/src/ds/d500/hw_monitor_extended_buffers.h b/src/ds/d500/hw_monitor_extended_buffers.h index 1a91a596cf..9b82fa5e66 100644 --- a/src/ds/d500/hw_monitor_extended_buffers.h +++ b/src/ds/d500/hw_monitor_extended_buffers.h @@ -21,8 +21,8 @@ namespace librealsense extended_send }; - explicit hw_monitor_extended_buffers(std::shared_ptr locked_transfer) - : hw_monitor(locked_transfer) + explicit hw_monitor_extended_buffers(std::shared_ptr locked_transfer, std::shared_ptr hwmon_response_handler) + : hw_monitor(locked_transfer, hwmon_response_handler) {} virtual std::vector send(std::vector const& data) const override; diff --git a/src/ds/ds-options.cpp b/src/ds/ds-options.cpp index 07a1086c9b..bb1548e20e 100644 --- a/src/ds/ds-options.cpp +++ b/src/ds/ds-options.cpp @@ -517,8 +517,8 @@ namespace librealsense return "Inter-camera synchronization mode: 0:Default, 1:Master, 2:Slave"; } - alternating_emitter_option::alternating_emitter_option(hw_monitor& hwm, bool is_fw_version_using_id) - : _hwm(hwm), _is_fw_version_using_id(is_fw_version_using_id) + alternating_emitter_option::alternating_emitter_option(hw_monitor& hwm, bool is_fw_version_using_id, int no_data_to_return_opcode) + : _hwm(hwm), _is_fw_version_using_id(is_fw_version_using_id), no_data_to_return_opcode(no_data_to_return_opcode) { _range = [this]() { @@ -554,18 +554,13 @@ namespace librealsense { hwmon_response response; auto res = _hwm.send( cmd, &response ); // avoid the throw - switch( response ) + if (response != no_data_to_return_opcode) // If no subpreset is streaming, the firmware returns "NO_DATA_TO_RETURN" error { - case hwmon_response::hwm_NoDataToReturn: - // If no subpreset is streaming, the firmware returns "NO_DATA_TO_RETURN" error - break; - default: // if a subpreset is streaming, checking this is the alternating emitter sub preset if( res.size() ) rv = ( res[0] == ds::ALTERNATING_EMITTER_SUBPRESET_ID ) ? 1.0f : 0.f; else - LOG_DEBUG( "alternating emitter query: " << hwmon_error_string( cmd, response ) ); - break; + LOG_DEBUG( "alternating emitter query: " << _hwm.hwmon_error_string( cmd, response ) ); } } catch (...) diff --git a/src/ds/ds-options.h b/src/ds/ds-options.h index 581dbe2393..6d6c97cb59 100644 --- a/src/ds/ds-options.h +++ b/src/ds/ds-options.h @@ -244,7 +244,7 @@ namespace librealsense class alternating_emitter_option : public option { public: - alternating_emitter_option(hw_monitor& hwm, bool is_fw_version_using_id); + alternating_emitter_option(hw_monitor& hwm, bool is_fw_version_using_id, int no_data_to_return_opcode); virtual ~alternating_emitter_option() = default; virtual void set(float value) override; virtual float query() const override; @@ -261,6 +261,7 @@ namespace librealsense rsutils::lazy< option_range > _range; hw_monitor& _hwm; bool _is_fw_version_using_id; + int no_data_to_return_opcode; }; class emitter_always_on_option : public option diff --git a/src/hdr-config.cpp b/src/hdr-config.cpp index 63f8cacf4b..d6226c9cce 100644 --- a/src/hdr-config.cpp +++ b/src/hdr-config.cpp @@ -232,18 +232,13 @@ namespace librealsense { hwmon_response response; auto res = _hwm.send( cmd, &response ); // avoid the throw - switch( response ) + if (response != _hwm.hwmon_response_handler->hwmon_Success()) // If no subpreset is streaming, the firmware returns "NO_DATA_TO_RETURN" error { - case hwmon_response::hwm_NoDataToReturn: - // If no subpreset is streaming, the firmware returns "NO_DATA_TO_RETURN" error - break; - default: // If a subpreset is streaming, checking this is the current HDR sub preset if( res.size() ) rv = ( is_hdr_id( res[0] ) ) ? 1.0f : 0.f; else - LOG_DEBUG( "hdr_config query: " << hwmon_error_string( cmd, response ) ); - break; + LOG_DEBUG( "hdr_config query: " << _hwm.hwmon_error_string( cmd, response ) ); } } catch (...) diff --git a/src/hw-monitor.cpp b/src/hw-monitor.cpp index 8ce92fe40d..0b90ce1b05 100644 --- a/src/hw-monitor.cpp +++ b/src/hw-monitor.cpp @@ -170,7 +170,7 @@ namespace librealsense // Error/exit conditions if (p_response) - *p_response = hwm_Success; + *p_response = hwmon_response_handler->hwmon_Success(); if( ! cmd.require_response ) return {}; @@ -212,16 +212,16 @@ namespace librealsense return result; } - std::string hwmon_error_string( command const & cmd, hwmon_response e ) + std::string hw_monitor::hwmon_error_string( command const & cmd, int response_code ) const { - auto str = hwmon_error2str( e ); + auto str = hwmon_response_handler->hwmon_error2str(response_code); std::ostringstream err; err << "hwmon command 0x" << std::hex << unsigned(cmd.cmd) << '('; err << ' ' << cmd.param1; err << ' ' << cmd.param2; err << ' ' << cmd.param3; err << ' ' << cmd.param4 << std::dec; - err << " ) failed (response " << e << "= " << ( str.empty() ? "unknown" : str ) << ")"; + err << " ) failed (response " << response_code << "= " << ( str.empty() ? "unknown" : str ) << ")"; return err.str(); } diff --git a/src/hw-monitor.h b/src/hw-monitor.h index a83e74b700..122507d161 100644 --- a/src/hw-monitor.h +++ b/src/hw-monitor.h @@ -48,153 +48,6 @@ namespace librealsense class uvc_sensor; - enum hwmon_response: int32_t - { - hwm_Success = 0, - hwm_WrongCommand = -1, - hwm_StartNGEndAddr = -2, - hwm_AddressSpaceNotAligned = -3, - hwm_AddressSpaceTooSmall = -4, - hwm_ReadOnly = -5, - hwm_WrongParameter = -6, - hwm_HWNotReady = -7, - hwm_I2CAccessFailed = -8, - hwm_NoExpectedUserAction = -9, - hwm_IntegrityError = -10, - hwm_NullOrZeroSizeString = -11, - hwm_GPIOPinNumberInvalid = -12, - hwm_GPIOPinDirectionInvalid = -13, - hwm_IllegalAddress = -14, - hwm_IllegalSize = -15, - hwm_ParamsTableNotValid = -16, - hwm_ParamsTableIdNotValid = -17, - hwm_ParamsTableWrongExistingSize = -18, - hwm_WrongCRC = -19, - hwm_NotAuthorisedFlashWrite = -20, - hwm_NoDataToReturn = -21, - hwm_SpiReadFailed = -22, - hwm_SpiWriteFailed = -23, - hwm_SpiEraseSectorFailed = -24, - hwm_TableIsEmpty = -25, - hwm_I2cSeqDelay = -26, - hwm_CommandIsLocked = -27, - hwm_CalibrationWrongTableId = -28, - hwm_ValueOutOfRange = -29, - hwm_InvalidDepthFormat = -30, - hwm_DepthFlowError = -31, - hwm_Timeout = -32, - hwm_NotSafeCheckFailed = -33, - hwm_FlashRegionIsLocked = -34, - hwm_SummingEventTimeout = -35, - hwm_SDSCorrupted = -36, - hwm_SDSVerifyFailed = -37, - hwm_IllegalHwState = -38, - hwm_RealtekNotLoaded = -39, - hwm_WakeUpDeviceNotSupported = -40, - hwm_ResourceBusy = -41, - hwm_MaxErrorValue = -42, - hwm_PwmNotSupported = -43, - hwm_PwmStereoModuleNotConnected = -44, - hwm_UvcStreamInvalidStreamRequest = -45, - hwm_UvcControlManualExposureInvalid = -46, - hwm_UvcControlManualGainInvalid = -47, - hwm_EyesafetyPayloadFailure = -48, - hwm_ProjectorTestFailed = -49, - hwm_ThreadModifyFailed = -50, - hwm_HotLaserPwrReduce = -51, // reported to error depth XU control - hwm_HotLaserDisable = -52, // reported to error depth XU control - hwm_FlagBLaserDisable = -53, // reported to error depth XU control - hwm_NoStateChange = -54, - hwm_EEPROMIsLocked = -55, - hwm_OEMIdWrong = -56, - hwm_RealtekNotUpdated = -57, - hwm_FunctionNotSupported = -58, - hwm_IspNotImplemented = -59, - hwm_IspNotSupported = -60, - hwm_IspNotPermited = -61, - hwm_IspNotExists = -62, - hwm_IspFail = -63, - hwm_Unknown = -64, - hwm_LastError = hwm_Unknown - 1, - }; - - // Elaborate HW Monitor response - const std::map< hwmon_response, std::string> hwmon_response_report = { - { hwmon_response::hwm_Success, "Success" }, - { hwmon_response::hwm_WrongCommand, "Invalid Command" }, - { hwmon_response::hwm_StartNGEndAddr, "Start NG End Address" }, - { hwmon_response::hwm_AddressSpaceNotAligned, "Address space not aligned" }, - { hwmon_response::hwm_AddressSpaceTooSmall, "Address space too small" }, - { hwmon_response::hwm_ReadOnly, "Read-only" }, - { hwmon_response::hwm_WrongParameter, "Invalid parameter" }, - { hwmon_response::hwm_HWNotReady, "HW not ready" }, - { hwmon_response::hwm_I2CAccessFailed, "I2C access failed" }, - { hwmon_response::hwm_NoExpectedUserAction, "No expected user action" }, - { hwmon_response::hwm_IntegrityError, "Integrity error" }, - { hwmon_response::hwm_NullOrZeroSizeString, "Null or zero size string" }, - { hwmon_response::hwm_GPIOPinNumberInvalid, "GPIOP in number invalid" }, - { hwmon_response::hwm_GPIOPinDirectionInvalid, "GPIOP in direction invalid" }, - { hwmon_response::hwm_IllegalAddress, "Illegal address" }, - { hwmon_response::hwm_IllegalSize, "Illegal size" }, - { hwmon_response::hwm_ParamsTableNotValid, "Params table not valid" }, - { hwmon_response::hwm_ParamsTableIdNotValid, "Params table id not valid" }, - { hwmon_response::hwm_ParamsTableWrongExistingSize, "Params rable wrong existing size" }, - { hwmon_response::hwm_WrongCRC, "Invalid CRC" }, - { hwmon_response::hwm_NotAuthorisedFlashWrite, "Not authorised flash write" }, - { hwmon_response::hwm_NoDataToReturn, "No data to return" }, - { hwmon_response::hwm_SpiReadFailed, "Spi read failed" }, - { hwmon_response::hwm_SpiWriteFailed, "Spi write failed" }, - { hwmon_response::hwm_SpiEraseSectorFailed, "Spi erase sector failed" }, - { hwmon_response::hwm_TableIsEmpty, "Table is empty" }, - { hwmon_response::hwm_I2cSeqDelay, "I2c seq delay" }, - { hwmon_response::hwm_CommandIsLocked, "Command is locked" }, - { hwmon_response::hwm_CalibrationWrongTableId, "Calibration invalid table id" }, - { hwmon_response::hwm_ValueOutOfRange, "Value out of range" }, - { hwmon_response::hwm_InvalidDepthFormat, "Invalid depth format" }, - { hwmon_response::hwm_DepthFlowError, "Depth flow error" }, - { hwmon_response::hwm_Timeout, "Timeout" }, - { hwmon_response::hwm_NotSafeCheckFailed, "Not safe check failed" }, - { hwmon_response::hwm_FlashRegionIsLocked, "Flash region is locked" }, - { hwmon_response::hwm_SummingEventTimeout, "Summing event timeout" }, - { hwmon_response::hwm_SDSCorrupted, "SDS corrupted" }, - { hwmon_response::hwm_SDSVerifyFailed, "SDS verification failed" }, - { hwmon_response::hwm_IllegalHwState, "Illegal HW state" }, - { hwmon_response::hwm_RealtekNotLoaded, "Realtek not loaded" }, - { hwmon_response::hwm_WakeUpDeviceNotSupported, "Wake up device not supported" }, - { hwmon_response::hwm_ResourceBusy, "Resource busy" }, - { hwmon_response::hwm_MaxErrorValue, "Max error value" }, - { hwmon_response::hwm_PwmNotSupported, "Pwm not supported" }, - { hwmon_response::hwm_PwmStereoModuleNotConnected, "Pwm stereo module not connected" }, - { hwmon_response::hwm_UvcStreamInvalidStreamRequest,"Uvc stream invalid stream request" }, - { hwmon_response::hwm_UvcControlManualExposureInvalid,"Uvc control manual exposure invalid" }, - { hwmon_response::hwm_UvcControlManualGainInvalid, "Uvc control manual gain invalid" }, - { hwmon_response::hwm_EyesafetyPayloadFailure, "Eyesafety payload failure" }, - { hwmon_response::hwm_ProjectorTestFailed, "Projector test failed" }, - { hwmon_response::hwm_ThreadModifyFailed, "Thread modify failed" }, - { hwmon_response::hwm_HotLaserPwrReduce, "Hot laser pwr reduce" }, - { hwmon_response::hwm_HotLaserDisable, "Hot laser disable" }, - { hwmon_response::hwm_FlagBLaserDisable, "FlagB laser disable" }, - { hwmon_response::hwm_NoStateChange, "No state change" }, - { hwmon_response::hwm_EEPROMIsLocked, "EEPROM is locked" }, - { hwmon_response::hwm_EEPROMIsLocked, "EEPROM Is locked" }, - { hwmon_response::hwm_OEMIdWrong, "OEM invalid id" }, - { hwmon_response::hwm_RealtekNotUpdated, "Realtek not updated" }, - { hwmon_response::hwm_FunctionNotSupported, "Function not supported" }, - { hwmon_response::hwm_IspNotImplemented, "Isp not implemented" }, - { hwmon_response::hwm_IspNotSupported, "Isp not supported" }, - { hwmon_response::hwm_IspNotPermited, "Isp not permited" }, - { hwmon_response::hwm_IspNotExists, "Isp not present" }, - { hwmon_response::hwm_IspFail, "Isp fail" }, - { hwmon_response::hwm_Unknown, "Unresolved error" }, - { hwmon_response::hwm_LastError, "Last error" }, - }; - - inline std::string hwmon_error2str(hwmon_response e) { - if (hwmon_response_report.find(e) != hwmon_response_report.end()) - return hwmon_response_report.at(e); - return {}; - } - class locked_transfer { public: @@ -266,7 +119,12 @@ namespace librealsense } }; - std::string hwmon_error_string( command const &, hwmon_response e ); + typedef int32_t hwmon_response; + class base_hwmon_response_handler { // base class for different product number to implement responses + public: + inline virtual std::string hwmon_error2str(int e) const = 0; + virtual hwmon_response hwmon_Success() const = 0; + }; class hw_monitor { @@ -292,8 +150,8 @@ namespace librealsense static const size_t size_of_command_without_data = 24U; public: - explicit hw_monitor(std::shared_ptr locked_transfer) - : _locked_transfer(std::move(locked_transfer)) + explicit hw_monitor(std::shared_ptr locked_transfer, std::shared_ptr hwmon_response_handler) + : _locked_transfer(std::move(locked_transfer)), hwmon_response_handler(hwmon_response_handler) {} static void fill_usb_buffer( int opCodeNumber, @@ -373,5 +231,9 @@ namespace librealsense rv += data[index + i] << (i * 8); return rv; } + + std::shared_ptr hwmon_response_handler; + + std::string hwmon_error_string(command const&, int e) const; }; } From 0dd7aa961535d1ba7269696fadb7606c519b928d Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 25 Oct 2024 19:24:44 +0530 Subject: [PATCH 62/75] Hardcoding the AE control range of MIPI device --- src/linux/backend-v4l2.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index d4147a7d51..ad271ec3eb 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -2767,6 +2767,15 @@ namespace librealsense control_range v4l_mipi_device::get_pu_range(rs2_option option) const { + // Auto controls range is trimed to {0,1} range + if(option >= RS2_OPTION_ENABLE_AUTO_EXPOSURE && option <= RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE) + { + static const int32_t min = 0, max = 1, step = 1, def = 1; + control_range range(min, max, step, def); + + return range; + } + struct v4l2_query_ext_ctrl query = {}; query.id = get_cid(option); if (xioctl(_fd, VIDIOC_QUERY_EXT_CTRL, &query) < 0) From 71d9484c0f1737a88c05440c8494ff3e84838c15 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 25 Oct 2024 19:30:58 +0530 Subject: [PATCH 63/75] MIPI - Mapping AE attribute value in RGB metadata to 0 or 1 --- src/ds/d400/d400-color.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/ds/d400/d400-color.cpp b/src/ds/d400/d400-color.cpp index 14f3f090da..88a0290073 100644 --- a/src/ds/d400/d400-color.cpp +++ b/src/ds/d400/d400-color.cpp @@ -319,7 +319,8 @@ namespace librealsense color_ep.register_metadata(RS2_FRAME_METADATA_AUTO_EXPOSURE, make_attribute_parser(&md_mipi_rgb_mode::auto_exposure_mode, md_mipi_rgb_control_attributes::auto_exposure_mode_attribute, - md_prop_offset)); + md_prop_offset, + [](rs2_metadata_type param) { return (param != 1); })); // OFF value via UVC is 1 (ON is 8) color_ep.register_metadata(RS2_FRAME_METADATA_GAIN_LEVEL, make_attribute_parser(&md_mipi_rgb_mode::gain, md_mipi_rgb_control_attributes::gain_attribute, From e8fa64749a4fb1357a193a1d7722f47f35a2cceb Mon Sep 17 00:00:00 2001 From: ohadmeir Date: Mon, 21 Oct 2024 21:50:47 +0300 Subject: [PATCH 64/75] DDS pipeline streams from sensor before opening another --- src/pipeline/pipeline.cpp | 20 ++++++++++++++++++-- src/pipeline/resolver.h | 13 +++++++++++++ 2 files changed, 31 insertions(+), 2 deletions(-) diff --git a/src/pipeline/pipeline.cpp b/src/pipeline/pipeline.cpp index f5eed03229..cb7d6bc193 100644 --- a/src/pipeline/pipeline.cpp +++ b/src/pipeline/pipeline.cpp @@ -9,6 +9,9 @@ #include "media/ros/ros_writer.h" #include #include +#ifdef BUILD_WITH_DDS +#include +#endif #include @@ -127,8 +130,21 @@ namespace librealsense } _dispatcher.start(); - profile->_multistream.open(); - profile->_multistream.start(callbacks); +#ifdef BUILD_WITH_DDS + if( Is< librealsense::dds_device_proxy >( dev ) ) + { + // For DDS devices open() only sets requested profiles, start() actually starts streaming. + // Calling open() then start() sends open-streams control for all sensors before starting to stream, + // second open-streams control will resets the first (reverting first requested streams to defaults). + // open_and_start() starts streaming from the first sensor before the second open so it does not reset. + profile->_multistream.open_and_start( callbacks ); + } + else +#endif + { + profile->_multistream.open(); + profile->_multistream.start( callbacks ); + } _active_profile = profile; _prev_conf = std::make_shared(*conf); } diff --git a/src/pipeline/resolver.h b/src/pipeline/resolver.h index 9a3e753935..2d91facb2a 100644 --- a/src/pipeline/resolver.h +++ b/src/pipeline/resolver.h @@ -157,6 +157,19 @@ namespace librealsense sensor.second->start(callback); } + template< class T > + void open_and_start( T callback ) + { + // Calling open() than start() opens all relevant sensors before starting any of them. + // For some device backends we want to open and start each sensor before opening the next sensor. + for( auto it = _dev_to_profiles.rbegin(); it != _dev_to_profiles.rend(); it++ ) + { + auto && sub = _results.at( it->first ); + sub->open( it->second ); + sub->start( callback ); + } + } + void stop() { for (auto&& sensor : _results) From a64cc1f74af1fb599069d2f9dae361178737436b Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Sun, 27 Oct 2024 16:05:46 +0200 Subject: [PATCH 65/75] add helper function, unignore warnings on dds target --- CMake/external_fastdds.cmake | 5 ----- CMake/external_libcurl.cmake | 7 +++---- CMake/security_flags_helper_functions.cmake | 20 ++++++++++++++++++++ CMake/unix_config.cmake | 3 +-- CMake/windows_config.cmake | 3 +-- CMakeLists.txt | 9 +++++---- examples/CMakeLists.txt | 4 ++-- third-party/CMakeLists.txt | 8 +++----- tools/CMakeLists.txt | 4 ++-- 9 files changed, 37 insertions(+), 26 deletions(-) create mode 100644 CMake/security_flags_helper_functions.cmake diff --git a/CMake/external_fastdds.cmake b/CMake/external_fastdds.cmake index 00f8d7b8b9..856e0dbfc7 100644 --- a/CMake/external_fastdds.cmake +++ b/CMake/external_fastdds.cmake @@ -60,11 +60,6 @@ function(get_fastdds) add_library(dds INTERFACE) target_link_libraries( dds INTERFACE fastcdr fastrtps ) - if (MSVC) - target_compile_options( dds INTERFACE "/W0" ) - elseif (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") - target_compile_options( dds INTERFACE "-w" ) - endif() add_definitions(-DBUILD_WITH_DDS) diff --git a/CMake/external_libcurl.cmake b/CMake/external_libcurl.cmake index 36750d47fb..61f804814b 100644 --- a/CMake/external_libcurl.cmake +++ b/CMake/external_libcurl.cmake @@ -1,7 +1,7 @@ if(CHECK_FOR_UPDATES) - string(REPLACE "${SECURITY_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") # remove flags - string(REPLACE "${SECURITY_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") + pop_security_flags() # remove security flags + include(ExternalProject) message(STATUS "Building libcurl enabled") @@ -63,6 +63,5 @@ if(CHECK_FOR_UPDATES) endif() endif() - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${SECURITY_COMPILER_FLAGS}") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${SECURITY_COMPILER_FLAGS}") + push_security_flags() endif() #CHECK_FOR_UPDATES diff --git a/CMake/security_flags_helper_functions.cmake b/CMake/security_flags_helper_functions.cmake new file mode 100644 index 0000000000..5dc52e16e2 --- /dev/null +++ b/CMake/security_flags_helper_functions.cmake @@ -0,0 +1,20 @@ +macro(push_security_flags) # remove security flags + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${SECURITY_COMPILER_FLAGS}") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${SECURITY_COMPILER_FLAGS}") +endmacro() + +macro(pop_security_flags) # append security flags + string(REPLACE "${SECURITY_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") + string(REPLACE "${SECURITY_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") +endmacro() + +macro(set_security_flags_for_executable) # replace flag fPIC (Position-Independent Code) with fPIE (Position-Independent Executable) + string(REPLACE "-fPIC" "-fPIE" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") + string(REPLACE "-fPIC" "-fPIE" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") +endmacro() + +macro(unset_security_flags_for_executable) # replace flag fPIE (Position-Independent Executable) with fPIC (Position-Independent Code) + string(REPLACE "-fPIE" "-fPIC" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") + string(REPLACE "-fPIE" "-fPIC" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") +endmacro() + diff --git a/CMake/unix_config.cmake b/CMake/unix_config.cmake index bc88fcb408..1e22852d0f 100644 --- a/CMake/unix_config.cmake +++ b/CMake/unix_config.cmake @@ -84,8 +84,7 @@ macro(os_set_flags) set(SECURITY_COMPILER_FLAGS "${SECURITY_COMPILER_FLAGS} -Werror -z noexecstack -Wl,-z,relro,-z,now -fstack-protector-strong") endif() - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${SECURITY_COMPILER_FLAGS}") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${SECURITY_COMPILER_FLAGS}") + push_security_flags() set(CMAKE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS} -pie") diff --git a/CMake/windows_config.cmake b/CMake/windows_config.cmake index 4b94d06133..dd2a9dfe80 100644 --- a/CMake/windows_config.cmake +++ b/CMake/windows_config.cmake @@ -66,8 +66,7 @@ macro(os_set_flags) set(SECURITY_COMPILER_FLAGS "${SECURITY_COMPILER_FLAGS} /WX /sdl") endif() - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${SECURITY_COMPILER_FLAGS}") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${SECURITY_COMPILER_FLAGS}") + push_security_flags() if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") set(CMAKE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS} /INCREMENTAL:NO /LTCG /NXCOMPAT") # ignoring '/INCREMENTAL' due to '/LTCG' specification diff --git a/CMakeLists.txt b/CMakeLists.txt index 3f9fd95030..8c4f51966c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,6 +29,9 @@ endif() list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/CMake) +# include security flags helper functions +include(CMake/security_flags_helper_functions.cmake) + # include librealsense general configuration include(CMake/global_config.cmake) @@ -61,8 +64,7 @@ target_link_libraries( ${LRS_TARGET} PUBLIC rsutils ) if(BUILD_WITH_DDS) if (CMAKE_SYSTEM MATCHES "Windows" OR CMAKE_SYSTEM MATCHES "Linux") - string(REPLACE "${SECURITY_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") # remove security flags - string(REPLACE "${SECURITY_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") + pop_security_flags() message(STATUS "Building with FastDDS") include(CMake/external_foonathan_memory.cmake) @@ -70,8 +72,7 @@ if(BUILD_WITH_DDS) target_link_libraries( ${LRS_TARGET} PRIVATE realdds ) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${SECURITY_COMPILER_FLAGS}") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${SECURITY_COMPILER_FLAGS}") + push_security_flags() else() MESSAGE(STATUS "Turning off `BUILD_WITH_DDS` as it's only supported on Windows & Linux and not on ${CMAKE_SYSTEM}") diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index aaf3c1d369..b43469404d 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -7,7 +7,7 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS 1) # View the makefile commands during build #set(CMAKE_VERBOSE_MAKEFILE on) -string(REPLACE "-fPIC" "-fPIE" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") # examples are executables so we want position indepandent executables and not libraries +set_security_flags_for_executable() # examples are executables so we want position indepandent executables and not libraries set( DEPENDENCIES ${LRS_TARGET} ) if(BUILD_GRAPHICAL_EXAMPLES) @@ -43,4 +43,4 @@ add_subdirectory(motion) add_subdirectory(gl) add_subdirectory(hdr) -string(REPLACE "-fPIE" "-fPIC" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") +unset_security_flags_for_executable() diff --git a/third-party/CMakeLists.txt b/third-party/CMakeLists.txt index 47164ed9a4..708ffc8439 100644 --- a/third-party/CMakeLists.txt +++ b/third-party/CMakeLists.txt @@ -3,8 +3,7 @@ string(REPLACE ${PROJECT_SOURCE_DIR}/ "" _rel_path ${CMAKE_CURRENT_LIST_DIR}) add_subdirectory( "${CMAKE_CURRENT_LIST_DIR}/rsutils" ) -string(REPLACE "${SECURITY_COMPILER_FLAGS}" "" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") # remove security flags -string(REPLACE "${SECURITY_COMPILER_FLAGS}" "" CMAKE_C_FLAGS "${CMAKE_C_FLAGS}") +pop_security_flags() include(CMake/external_json.cmake) # Add additional include directories to allow file to include rosbag headers @@ -21,6 +20,5 @@ if( BUILD_WITH_DDS ) add_subdirectory( "${CMAKE_CURRENT_LIST_DIR}/realdds" ) endif() -# restore flags -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${SECURITY_COMPILER_FLAGS}") -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${SECURITY_COMPILER_FLAGS}") +# restore security flags +push_security_flags() diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index d758cd01c4..7098757b98 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -7,7 +7,7 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS 1) # View the makefile commands during build #set(CMAKE_VERBOSE_MAKEFILE on) -string(REPLACE "-fPIC" "-fPIE" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") # tools are executables so we want position indepandent executables and not libraries +set_security_flags_for_executable() # tools are executables so we want position indepandent executables and not libraries list( APPEND DEPENDENCIES ${LRS_TARGET} tclap ) @@ -48,4 +48,4 @@ if(BUILD_EXAMPLES) endif() endif() -string(REPLACE "-fPIE" "-fPIC" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") +unset_security_flags_for_executable() From 303c5af955dadbf0f22d87d12283185081e38a18 Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Tue, 29 Oct 2024 10:32:03 +0200 Subject: [PATCH 66/75] renaming types and small fixes --- src/ds/d400/d400-device.cpp | 8 +- src/ds/d400/d400-device.h | 1 - src/ds/d400/d400-private.h | 275 ++++++++++---------- src/ds/d500/d500-device.cpp | 4 +- src/ds/d500/d500-device.h | 2 - src/ds/d500/d500-private.h | 174 ++++++------- src/ds/d500/hw_monitor_extended_buffers.cpp | 6 +- src/ds/d500/hw_monitor_extended_buffers.h | 10 +- src/ds/ds-options.cpp | 8 +- src/ds/ds-options.h | 4 +- src/hdr-config.cpp | 9 +- src/hdr-config.h | 3 +- src/hw-monitor.cpp | 10 +- src/hw-monitor.h | 18 +- 14 files changed, 265 insertions(+), 267 deletions(-) diff --git a/src/ds/d400/d400-device.cpp b/src/ds/d400/d400-device.cpp index be105ee927..fc10139a4e 100644 --- a/src/ds/d400/d400-device.cpp +++ b/src/ds/d400/d400-device.cpp @@ -317,7 +317,7 @@ namespace librealsense void init_hdr_config(const option_range& exposure_range, const option_range& gain_range) { _hdr_cfg = std::make_shared(*(_owner->_hw_monitor), get_raw_sensor(), - exposure_range, gain_range); + exposure_range, gain_range, ds::d400_hwmon_response::opcodes::NO_DATA_TO_RETURN); } std::shared_ptr get_hdr_config() { return _hdr_cfg; } @@ -552,7 +552,7 @@ namespace librealsense _hw_monitor = std::make_shared( std::make_shared( std::make_shared( *raw_sensor, depth_xu, DS5_HWMONITOR ), - raw_sensor ), hw_monitor_response); + raw_sensor ), std::make_shared()); } else { @@ -560,7 +560,7 @@ namespace librealsense _hw_monitor = std::make_shared< hw_monitor >( std::make_shared< locked_transfer >( get_backend()->create_usb_device( group.usb_devices.front() ), - raw_sensor ), hw_monitor_response); + raw_sensor ), std::make_shared()); } set_hw_monitor_for_auto_calib(_hw_monitor); @@ -841,7 +841,7 @@ namespace librealsense if ((_fw_version >= firmware_version("5.11.3.0")) && ((_device_capabilities & mask) == mask)) { bool is_fw_version_using_id = (_fw_version >= firmware_version("5.12.8.100")); - auto alternating_emitter_opt = std::make_shared(*_hw_monitor, is_fw_version_using_id, ds::d400_hwmon_response_handler::d400_hwmon_response_names::hwm_NoDataToReturn); + auto alternating_emitter_opt = std::make_shared(*_hw_monitor, is_fw_version_using_id, ds::d400_hwmon_response::opcodes::NO_DATA_TO_RETURN); auto emitter_always_on_opt = std::make_shared( _hw_monitor, ds::LASERONCONST, ds::LASERONCONST ); if ((_fw_version >= firmware_version("5.12.1.0")) && ((_device_capabilities & ds_caps::CAP_GLOBAL_SHUTTER) == ds_caps::CAP_GLOBAL_SHUTTER)) diff --git a/src/ds/d400/d400-device.h b/src/ds/d400/d400-device.h index 0b773bc5f8..0816b54d03 100644 --- a/src/ds/d400/d400-device.h +++ b/src/ds/d400/d400-device.h @@ -94,7 +94,6 @@ namespace librealsense friend class d400_depth_sensor; std::shared_ptr _hw_monitor; - std::shared_ptr hw_monitor_response = std::make_shared(); firmware_version _fw_version; firmware_version _recommended_fw_version; ds::ds_caps _device_capabilities; diff --git a/src/ds/d400/d400-private.h b/src/ds/d400/d400-private.h index 73022bb470..c7ea54c5f6 100644 --- a/src/ds/d400/d400-private.h +++ b/src/ds/d400/d400-private.h @@ -341,157 +341,156 @@ namespace librealsense { isp_boot_data_upload_failed, "ISP boot data upload failure" }, }; - class d400_hwmon_response_handler : public base_hwmon_response_handler + class d400_hwmon_response : public hwmon_response_interface { public: - enum d400_hwmon_response_names : int32_t + enum opcodes : hwmon_response_type { - hwm_Success = 0, - hwm_WrongCommand = -1, - hwm_StartNGEndAddr = -2, - hwm_AddressSpaceNotAligned = -3, - hwm_AddressSpaceTooSmall = -4, - hwm_ReadOnly = -5, - hwm_WrongParameter = -6, - hwm_HWNotReady = -7, - hwm_I2CAccessFailed = -8, - hwm_NoExpectedUserAction = -9, - hwm_IntegrityError = -10, - hwm_NullOrZeroSizeString = -11, - hwm_GPIOPinNumberInvalid = -12, - hwm_GPIOPinDirectionInvalid = -13, - hwm_IllegalAddress = -14, - hwm_IllegalSize = -15, - hwm_ParamsTableNotValid = -16, - hwm_ParamsTableIdNotValid = -17, - hwm_ParamsTableWrongExistingSize = -18, - hwm_WrongCRC = -19, - hwm_NotAuthorisedFlashWrite = -20, - hwm_NoDataToReturn = -21, - hwm_SpiReadFailed = -22, - hwm_SpiWriteFailed = -23, - hwm_SpiEraseSectorFailed = -24, - hwm_TableIsEmpty = -25, - hwm_I2cSeqDelay = -26, - hwm_CommandIsLocked = -27, - hwm_CalibrationWrongTableId = -28, - hwm_ValueOutOfRange = -29, - hwm_InvalidDepthFormat = -30, - hwm_DepthFlowError = -31, - hwm_Timeout = -32, - hwm_NotSafeCheckFailed = -33, - hwm_FlashRegionIsLocked = -34, - hwm_SummingEventTimeout = -35, - hwm_SDSCorrupted = -36, - hwm_SDSVerifyFailed = -37, - hwm_IllegalHwState = -38, - hwm_RealtekNotLoaded = -39, - hwm_WakeUpDeviceNotSupported = -40, - hwm_ResourceBusy = -41, - hwm_MaxErrorValue = -42, - hwm_PwmNotSupported = -43, - hwm_PwmStereoModuleNotConnected = -44, - hwm_UvcStreamInvalidStreamRequest = -45, - hwm_UvcControlManualExposureInvalid = -46, - hwm_UvcControlManualGainInvalid = -47, - hwm_EyesafetyPayloadFailure = -48, - hwm_ProjectorTestFailed = -49, - hwm_ThreadModifyFailed = -50, - hwm_HotLaserPwrReduce = -51, // reported to error depth XU control - hwm_HotLaserDisable = -52, // reported to error depth XU control - hwm_FlagBLaserDisable = -53, // reported to error depth XU control - hwm_NoStateChange = -54, - hwm_EEPROMIsLocked = -55, - hwm_OEMIdWrong = -56, - hwm_RealtekNotUpdated = -57, - hwm_FunctionNotSupported = -58, - hwm_IspNotImplemented = -59, - hwm_IspNotSupported = -60, - hwm_IspNotPermited = -61, - hwm_IspNotExists = -62, - hwm_IspFail = -63, - hwm_Unknown = -64, - hwm_LastError = hwm_Unknown - 1, + SUCCESS = 0, + WRONG_COMMAND = -1, + START_NG_END_ADDR = -2, + ADDRESS_SPACE_NOT_ALIGNED = -3, + ADDRESS_SPACE_TOO_SMALL = -4, + READ_ONLY = -5, + WRONG_PARAMETER = -6, + HW_NOT_READY = -7, + I2C_ACCESS_FAILED = -8, + NO_EXPECTED_USER_ACTION = -9, + INTEGRITY_ERROR = -10, + NULL_OR_ZERO_SIZE_STRING = -11, + GPIO_PIN_NUMBER_INVALID = -12, + GPIO_PIN_DIRECTION_INVALID = -13, + ILLEGAL_ADDRESS = -14, + ILLEGAL_SIZE = -15, + PARAMS_TABLE_NOT_VALID = -16, + PARAMS_TABLE_ID_NOT_VALID = -17, + PARAMS_TABLE_WRONG_EXISTING_SIZE = -18, + WRONG_CRC = -19, + NOT_AUTHORISED_FLASH_WRITE = -20, + NO_DATA_TO_RETURN = -21, + SPI_READ_FAILED = -22, + SPI_WRITE_FAILED = -23, + SPI_ERASE_SECTOR_FAILED = -24, + TABLE_IS_EMPTY = -25, + I2C_SEQ_DELAY = -26, + COMMAND_IS_LOCKED = -27, + CALIBRATION_WRONG_TABLE_ID = -28, + VALUE_OUT_OF_RANGE = -29, + INVALID_DEPTH_FORMAT = -30, + DEPTH_FLOW_ERROR = -31, + TIMEOUT = -32, + NOT_SAFE_CHECK_FAILED = -33, + FLASH_REGION_IS_LOCKED = -34, + SUMMING_EVENT_TIMEOUT = -35, + SDS_CORRUPTED = -36, + SDS_VERIFY_FAILED = -37, + ILLEGAL_HW_STATE = -38, + REALTEK_NOT_LOADED = -39, + WAKE_UP_DEVICE_NOT_SUPPORTED = -40, + RESOURCE_BUSY = -41, + MAX_ERROR_VALUE = -42, + PWM_NOT_SUPPORTED = -43, + PWM_STEREO_MODULE_NOT_CONNECTED = -44, + UVC_STREAM_INVALID_STREAM_REQUEST = -45, + UVC_CONTROL_MANUAL_EXPOSURE_INVALID = -46, + UVC_CONTROL_MANUAL_GAIN_INVALID = -47, + EYE_SAFETY_PAYLOAD_FAILURE = -48, + PROJECTOR_TEST_FAILED = -49, + THREAD_MODIFY_FAILED = -50, + HOT_LASER_PWR_REDUCE = -51, // reported to error depth XU control + HOT_LASER_DISABLE = -52, // reported to error depth XU control + FLAG_B_LASER_DISABLE = -53, // reported to error depth XU control + NO_STATE_CHANGE = -54, + EEPROM_IS_LOCKED = -55, + OEM_ID_WRONG = -56, + REALTEK_NOT_UPDATED = -57, + FUNCTION_NOT_SUPPORTED = -58, + ISP_NOT_IMPLEMENTED = -59, + ISP_NOT_SUPPORTED = -60, + ISP_NOT_PERMITTED = -61, + ISP_NOT_EXISTS = -62, + ISP_FAIL = -63, + UNKNOWN = -64, + LAST_ERROR = UNKNOWN - 1, }; // Elaborate HW Monitor response - std::map< hwmon_response, std::string> hwmon_response_report = { - { d400_hwmon_response_names::hwm_Success, "Success" }, - { d400_hwmon_response_names::hwm_WrongCommand, "Invalid Command" }, - { d400_hwmon_response_names::hwm_StartNGEndAddr, "Start NG End Address" }, - { d400_hwmon_response_names::hwm_AddressSpaceNotAligned, "Address space not aligned" }, - { d400_hwmon_response_names::hwm_AddressSpaceTooSmall, "Address space too small" }, - { d400_hwmon_response_names::hwm_ReadOnly, "Read-only" }, - { d400_hwmon_response_names::hwm_WrongParameter, "Invalid parameter" }, - { d400_hwmon_response_names::hwm_HWNotReady, "HW not ready" }, - { d400_hwmon_response_names::hwm_I2CAccessFailed, "I2C access failed" }, - { d400_hwmon_response_names::hwm_NoExpectedUserAction, "No expected user action" }, - { d400_hwmon_response_names::hwm_IntegrityError, "Integrity error" }, - { d400_hwmon_response_names::hwm_NullOrZeroSizeString, "Null or zero size string" }, - { d400_hwmon_response_names::hwm_GPIOPinNumberInvalid, "GPIOP in number invalid" }, - { d400_hwmon_response_names::hwm_GPIOPinDirectionInvalid, "GPIOP in direction invalid" }, - { d400_hwmon_response_names::hwm_IllegalAddress, "Illegal address" }, - { d400_hwmon_response_names::hwm_IllegalSize, "Illegal size" }, - { d400_hwmon_response_names::hwm_ParamsTableNotValid, "Params table not valid" }, - { d400_hwmon_response_names::hwm_ParamsTableIdNotValid, "Params table id not valid" }, - { d400_hwmon_response_names::hwm_ParamsTableWrongExistingSize, "Params rable wrong existing size" }, - { d400_hwmon_response_names::hwm_WrongCRC, "Invalid CRC" }, - { d400_hwmon_response_names::hwm_NotAuthorisedFlashWrite, "Not authorised flash write" }, - { d400_hwmon_response_names::hwm_NoDataToReturn, "No data to return" }, - { d400_hwmon_response_names::hwm_SpiReadFailed, "Spi read failed" }, - { d400_hwmon_response_names::hwm_SpiWriteFailed, "Spi write failed" }, - { d400_hwmon_response_names::hwm_SpiEraseSectorFailed, "Spi erase sector failed" }, - { d400_hwmon_response_names::hwm_TableIsEmpty, "Table is empty" }, - { d400_hwmon_response_names::hwm_I2cSeqDelay, "I2c seq delay" }, - { d400_hwmon_response_names::hwm_CommandIsLocked, "Command is locked" }, - { d400_hwmon_response_names::hwm_CalibrationWrongTableId, "Calibration invalid table id" }, - { d400_hwmon_response_names::hwm_ValueOutOfRange, "Value out of range" }, - { d400_hwmon_response_names::hwm_InvalidDepthFormat, "Invalid depth format" }, - { d400_hwmon_response_names::hwm_DepthFlowError, "Depth flow error" }, - { d400_hwmon_response_names::hwm_Timeout, "Timeout" }, - { d400_hwmon_response_names::hwm_NotSafeCheckFailed, "Not safe check failed" }, - { d400_hwmon_response_names::hwm_FlashRegionIsLocked, "Flash region is locked" }, - { d400_hwmon_response_names::hwm_SummingEventTimeout, "Summing event timeout" }, - { d400_hwmon_response_names::hwm_SDSCorrupted, "SDS corrupted" }, - { d400_hwmon_response_names::hwm_SDSVerifyFailed, "SDS verification failed" }, - { d400_hwmon_response_names::hwm_IllegalHwState, "Illegal HW state" }, - { d400_hwmon_response_names::hwm_RealtekNotLoaded, "Realtek not loaded" }, - { d400_hwmon_response_names::hwm_WakeUpDeviceNotSupported, "Wake up device not supported" }, - { d400_hwmon_response_names::hwm_ResourceBusy, "Resource busy" }, - { d400_hwmon_response_names::hwm_MaxErrorValue, "Max error value" }, - { d400_hwmon_response_names::hwm_PwmNotSupported, "Pwm not supported" }, - { d400_hwmon_response_names::hwm_PwmStereoModuleNotConnected, "Pwm stereo module not connected" }, - { d400_hwmon_response_names::hwm_UvcStreamInvalidStreamRequest,"Uvc stream invalid stream request" }, - { d400_hwmon_response_names::hwm_UvcControlManualExposureInvalid,"Uvc control manual exposure invalid" }, - { d400_hwmon_response_names::hwm_UvcControlManualGainInvalid, "Uvc control manual gain invalid" }, - { d400_hwmon_response_names::hwm_EyesafetyPayloadFailure, "Eyesafety payload failure" }, - { d400_hwmon_response_names::hwm_ProjectorTestFailed, "Projector test failed" }, - { d400_hwmon_response_names::hwm_ThreadModifyFailed, "Thread modify failed" }, - { d400_hwmon_response_names::hwm_HotLaserPwrReduce, "Hot laser pwr reduce" }, - { d400_hwmon_response_names::hwm_HotLaserDisable, "Hot laser disable" }, - { d400_hwmon_response_names::hwm_FlagBLaserDisable, "FlagB laser disable" }, - { d400_hwmon_response_names::hwm_NoStateChange, "No state change" }, - { d400_hwmon_response_names::hwm_EEPROMIsLocked, "EEPROM is locked" }, - { d400_hwmon_response_names::hwm_EEPROMIsLocked, "EEPROM Is locked" }, - { d400_hwmon_response_names::hwm_OEMIdWrong, "OEM invalid id" }, - { d400_hwmon_response_names::hwm_RealtekNotUpdated, "Realtek not updated" }, - { d400_hwmon_response_names::hwm_FunctionNotSupported, "Function not supported" }, - { d400_hwmon_response_names::hwm_IspNotImplemented, "Isp not implemented" }, - { d400_hwmon_response_names::hwm_IspNotSupported, "Isp not supported" }, - { d400_hwmon_response_names::hwm_IspNotPermited, "Isp not permited" }, - { d400_hwmon_response_names::hwm_IspNotExists, "Isp not present" }, - { d400_hwmon_response_names::hwm_IspFail, "Isp fail" }, - { d400_hwmon_response_names::hwm_Unknown, "Unresolved error" }, - { d400_hwmon_response_names::hwm_LastError, "Last error" }, + std::map hwmon_response_report = { + { SUCCESS, "Success" }, + { WRONG_COMMAND, "Invalid Command" }, + { START_NG_END_ADDR, "Start NG End Address" }, + { ADDRESS_SPACE_NOT_ALIGNED, "Address space not aligned" }, + { ADDRESS_SPACE_TOO_SMALL, "Address space too small" }, + { READ_ONLY, "Read-only" }, + { WRONG_PARAMETER, "Invalid parameter" }, + { HW_NOT_READY, "HW not ready" }, + { I2C_ACCESS_FAILED, "I2C access failed" }, + { NO_EXPECTED_USER_ACTION, "No expected user action" }, + { INTEGRITY_ERROR, "Integrity error" }, + { NULL_OR_ZERO_SIZE_STRING, "Null or zero size string" }, + { GPIO_PIN_NUMBER_INVALID, "GPIO pin number invalid" }, + { GPIO_PIN_DIRECTION_INVALID, "GPIO pin direction invalid" }, + { ILLEGAL_ADDRESS, "Illegal address" }, + { ILLEGAL_SIZE, "Illegal size" }, + { PARAMS_TABLE_NOT_VALID, "Params table not valid" }, + { PARAMS_TABLE_ID_NOT_VALID, "Params table id not valid" }, + { PARAMS_TABLE_WRONG_EXISTING_SIZE, "Params table wrong existing size" }, + { WRONG_CRC, "Invalid CRC" }, + { NOT_AUTHORISED_FLASH_WRITE, "Not authorised flash write" }, + { NO_DATA_TO_RETURN, "No data to return" }, + { SPI_READ_FAILED, "SPI read failed" }, + { SPI_WRITE_FAILED, "SPI write failed" }, + { SPI_ERASE_SECTOR_FAILED, "SPI erase sector failed" }, + { TABLE_IS_EMPTY, "Table is empty" }, + { I2C_SEQ_DELAY, "I2C seq delay" }, + { COMMAND_IS_LOCKED, "Command is locked" }, + { CALIBRATION_WRONG_TABLE_ID, "Calibration invalid table id" }, + { VALUE_OUT_OF_RANGE, "Value out of range" }, + { INVALID_DEPTH_FORMAT, "Invalid depth format" }, + { DEPTH_FLOW_ERROR, "Depth flow error" }, + { TIMEOUT, "Timeout" }, + { NOT_SAFE_CHECK_FAILED, "Not safe check failed" }, + { FLASH_REGION_IS_LOCKED, "Flash region is locked" }, + { SUMMING_EVENT_TIMEOUT, "Summing event timeout" }, + { SDS_CORRUPTED, "SDS corrupted" }, + { SDS_VERIFY_FAILED, "SDS verification failed" }, + { ILLEGAL_HW_STATE, "Illegal HW state" }, + { REALTEK_NOT_LOADED, "Realtek not loaded" }, + { WAKE_UP_DEVICE_NOT_SUPPORTED, "Wake up device not supported" }, + { RESOURCE_BUSY, "Resource busy" }, + { MAX_ERROR_VALUE, "Max error value" }, + { PWM_NOT_SUPPORTED, "PWM not supported" }, + { PWM_STEREO_MODULE_NOT_CONNECTED, "PWM stereo module not connected" }, + { UVC_STREAM_INVALID_STREAM_REQUEST, "UVC stream invalid stream request" }, + { UVC_CONTROL_MANUAL_EXPOSURE_INVALID, "UVC control manual exposure invalid" }, + { UVC_CONTROL_MANUAL_GAIN_INVALID, "UVC control manual gain invalid" }, + { EYE_SAFETY_PAYLOAD_FAILURE, "Eye safety payload failure" }, + { PROJECTOR_TEST_FAILED, "Projector test failed" }, + { THREAD_MODIFY_FAILED, "Thread modify failed" }, + { HOT_LASER_PWR_REDUCE, "Hot laser power reduce" }, + { HOT_LASER_DISABLE, "Hot laser disable" }, + { FLAG_B_LASER_DISABLE, "Flag B laser disable" }, + { NO_STATE_CHANGE, "No state change" }, + { EEPROM_IS_LOCKED, "EEPROM is locked" }, + { OEM_ID_WRONG, "OEM ID wrong" }, + { REALTEK_NOT_UPDATED, "Realtek not updated" }, + { FUNCTION_NOT_SUPPORTED, "Function not supported" }, + { ISP_NOT_IMPLEMENTED, "ISP not implemented" }, + { ISP_NOT_SUPPORTED, "ISP not supported" }, + { ISP_NOT_PERMITTED, "ISP not permitted" }, + { ISP_NOT_EXISTS, "ISP not exists" }, + { ISP_FAIL, "ISP fail" }, + { UNKNOWN, "Unresolved error" }, + { LAST_ERROR, "Last error" }, }; - inline virtual std::string hwmon_error2str(hwmon_response opcode) const override { + virtual std::string hwmon_error2str(hwmon_response_type opcode) const override { if (hwmon_response_report.find(opcode) != hwmon_response_report.end()) return hwmon_response_report.at(opcode); return {}; } - virtual hwmon_response hwmon_Success() const override { return d400_hwmon_response_names::hwm_Success; }; + virtual hwmon_response_type success_value() const override { return SUCCESS; }; }; diff --git a/src/ds/d500/d500-device.cpp b/src/ds/d500/d500-device.cpp index 208a18c729..b88393d107 100644 --- a/src/ds/d500/d500-device.cpp +++ b/src/ds/d500/d500-device.cpp @@ -407,13 +407,13 @@ namespace librealsense _hw_monitor = std::make_shared( std::make_shared( std::make_shared( *raw_sensor, depth_xu, DS5_HWMONITOR ), - raw_sensor), hw_monitor_response); + raw_sensor), std::make_shared()); } 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 ), hw_monitor_response); + raw_sensor ), std::make_shared()); } _ds_device_common = std::make_shared(this, _hw_monitor); diff --git a/src/ds/d500/d500-device.h b/src/ds/d500/d500-device.h index 2a95cc4079..a0c14ea43c 100644 --- a/src/ds/d500/d500-device.h +++ b/src/ds/d500/d500-device.h @@ -51,8 +51,6 @@ namespace librealsense d500_device( std::shared_ptr< const d500_info > const & ); - std::shared_ptr hw_monitor_response = std::make_shared(); - std::vector send_receive_raw_data(const std::vector& input) override; std::vector build_command(uint32_t opcode, diff --git a/src/ds/d500/d500-private.h b/src/ds/d500/d500-private.h index 080f4a3de8..092ed670e1 100644 --- a/src/ds/d500/d500-private.h +++ b/src/ds/d500/d500-private.h @@ -454,106 +454,106 @@ namespace librealsense { EHU_IDX_DSP_UP_CHECKSUM_ERR, "DSP UP CHECKSUM ERROR" }, }; - class d500_hwmon_response_handler : public base_hwmon_response_handler + class d500_hwmon_response : public hwmon_response_interface { public: - enum d500_hwmon_response_names : int32_t + enum opcodes : hwmon_response_type { - hwm_Success = 0, - hwm_InvalidCommand = -1, - hwm_InvalidParam = -2, - hwm_HWNotReady = -3, // (different from #21) - hwm_UnauthorizedUserAction = -4, - hwm_IntegrityError = -5, - hwm_CRC_Error = -6, - hwm_GPIOPinNumberInvalid = -7, - hwm_GPIOPinDirectionInvalid = -8, - hwm_IllegalAddress = -9, - hwm_IllegalSize = -10, - hwm_ParamsTableNotValid = -11, - hwm_ParamsTableIdNotValid = -12, - hwm_ParamsTableWrongExistingSize = -13, - hwm_SpiReadFailed = -14, - hwm_SpiWriteFailed = -15, - hwm_TableIsEmpty = -16, - hwm_ValueOutOfRange = -17, - hwm_Operation_Timeout = -18, - hwm_CommandNotSupported = -19, // (inappropriate FW/SKU) - hwm_IncompleteData = -20, - hwm_SWNotReady = -21, // (mind the difference from #3) - hwm_Reserved_22 = -22, - hwm_Reserved_23 = -23, - hwm_Reserved_24 = -24, - hwm_Reserved_25 = -25, - hwm_Reserved_26 = -26, - hwm_Reserved_27 = -27, - hwm_Reserved_28 = -28, - hwm_Reserved_29 = -29, - hwm_Reserved_30 = -30, - hwm_Reserved_31 = -31, - hwm_Reserved_32 = -32, - hwm_Reserved_33 = -33, - hwm_Reserved_34 = -34, - hwm_Reserved_35 = -35, - hwm_Reserved_36 = -36, - hwm_Reserved_37 = -37, - hwm_Reserved_38 = -38, - hwm_Reserved_39 = -39, - hwm_LastError = hwm_Reserved_39 - 1, // if more error codes are added, this value should be updated + SUCCESS = 0, + INVALID_COMMAND = -1, + INVALID_PARAM = -2, + HW_NOT_READY = -3, // (different from #21) + UNAUTHORIZED_USER_ACTION = -4, + INTEGRITY_ERROR = -5, + CRC_ERROR = -6, + GPIO_PIN_NUMBER_INVALID = -7, + GPIO_PIN_DIRECTION_INVALID = -8, + ILLEGAL_ADDRESS = -9, + ILLEGAL_SIZE = -10, + PARAMS_TABLE_NOT_VALID = -11, + PARAMS_TABLE_ID_NOT_VALID = -12, + PARAMS_TABLE_WRONG_EXISTING_SIZE = -13, + SPI_READ_FAILED = -14, + SPI_WRITE_FAILED = -15, + TABLE_IS_EMPTY = -16, + VALUE_OUT_OF_RANGE = -17, + OPERATION_TIMEOUT = -18, + COMMAND_NOT_SUPPORTED = -19, // (inappropriate FW/SKU) + INCOMPLETE_DATA = -20, + SW_NOT_READY = -21, // (mind the difference from #3) + RESERVED_22 = -22, + RESERVED_23 = -23, + RESERVED_24 = -24, + RESERVED_25 = -25, + RESERVED_26 = -26, + RESERVED_27 = -27, + RESERVED_28 = -28, + RESERVED_29 = -29, + RESERVED_30 = -30, + RESERVED_31 = -31, + RESERVED_32 = -32, + RESERVED_33 = -33, + RESERVED_34 = -34, + RESERVED_35 = -35, + RESERVED_36 = -36, + RESERVED_37 = -37, + RESERVED_38 = -38, + RESERVED_39 = -39, + LAST_ERROR = RESERVED_39 - 1, // if more error codes are added, this value should be updated }; // Elaborate HW Monitor response - const std::map< hwmon_response, std::string> hwmon_response_report = { - { d500_hwmon_response_names::hwm_Success, "Success" }, - { d500_hwmon_response_names::hwm_InvalidCommand, "Invalid Command" }, - { d500_hwmon_response_names::hwm_InvalidParam, "Invalid Param" }, - { d500_hwmon_response_names::hwm_HWNotReady, "HW Not Ready" }, - { d500_hwmon_response_names::hwm_UnauthorizedUserAction, "Unauthorized User Action" }, - { d500_hwmon_response_names::hwm_IntegrityError, "Integrity Error" }, - { d500_hwmon_response_names::hwm_CRC_Error, "CRC Error" }, - { d500_hwmon_response_names::hwm_GPIOPinNumberInvalid, "GPIO Pin Number Invalid" }, - { d500_hwmon_response_names::hwm_GPIOPinDirectionInvalid, "GPIO Pin Direction Invalid" }, - { d500_hwmon_response_names::hwm_IllegalAddress, "Illegal Address" }, - { d500_hwmon_response_names::hwm_IllegalSize, "Illegal Size" }, - { d500_hwmon_response_names::hwm_ParamsTableNotValid, "Params Table Not Valid" }, - { d500_hwmon_response_names::hwm_ParamsTableIdNotValid, "Params Table Id Not Valid" }, - { d500_hwmon_response_names::hwm_ParamsTableWrongExistingSize, "Params Table Wrong Existing Size" }, - { d500_hwmon_response_names::hwm_SpiReadFailed, "Spi Read Failed" }, - { d500_hwmon_response_names::hwm_SpiWriteFailed, "Spi Write Failed" }, - { d500_hwmon_response_names::hwm_TableIsEmpty, "Table Is Empty" }, - { d500_hwmon_response_names::hwm_ValueOutOfRange, "Value Out Of Range" }, - { d500_hwmon_response_names::hwm_Operation_Timeout, "Operation Timeout" }, - { d500_hwmon_response_names::hwm_CommandNotSupported, "Command Not Supported" }, - { d500_hwmon_response_names::hwm_IncompleteData, "Incomplete Data" }, - { d500_hwmon_response_names::hwm_SWNotReady, "SW Not Ready" }, - { d500_hwmon_response_names::hwm_Reserved_22, "Reserved 22" }, - { d500_hwmon_response_names::hwm_Reserved_23, "Reserved 23" }, - { d500_hwmon_response_names::hwm_Reserved_24, "Reserved 24" }, - { d500_hwmon_response_names::hwm_Reserved_25, "Reserved 25" }, - { d500_hwmon_response_names::hwm_Reserved_26, "Reserved 26" }, - { d500_hwmon_response_names::hwm_Reserved_27, "Reserved 27" }, - { d500_hwmon_response_names::hwm_Reserved_28, "Reserved 28" }, - { d500_hwmon_response_names::hwm_Reserved_29, "Reserved 29" }, - { d500_hwmon_response_names::hwm_Reserved_30, "Reserved 30" }, - { d500_hwmon_response_names::hwm_Reserved_31, "Reserved 31" }, - { d500_hwmon_response_names::hwm_Reserved_32, "Reserved 32" }, - { d500_hwmon_response_names::hwm_Reserved_33, "Reserved 33" }, - { d500_hwmon_response_names::hwm_Reserved_34, "Reserved 34" }, - { d500_hwmon_response_names::hwm_Reserved_35, "Reserved 35" }, - { d500_hwmon_response_names::hwm_Reserved_36, "Reserved 36" }, - { d500_hwmon_response_names::hwm_Reserved_37, "Reserved 37" }, - { d500_hwmon_response_names::hwm_Reserved_38, "Reserved 38" }, - { d500_hwmon_response_names::hwm_Reserved_39, "Reserved 39" }, - { d500_hwmon_response_names::hwm_LastError, "Last Error"} + const std::map hwmon_response_report = { + { SUCCESS, "Success" }, + { INVALID_COMMAND, "Invalid Command" }, + { INVALID_PARAM, "Invalid Param" }, + { HW_NOT_READY, "HW Not Ready" }, + { UNAUTHORIZED_USER_ACTION, "Unauthorized User Action" }, + { INTEGRITY_ERROR, "Integrity Error" }, + { CRC_ERROR, "CRC Error" }, + { GPIO_PIN_NUMBER_INVALID, "GPIO Pin Number Invalid" }, + { GPIO_PIN_DIRECTION_INVALID, "GPIO Pin Direction Invalid" }, + { ILLEGAL_ADDRESS, "Illegal Address" }, + { ILLEGAL_SIZE, "Illegal Size" }, + { PARAMS_TABLE_NOT_VALID, "Params Table Not Valid" }, + { PARAMS_TABLE_ID_NOT_VALID, "Params Table Id Not Valid" }, + { PARAMS_TABLE_WRONG_EXISTING_SIZE, "Params Table Wrong Existing Size" }, + { SPI_READ_FAILED, "Spi Read Failed" }, + { SPI_WRITE_FAILED, "Spi Write Failed" }, + { TABLE_IS_EMPTY, "Table Is Empty" }, + { VALUE_OUT_OF_RANGE, "Value Out Of Range" }, + { OPERATION_TIMEOUT, "Operation Timeout" }, + { COMMAND_NOT_SUPPORTED, "Command Not Supported" }, + { INCOMPLETE_DATA, "Incomplete Data" }, + { SW_NOT_READY, "SW Not Ready" }, + { RESERVED_22, "Reserved 22" }, + { RESERVED_23, "Reserved 23" }, + { RESERVED_24, "Reserved 24" }, + { RESERVED_25, "Reserved 25" }, + { RESERVED_26, "Reserved 26" }, + { RESERVED_27, "Reserved 27" }, + { RESERVED_28, "Reserved 28" }, + { RESERVED_29, "Reserved 29" }, + { RESERVED_30, "Reserved 30" }, + { RESERVED_31, "Reserved 31" }, + { RESERVED_32, "Reserved 32" }, + { RESERVED_33, "Reserved 33" }, + { RESERVED_34, "Reserved 34" }, + { RESERVED_35, "Reserved 35" }, + { RESERVED_36, "Reserved 36" }, + { RESERVED_37, "Reserved 37" }, + { RESERVED_38, "Reserved 38" }, + { RESERVED_39, "Reserved 39" }, + { LAST_ERROR, "Last Error" } }; - inline virtual std::string hwmon_error2str(hwmon_response opcode) const override { + virtual std::string hwmon_error2str(hwmon_response_type opcode) const override { if (hwmon_response_report.find(opcode) != hwmon_response_report.end()) return hwmon_response_report.at(opcode); return {}; } - virtual hwmon_response hwmon_Success() const override { return d500_hwmon_response_names::hwm_Success; }; + virtual hwmon_response_type success_value() const override { return SUCCESS; }; }; } // namespace ds diff --git a/src/ds/d500/hw_monitor_extended_buffers.cpp b/src/ds/d500/hw_monitor_extended_buffers.cpp index 00c8963561..cc22c04747 100644 --- a/src/ds/d500/hw_monitor_extended_buffers.cpp +++ b/src/ds/d500/hw_monitor_extended_buffers.cpp @@ -31,7 +31,7 @@ namespace librealsense // - no buffer bigger than 1 KB is expected with the current command => work as normal hw_monitor::send method // - buffer bigger than 1 KB expected to be received => iterate the hw_monitor::send method and append the results // - buffer bigger than 1 KB expected to be sent => iterate the hw_monitor, while iterating over the input - std::vector hw_monitor_extended_buffers::send(command const & cmd, hwmon_response* p_response, bool locked_transfer) const + std::vector hw_monitor_extended_buffers::send(command const & cmd, hwmon_response_type* p_response, bool locked_transfer) const { hwm_buffer_type buffer_type = get_buffer_type(cmd); switch( buffer_type) @@ -49,7 +49,7 @@ namespace librealsense return std::vector(); } - std::vector hw_monitor_extended_buffers::extended_receive(command cmd, hwmon_response* p_response, bool locked_transfer) const + std::vector hw_monitor_extended_buffers::extended_receive(command cmd, hwmon_response_type* p_response, bool locked_transfer) const { std::vector< uint8_t > recv_msg; @@ -83,7 +83,7 @@ namespace librealsense return recv_msg; } - void hw_monitor_extended_buffers::extended_send(command cmd, hwmon_response* p_response, bool locked_transfer) const + void hw_monitor_extended_buffers::extended_send(command cmd, hwmon_response_type* p_response, bool locked_transfer) const { // copying the data, so that this param can be reused for the sending via HWM auto table_data = cmd.data; diff --git a/src/ds/d500/hw_monitor_extended_buffers.h b/src/ds/d500/hw_monitor_extended_buffers.h index 9b82fa5e66..95a26b3c5f 100644 --- a/src/ds/d500/hw_monitor_extended_buffers.h +++ b/src/ds/d500/hw_monitor_extended_buffers.h @@ -21,18 +21,18 @@ namespace librealsense extended_send }; - explicit hw_monitor_extended_buffers(std::shared_ptr locked_transfer, std::shared_ptr hwmon_response_handler) - : hw_monitor(locked_transfer, hwmon_response_handler) + explicit hw_monitor_extended_buffers(std::shared_ptr locked_transfer, std::shared_ptr hwmon_response) + : hw_monitor(locked_transfer, hwmon_response) {} virtual std::vector send(std::vector const& data) const override; - virtual std::vector send(command const & cmd, hwmon_response* = nullptr, bool locked_transfer = false) const override; + virtual std::vector send(command const & cmd, hwmon_response_type* = nullptr, bool locked_transfer = false) const override; private: int get_number_of_chunks(size_t msg_length) const; hwm_buffer_type get_buffer_type(command cmd) const; - std::vector extended_receive(command cmd, hwmon_response* p_response, bool locked_transfer) const; - void extended_send(command cmd, hwmon_response* p_response, bool locked_transfer) const; + std::vector extended_receive(command cmd, hwmon_response_type* p_response, bool locked_transfer) const; + void extended_send(command cmd, hwmon_response_type* p_response, bool locked_transfer) const; // The following method prepares the param4 of the command for extended buffers // The aim of this param is to send the chunk number out of max of expected chunks for the current command diff --git a/src/ds/ds-options.cpp b/src/ds/ds-options.cpp index bb1548e20e..3966d63974 100644 --- a/src/ds/ds-options.cpp +++ b/src/ds/ds-options.cpp @@ -517,8 +517,8 @@ namespace librealsense return "Inter-camera synchronization mode: 0:Default, 1:Master, 2:Slave"; } - alternating_emitter_option::alternating_emitter_option(hw_monitor& hwm, bool is_fw_version_using_id, int no_data_to_return_opcode) - : _hwm(hwm), _is_fw_version_using_id(is_fw_version_using_id), no_data_to_return_opcode(no_data_to_return_opcode) + alternating_emitter_option::alternating_emitter_option(hw_monitor& hwm, bool is_fw_version_using_id, hwmon_response_type no_data_to_return_opcode) + : _hwm(hwm), _is_fw_version_using_id(is_fw_version_using_id), _no_data_to_return_opcode(no_data_to_return_opcode) { _range = [this]() { @@ -552,9 +552,9 @@ namespace librealsense command cmd(ds::GETSUBPRESETID); try { - hwmon_response response; + hwmon_response_type response; auto res = _hwm.send( cmd, &response ); // avoid the throw - if (response != no_data_to_return_opcode) // If no subpreset is streaming, the firmware returns "NO_DATA_TO_RETURN" error + if (response != _no_data_to_return_opcode) // If no subpreset is streaming, the firmware returns "NO_DATA_TO_RETURN" error { // if a subpreset is streaming, checking this is the alternating emitter sub preset if( res.size() ) diff --git a/src/ds/ds-options.h b/src/ds/ds-options.h index 6d6c97cb59..01582c8d52 100644 --- a/src/ds/ds-options.h +++ b/src/ds/ds-options.h @@ -244,7 +244,7 @@ namespace librealsense class alternating_emitter_option : public option { public: - alternating_emitter_option(hw_monitor& hwm, bool is_fw_version_using_id, int no_data_to_return_opcode); + alternating_emitter_option(hw_monitor& hwm, bool is_fw_version_using_id, hwmon_response_type no_data_to_return_opcode); virtual ~alternating_emitter_option() = default; virtual void set(float value) override; virtual float query() const override; @@ -261,7 +261,7 @@ namespace librealsense rsutils::lazy< option_range > _range; hw_monitor& _hwm; bool _is_fw_version_using_id; - int no_data_to_return_opcode; + hwmon_response_type _no_data_to_return_opcode; }; class emitter_always_on_option : public option diff --git a/src/hdr-config.cpp b/src/hdr-config.cpp index d6226c9cce..43c5445ec3 100644 --- a/src/hdr-config.cpp +++ b/src/hdr-config.cpp @@ -10,7 +10,7 @@ namespace librealsense { hdr_config::hdr_config(hw_monitor& hwm, std::shared_ptr depth_ep, - const option_range& exposure_range, const option_range& gain_range) : + const option_range& exposure_range, const option_range& gain_range, hwmon_response_type no_data_to_return_opcode) : _hwm(hwm), _sensor(depth_ep), _is_enabled(false), @@ -23,7 +23,8 @@ namespace librealsense _exposure_range(exposure_range), _gain_range(gain_range), _use_workaround(true), - _pre_hdr_exposure(0.f) + _pre_hdr_exposure(0.f), + _no_data_to_return_opcode(no_data_to_return_opcode) { _hdr_sequence_params.clear(); _hdr_sequence_params.resize(DEFAULT_HDR_SEQUENCE_SIZE); @@ -230,9 +231,9 @@ namespace librealsense command cmd(ds::GETSUBPRESETID); try { - hwmon_response response; + hwmon_response_type response; auto res = _hwm.send( cmd, &response ); // avoid the throw - if (response != _hwm.hwmon_response_handler->hwmon_Success()) // If no subpreset is streaming, the firmware returns "NO_DATA_TO_RETURN" error + if (response != _no_data_to_return_opcode) // If no subpreset is streaming, the firmware returns "NO_DATA_TO_RETURN" error { // If a subpreset is streaming, checking this is the current HDR sub preset if( res.size() ) diff --git a/src/hdr-config.h b/src/hdr-config.h index 783ffb386c..e7c5994843 100644 --- a/src/hdr-config.h +++ b/src/hdr-config.h @@ -31,7 +31,7 @@ namespace librealsense { public: hdr_config(hw_monitor& hwm, std::shared_ptr depth_ep, - const option_range& exposure_range, const option_range& gain_range); + const option_range& exposure_range, const option_range& gain_range, hwmon_response_type no_data_to_return_opcode); float get(rs2_option option) const; @@ -87,6 +87,7 @@ namespace librealsense option_range _gain_range; bool _use_workaround; float _pre_hdr_exposure; + hwmon_response_type _no_data_to_return_opcode; }; } diff --git a/src/hw-monitor.cpp b/src/hw-monitor.cpp index 0b90ce1b05..aa6dce3681 100644 --- a/src/hw-monitor.cpp +++ b/src/hw-monitor.cpp @@ -143,7 +143,7 @@ namespace librealsense } std::vector< uint8_t > - hw_monitor::send( command const & cmd, hwmon_response * p_response, bool locked_transfer ) const + hw_monitor::send( command const & cmd, hwmon_response_type * p_response, bool locked_transfer ) const { uint32_t const opCodeXmit = cmd.cmd; @@ -170,7 +170,7 @@ namespace librealsense // Error/exit conditions if (p_response) - *p_response = hwmon_response_handler->hwmon_Success(); + *p_response = _hwmon_response->success_value(); if( ! cmd.require_response ) return {}; @@ -179,7 +179,7 @@ namespace librealsense details.receivedOpcode[1], details.receivedOpcode[0]); if (opCodeAsUint32 != opCodeXmit) { - auto err_type = static_cast(opCodeAsUint32); + auto err_type = static_cast(opCodeAsUint32); //LOG_DEBUG(err); // too intrusive; may be an expected error if( p_response ) { @@ -212,9 +212,9 @@ namespace librealsense return result; } - std::string hw_monitor::hwmon_error_string( command const & cmd, int response_code ) const + std::string hw_monitor::hwmon_error_string( command const & cmd, hwmon_response_type response_code ) const { - auto str = hwmon_response_handler->hwmon_error2str(response_code); + auto str = _hwmon_response->hwmon_error2str(response_code); std::ostringstream err; err << "hwmon command 0x" << std::hex << unsigned(cmd.cmd) << '('; err << ' ' << cmd.param1; diff --git a/src/hw-monitor.h b/src/hw-monitor.h index 122507d161..3f99198ba3 100644 --- a/src/hw-monitor.h +++ b/src/hw-monitor.h @@ -119,11 +119,11 @@ namespace librealsense } }; - typedef int32_t hwmon_response; - class base_hwmon_response_handler { // base class for different product number to implement responses + typedef int32_t hwmon_response_type; + class hwmon_response_interface { // base class for different product lines to implement responses public: - inline virtual std::string hwmon_error2str(int e) const = 0; - virtual hwmon_response hwmon_Success() const = 0; + virtual std::string hwmon_error2str(int e) const = 0; + virtual hwmon_response_type success_value() const = 0; }; class hw_monitor @@ -150,8 +150,8 @@ namespace librealsense static const size_t size_of_command_without_data = 24U; public: - explicit hw_monitor(std::shared_ptr locked_transfer, std::shared_ptr hwmon_response_handler) - : _locked_transfer(std::move(locked_transfer)), hwmon_response_handler(hwmon_response_handler) + explicit hw_monitor(std::shared_ptr locked_transfer, std::shared_ptr hwmon_response) + : _locked_transfer(std::move(locked_transfer)), _hwmon_response(hwmon_response) {} static void fill_usb_buffer( int opCodeNumber, @@ -167,7 +167,7 @@ namespace librealsense static command build_command_from_data(const std::vector data); virtual std::vector send( std::vector const & data ) const; - virtual std::vector send( command const & cmd, hwmon_response * = nullptr, bool locked_transfer = false ) const; + virtual std::vector send( command const & cmd, hwmon_response_type * = nullptr, bool locked_transfer = false ) const; static std::vector build_command(uint32_t opcode, uint32_t param1 = 0, uint32_t param2 = 0, @@ -232,8 +232,8 @@ namespace librealsense return rv; } - std::shared_ptr hwmon_response_handler; + std::shared_ptr _hwmon_response; - std::string hwmon_error_string(command const&, int e) const; + std::string hwmon_error_string(command const&, hwmon_response_type e) const; }; } From 4ce2041cc043a9fcd01e16b2d608319a1cf65019 Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Tue, 29 Oct 2024 11:22:20 +0200 Subject: [PATCH 67/75] add ENABLE_SECURITY_FLAGS option --- .github/workflows/buildsCI.yaml | 105 ++++++++++++++++++++++++++++++++ CMake/lrs_options.cmake | 1 + CMake/unix_config.cmake | 2 +- CMake/windows_config.cmake | 62 +++++++++---------- 4 files changed, 138 insertions(+), 32 deletions(-) diff --git a/.github/workflows/buildsCI.yaml b/.github/workflows/buildsCI.yaml index bc46e5bed6..515bd53196 100644 --- a/.github/workflows/buildsCI.yaml +++ b/.github/workflows/buildsCI.yaml @@ -215,6 +215,55 @@ jobs: run: | python3 unit-tests/run-unit-tests.py --no-color --debug --stdout --not-live --context "dds windows" ${{env.WIN_BUILD_DIR}}/Release +#-------------------------------------------------------------------------------- + Win_SH_Py_DDS_CI_SEC: # Windows, Shared, Python, Tools, DDS, libCI without executables, additional security checks + runs-on: windows-2019 + timeout-minutes: 60 + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 + with: + python-version: '3.8.1' + + - name: Enable Long Paths + shell: powershell + run: | + New-ItemProperty -Path "HKLM:\SYSTEM\CurrentControlSet\Control\FileSystem" -Name "LongPathsEnabled" -Value 1 -PropertyType DWORD -Force + + - name: Check_API + shell: bash + run: | + cd scripts + ./api_check.sh + cd .. + + - name: PreBuild + shell: bash + run: | + mkdir ${{env.WIN_BUILD_DIR}} + python3 -m pip install numpy + + - name: Configure CMake + shell: bash + run: | + LRS_SRC_DIR=$(pwd) + cd ${{env.WIN_BUILD_DIR}} + cmake ${LRS_SRC_DIR} -G "Visual Studio 16 2019" -DBUILD_SHARED_LIBS=true -DBUILD_EXAMPLES=false -DBUILD_TOOLS=true -DBUILD_UNIT_TESTS=false -DCHECK_FOR_UPDATES=false -DBUILD_WITH_DDS=true -DPYTHON_EXECUTABLE=${{env.PYTHON_PATH}} -DBUILD_PYTHON_BINDINGS=true -DENABLE_SECURITY_FLAGS=true + + - name: Build + # Build your program with the given configuration + shell: bash + run: | + cd ${{env.WIN_BUILD_DIR}} + cmake --build . --config ${{env.LRS_RUN_CONFIG}} -- -m + + - name: LibCI + # Note: we specifically disable BUILD_UNIT_TESTS so the executable C++ unit-tests won't run + # This is to save time as DDS already lengthens the build... + shell: bash + run: | + python3 unit-tests/run-unit-tests.py --no-color --debug --stdout --not-live --context "dds windows" ${{env.WIN_BUILD_DIR}}/Release + #-------------------------------------------------------------------------------- Win_SH_Py_RSUSB_Csharp: # Windows, Shared, Python, RSUSB backend, C# bindings @@ -394,6 +443,62 @@ jobs: python3 unit-tests/run-unit-tests.py --no-color --debug --stdout --not-live --context "dds linux" --tag dds + #-------------------------------------------------------------------------------- + U20_ST_Py_DDS_RSUSB_CI_SEC: # Ubuntu 2020, Static, Python, DDS, RSUSB, LibCI without executables, additional security checks + runs-on: ubuntu-20.04 + timeout-minutes: 60 + steps: + - uses: actions/checkout@v4 + + - name: Prebuild + shell: bash + run: | + sudo apt-get update; + sudo apt-get install -qq build-essential xorg-dev libgl1-mesa-dev libglu1-mesa-dev libglew-dev libglm-dev; + sudo apt-get install -qq libusb-1.0-0-dev; + sudo apt-get install -qq libgtk-3-dev; + sudo apt-get install libglfw3-dev libglfw3; + # We force compiling with GCC 7 because the default installed GCC 9 compiled with LTO and gives an internal compiler error + sudo apt-get install gcc-7 g++-7; + sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-7 60 --slave /usr/bin/g++ g++ /usr/bin/g++-7; + python3 -m pip install numpy + + - name: Check_API + shell: bash + run: | + cd scripts + ./api_check.sh + ./pr_check.sh + cd .. + mkdir build + + - name: Build + # Note: we force RSUSB because, on Linux, the context creation will fail on GHA: + # (backend-v4l2.cpp:555) Cannot access /sys/class/video4linux) + # And, well, we don't need any specific platform for DDS! + shell: bash + run: | + cd build + cmake .. -DCMAKE_BUILD_TYPE=${{env.LRS_RUN_CONFIG}} -DBUILD_SHARED_LIBS=false -DBUILD_EXAMPLES=false -DBUILD_TOOLS=false -DBUILD_UNIT_TESTS=false -DCHECK_FOR_UPDATES=false -DBUILD_WITH_DDS=true -DBUILD_PYTHON_BINDINGS=true -DPYTHON_EXECUTABLE=$(which python3) -DFORCE_RSUSB_BACKEND=true -DENABLE_SECURITY_FLAGS=true + cmake --build . -- -j4 + + - name: Client for realsense2-all + shell: bash + run: | + mkdir build/rs-all-client + cd build/rs-all-client + cmake ../../.github/workflows/rs-all-client -DBUILD_WITH_DDS=ON -DFORCE_RSUSB_BACKEND=ON + cmake --build . -- -j4 + ./rs-all-client + + - name: LibCI + # Note: we specifically disable BUILD_UNIT_TESTS so the executable C++ unit-tests won't run + # This is to save time as DDS already lengthens the build... + shell: bash + run: | + python3 unit-tests/run-unit-tests.py --no-color --debug --stdout --not-live --context "dds linux" --tag dds + + #-------------------------------------------------------------------------------- U22_U24_SH_Py_DDS_CI: # Ubuntu, Shared, Python, DDS, LibCI without executables runs-on: ${{ matrix.os }} diff --git a/CMake/lrs_options.cmake b/CMake/lrs_options.cmake index cc2b21de13..02ca70796b 100644 --- a/CMake/lrs_options.cmake +++ b/CMake/lrs_options.cmake @@ -47,4 +47,5 @@ endif() option(BUILD_PC_STITCHING "Build pointcloud-stitching example" OFF) option(BUILD_WITH_DDS "Access camera devices through DDS topics (requires CMake 3.16.3)" OFF) option(BUILD_RS2_ALL "Build realsense2-all static bundle containing all realsense libraries (with BUILD_SHARED_LIBS=OFF)" ON) +option(ENABLE_SECURITY_FLAGS "Enable additional compiler security flags to enhance the build's security" OFF) diff --git a/CMake/unix_config.cmake b/CMake/unix_config.cmake index 1e22852d0f..e6d02574dd 100644 --- a/CMake/unix_config.cmake +++ b/CMake/unix_config.cmake @@ -48,7 +48,7 @@ macro(os_set_flags) endif() - if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND ENABLE_SECURITY_FLAGS) # Due to security reasons we need to add the following flags for additional security: # Debug & Release: # -Wformat: Checks for format string vulnerabilities. diff --git a/CMake/windows_config.cmake b/CMake/windows_config.cmake index dd2a9dfe80..c01189b729 100644 --- a/CMake/windows_config.cmake +++ b/CMake/windows_config.cmake @@ -40,39 +40,39 @@ macro(os_set_flags) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /MP") - ############### - # Due to security reasons we need to add the following flags for additional security: - # Debug & Release: - # /Gy: Enables function-level linking to reduce executable size. - # /DYNAMICBASE: Enables Address Space Layout Randomization (ASLR) to improve security. - # /GS: Enables buffer security checks to prevent buffer overflows. - - # Release only: - # /WX: Treats all warnings as errors. - # /sdl: Enables additional security checks. - - # Release only linker flags: - # /LTCG (/GL): Enables Link Time Code Generation to improve performance. - # /NXCOMPAT: Enables Data Execution Prevention (DEP) to prevent code execution in data areas. - - # see https://readthedocs.intel.com/SecureCodingStandards/2023.Q2.0/compiler/c-cpp/ for more details - - set(SECURITY_COMPILER_FLAGS "/Gy /DYNAMICBASE /GS /wd4101") + if (ENABLE_SECURITY_FLAGS) + # Due to security reasons we need to add the following flags for additional security: + # Debug & Release: + # /Gy: Enables function-level linking to reduce executable size. + # /DYNAMICBASE: Enables Address Space Layout Randomization (ASLR) to improve security. + # /GS: Enables buffer security checks to prevent buffer overflows. + + # Release only: + # /WX: Treats all warnings as errors. + # /sdl: Enables additional security checks. + + # Release only linker flags: + # /LTCG (/GL): Enables Link Time Code Generation to improve performance. + # /NXCOMPAT: Enables Data Execution Prevention (DEP) to prevent code execution in data areas. + + # see https://readthedocs.intel.com/SecureCodingStandards/2023.Q2.0/compiler/c-cpp/ for more details + + set(SECURITY_COMPILER_FLAGS "/Gy /DYNAMICBASE /GS /wd4101") + + if(CMAKE_BUILD_TYPE STREQUAL "Debug") + message(STATUS "Configuring for Debug build") + else() # Release, RelWithDebInfo, or multi configuration generator is being used (aka not specifing build type, or building with VS) + message(STATUS "Configuring for Release build") + set(SECURITY_COMPILER_FLAGS "${SECURITY_COMPILER_FLAGS} /WX /sdl") + endif() + + push_security_flags() + + if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") + set(CMAKE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS} /INCREMENTAL:NO /LTCG /NXCOMPAT") # ignoring '/INCREMENTAL' due to '/LTCG' specification + endif() - if(CMAKE_BUILD_TYPE STREQUAL "Debug") - message(STATUS "Configuring for Debug build") - else() # Release, RelWithDebInfo, or multi configuration generator is being used (aka not specifing build type, or building with VS) - message(STATUS "Configuring for Release build") - set(SECURITY_COMPILER_FLAGS "${SECURITY_COMPILER_FLAGS} /WX /sdl") endif() - - push_security_flags() - - if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") - set(CMAKE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS} /INCREMENTAL:NO /LTCG /NXCOMPAT") # ignoring '/INCREMENTAL' due to '/LTCG' specification - endif() - - ################# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /bigobj /wd4819") set(LRS_TRY_USE_AVX true) From 61b6bf1b946683aada5446052853148a1e147e4b Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Mon, 28 Oct 2024 15:47:14 +0200 Subject: [PATCH 68/75] D457 timestamp handling back compat add test line --- src/ds/d400/d400-device.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/ds/d400/d400-device.cpp b/src/ds/d400/d400-device.cpp index 0506bd9cb5..d73501aca9 100644 --- a/src/ds/d400/d400-device.cpp +++ b/src/ds/d400/d400-device.cpp @@ -1090,13 +1090,25 @@ namespace librealsense // attributes of md_mipi_depth_control structure auto md_prop_offset = offsetof(metadata_mipi_depth_raw, depth_mode); - // optical_timestamp contains value of exposure/2 - depth_sensor.register_metadata(RS2_FRAME_METADATA_SENSOR_TIMESTAMP, + // timestamp metadata field alignment with FW was implemented on FW version 5.17.0.0 + if ( _fw_version >= firmware_version( "5.17.0.0" ) ) + { + // optical_timestamp contains value of exposure/2 + depth_sensor.register_metadata(RS2_FRAME_METADATA_SENSOR_TIMESTAMP, make_rs400_sensor_ts_parser(make_uvc_header_parser(&platform::uvc_header::timestamp), make_attribute_parser(&md_mipi_depth_mode::optical_timestamp, md_mipi_depth_control_attributes::optical_timestamp_attribute, md_prop_offset))); + } + else // keep backward compatible with previous behavior + { + depth_sensor.register_metadata(RS2_FRAME_METADATA_SENSOR_TIMESTAMP, + make_attribute_parser(&md_mipi_depth_mode::optical_timestamp, + md_mipi_depth_control_attributes::optical_timestamp_attribute, + md_prop_offset)); + } + depth_sensor.register_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE, make_attribute_parser(&md_mipi_depth_mode::exposure_time, md_mipi_depth_control_attributes::exposure_time_attribute, From 091a7355a629279f15afa19662ab29a2b9ffa734 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 30 Oct 2024 08:14:16 +0200 Subject: [PATCH 69/75] Fix width/height values in if statement --- common/model-views.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/model-views.cpp b/common/model-views.cpp index 909f3bc9f8..9ada1ea90b 100644 --- a/common/model-views.cpp +++ b/common/model-views.cpp @@ -46,7 +46,7 @@ return RS2_SENSOR_MODE_QVGA; else if ((width == 640 && height == 480) || (height == 640 && width == 480)) return RS2_SENSOR_MODE_VGA; - else if ((width == 1024 && height == 768) || (height == 768 && width == 1024)) + else if ((width == 1024 && height == 768) || (height == 1024 && width == 768)) return RS2_SENSOR_MODE_XGA; else return RS2_SENSOR_MODE_COUNT; From 6b0b06a4e42e50f515bec2115e7f9eb93d496c79 Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Wed, 30 Oct 2024 12:47:08 +0200 Subject: [PATCH 70/75] move security flags change into projects' cmake --- CMake/external_fastdds.cmake | 5 ++++- CMake/external_foonathan_memory.cmake | 4 ++++ CMakeLists.txt | 4 ---- third-party/CMakeLists.txt | 2 +- 4 files changed, 9 insertions(+), 6 deletions(-) diff --git a/CMake/external_fastdds.cmake b/CMake/external_fastdds.cmake index 856e0dbfc7..59e16290b9 100644 --- a/CMake/external_fastdds.cmake +++ b/CMake/external_fastdds.cmake @@ -67,7 +67,10 @@ function(get_fastdds) message(CHECK_PASS "Done") endfunction() + +pop_security_flags() + # Trigger the FastDDS build get_fastdds() - +push_security_flags() diff --git a/CMake/external_foonathan_memory.cmake b/CMake/external_foonathan_memory.cmake index f6c4938267..de91069005 100644 --- a/CMake/external_foonathan_memory.cmake +++ b/CMake/external_foonathan_memory.cmake @@ -41,4 +41,8 @@ function(get_foonathan_memory) endfunction() +pop_security_flags() + get_foonathan_memory() + +push_security_flags() diff --git a/CMakeLists.txt b/CMakeLists.txt index 8c4f51966c..24cfb79587 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,16 +63,12 @@ target_link_libraries( ${LRS_TARGET} PUBLIC rsutils ) if(BUILD_WITH_DDS) if (CMAKE_SYSTEM MATCHES "Windows" OR CMAKE_SYSTEM MATCHES "Linux") - - pop_security_flags() message(STATUS "Building with FastDDS") include(CMake/external_foonathan_memory.cmake) include(CMake/external_fastdds.cmake) target_link_libraries( ${LRS_TARGET} PRIVATE realdds ) - - push_security_flags() else() MESSAGE(STATUS "Turning off `BUILD_WITH_DDS` as it's only supported on Windows & Linux and not on ${CMAKE_SYSTEM}") diff --git a/third-party/CMakeLists.txt b/third-party/CMakeLists.txt index 708ffc8439..c28e66c84d 100644 --- a/third-party/CMakeLists.txt +++ b/third-party/CMakeLists.txt @@ -3,7 +3,7 @@ string(REPLACE ${PROJECT_SOURCE_DIR}/ "" _rel_path ${CMAKE_CURRENT_LIST_DIR}) add_subdirectory( "${CMAKE_CURRENT_LIST_DIR}/rsutils" ) -pop_security_flags() +pop_security_flags() # remove security flags for third party, as we cannot guarantee their security enforcment include(CMake/external_json.cmake) # Add additional include directories to allow file to include rosbag headers From dcc624bb6fb5cdcf2db197f74e5d698767f7e085 Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Wed, 30 Oct 2024 12:52:25 +0200 Subject: [PATCH 71/75] remove libci from security builds --- .github/workflows/buildsCI.yaml | 18 ++---------------- 1 file changed, 2 insertions(+), 16 deletions(-) diff --git a/.github/workflows/buildsCI.yaml b/.github/workflows/buildsCI.yaml index 515bd53196..78d4f98436 100644 --- a/.github/workflows/buildsCI.yaml +++ b/.github/workflows/buildsCI.yaml @@ -216,7 +216,7 @@ jobs: python3 unit-tests/run-unit-tests.py --no-color --debug --stdout --not-live --context "dds windows" ${{env.WIN_BUILD_DIR}}/Release #-------------------------------------------------------------------------------- - Win_SH_Py_DDS_CI_SEC: # Windows, Shared, Python, Tools, DDS, libCI without executables, additional security checks + Win_SH_Py_DDS_SEC: # Windows, Shared, Python, Tools, DDS, additional security checks runs-on: windows-2019 timeout-minutes: 60 steps: @@ -257,13 +257,6 @@ jobs: cd ${{env.WIN_BUILD_DIR}} cmake --build . --config ${{env.LRS_RUN_CONFIG}} -- -m - - name: LibCI - # Note: we specifically disable BUILD_UNIT_TESTS so the executable C++ unit-tests won't run - # This is to save time as DDS already lengthens the build... - shell: bash - run: | - python3 unit-tests/run-unit-tests.py --no-color --debug --stdout --not-live --context "dds windows" ${{env.WIN_BUILD_DIR}}/Release - #-------------------------------------------------------------------------------- Win_SH_Py_RSUSB_Csharp: # Windows, Shared, Python, RSUSB backend, C# bindings @@ -444,7 +437,7 @@ jobs: #-------------------------------------------------------------------------------- - U20_ST_Py_DDS_RSUSB_CI_SEC: # Ubuntu 2020, Static, Python, DDS, RSUSB, LibCI without executables, additional security checks + U20_ST_Py_DDS_RSUSB_SEC: # Ubuntu 2020, Static, Python, DDS, RSUSB, additional security checks runs-on: ubuntu-20.04 timeout-minutes: 60 steps: @@ -490,13 +483,6 @@ jobs: cmake ../../.github/workflows/rs-all-client -DBUILD_WITH_DDS=ON -DFORCE_RSUSB_BACKEND=ON cmake --build . -- -j4 ./rs-all-client - - - name: LibCI - # Note: we specifically disable BUILD_UNIT_TESTS so the executable C++ unit-tests won't run - # This is to save time as DDS already lengthens the build... - shell: bash - run: | - python3 unit-tests/run-unit-tests.py --no-color --debug --stdout --not-live --context "dds linux" --tag dds #-------------------------------------------------------------------------------- From fcb9d0fa0ce320ab5450ef56eaaa0a0b5ff48345 Mon Sep 17 00:00:00 2001 From: Avia Avraham <145359432+AviaAv@users.noreply.github.com> Date: Wed, 30 Oct 2024 14:23:00 +0200 Subject: [PATCH 72/75] explicitly init SECURITY_COMPILER_FLAGS --- CMake/unix_config.cmake | 2 +- CMake/windows_config.cmake | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/CMake/unix_config.cmake b/CMake/unix_config.cmake index e6d02574dd..c038809d51 100644 --- a/CMake/unix_config.cmake +++ b/CMake/unix_config.cmake @@ -47,7 +47,7 @@ macro(os_set_flags) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") endif() - + set(SECURITY_COMPILER_FLAGS "") if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND ENABLE_SECURITY_FLAGS) # Due to security reasons we need to add the following flags for additional security: # Debug & Release: diff --git a/CMake/windows_config.cmake b/CMake/windows_config.cmake index c01189b729..8f87a59942 100644 --- a/CMake/windows_config.cmake +++ b/CMake/windows_config.cmake @@ -40,6 +40,7 @@ macro(os_set_flags) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /MP") + set(SECURITY_COMPILER_FLAGS "") if (ENABLE_SECURITY_FLAGS) # Due to security reasons we need to add the following flags for additional security: # Debug & Release: From 66c402b011f942202b235ef9a06ca5d592b777bf Mon Sep 17 00:00:00 2001 From: Benjamin Wuethrich Date: Wed, 30 Oct 2024 09:12:30 -0700 Subject: [PATCH 73/75] Add step to change directory --- doc/installation.md | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/installation.md b/doc/installation.md index 498bc5b15a..9e770a5c21 100644 --- a/doc/installation.md +++ b/doc/installation.md @@ -73,6 +73,7 @@ if not the SDK will use a timer polling approach which is less sensitive for dev 2. Run Intel Realsense permissions script from _librealsense2_ root directory: ```sh + cd librealsense ./scripts/setup_udev_rules.sh ``` Notice: You can always remove permissions by running: `./scripts/setup_udev_rules.sh --uninstall` From 0a7dbb6dbfa1498eee8261c2310b163bbee944f8 Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Thu, 31 Oct 2024 09:40:44 +0200 Subject: [PATCH 74/75] update version to 2.56.3 --- include/librealsense2/rs.h | 2 +- package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/librealsense2/rs.h b/include/librealsense2/rs.h index 15ee7deaea..fe0ef17edf 100644 --- a/include/librealsense2/rs.h +++ b/include/librealsense2/rs.h @@ -25,7 +25,7 @@ extern "C" { #define RS2_API_MAJOR_VERSION 2 #define RS2_API_MINOR_VERSION 56 -#define RS2_API_PATCH_VERSION 2 +#define RS2_API_PATCH_VERSION 3 #define RS2_API_BUILD_VERSION 0 #ifndef STRINGIFY diff --git a/package.xml b/package.xml index 0cea9f5df1..01ddbff68b 100644 --- a/package.xml +++ b/package.xml @@ -6,7 +6,7 @@ librealsense2 - 2.56.2 + 2.56.3 Library for controlling and capturing data from the Intel(R) RealSense(TM) D400 devices. From 4b2ad412aae385ef6eec5f8943a377a5d1501aa3 Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Thu, 31 Oct 2024 09:54:57 +0200 Subject: [PATCH 75/75] remove redundent leftovers --- third-party/libusb/CMakeLists-remove.txt | 96 ------------------------ 1 file changed, 96 deletions(-) delete mode 100644 third-party/libusb/CMakeLists-remove.txt diff --git a/third-party/libusb/CMakeLists-remove.txt b/third-party/libusb/CMakeLists-remove.txt deleted file mode 100644 index 69fb41c2cd..0000000000 --- a/third-party/libusb/CMakeLists-remove.txt +++ /dev/null @@ -1,96 +0,0 @@ -# our current cmake version is 3.8 -cmake_minimum_required(VERSION 3.8) - -project(usb) - - set(LIBUSB_C - libusb/core.c - libusb/descriptor.c - libusb/hotplug.c - libusb/io.c - libusb/strerror.c - libusb/sync.c - ) - -if(WIN32) - LIST(APPEND LIBUSB_C - libusb/os/threads_windows.c - libusb/os/poll_windows.c - libusb/os/windows_winusb.c - libusb/os/windows_nt_common.c - libusb/os/windows_usbdk.c - ) -elseif (APPLE) - LIST(APPEND LIBUSB_C - libusb/os/poll_posix.c - libusb/os/threads_posix.c - libusb/os/darwin_usb.c - ) -elseif(ANDROID) - LIST(APPEND LIBUSB_C - libusb/os/linux_usbfs.c - libusb/os/poll_posix.c - libusb/os/threads_posix.c - libusb/os/linux_netlink.c - ) -else() - LIST(APPEND LIBUSB_C - libusb/os/linux_usbfs.c - libusb/os/poll_posix.c - libusb/os/threads_posix.c - libusb/os/linux_udev.c - ) -endif() - -set(LIBUSB_H - libusb/libusb.h -) - -include_directories( - libusb - libusb/os -) - -add_library(usb STATIC ${LIBUSB_C} ${LIBUSB_H}) - -if(WIN32) - set_target_properties (usb PROPERTIES - FOLDER "3rd Party" - ) - include_directories(msvc) - foreach(flag_var - CMAKE_CXX_FLAGS CMAKE_CXX_FLAGS_DEBUG CMAKE_CXX_FLAGS_RELEASE - CMAKE_CXX_FLAGS_MINSIZEREL CMAKE_CXX_FLAGS_RELWITHDEBINFO - CMAKE_C_FLAGS CMAKE_C_FLAGS_DEBUG CMAKE_C_FLAGS_RELEASE - CMAKE_C_FLAGS_MINSIZEREL CMAKE_C_FLAGS_RELWITHDEBINFO) - if(${flag_var} MATCHES "/MD") - string(REGEX REPLACE "/MD" "/MT" ${flag_var} "${${flag_var}}") - endif(${flag_var} MATCHES "/MD") - endforeach(flag_var) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /utf-8") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /utf-8") -endif() - -if(ANDROID) - include_directories(android) -endif() - -if(APPLE) - find_library(corefoundation_lib CoreFoundation) - find_library(iokit_lib IOKit) - target_include_directories(usb PRIVATE XCode) - TARGET_LINK_LIBRARIES(usb objc ${corefoundation_lib} ${iokit_lib}) -endif() - -if((NOT APPLE) AND (NOT ANDROID) AND (NOT WIN32)) - TARGET_LINK_LIBRARIES(usb udev) -endif() - -#set_target_properties(usb PROPERTIES PREFIX "") - -install(TARGETS ${PROJECT_NAME} - EXPORT realsense2Targets - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - )