diff --git a/.github/workflows/buildsCI.yaml b/.github/workflows/buildsCI.yaml index e1a8fe66c3..564d6e3796 100644 --- a/.github/workflows/buildsCI.yaml +++ b/.github/workflows/buildsCI.yaml @@ -351,7 +351,7 @@ jobs: #-------------------------------------------------------------------------------- - U20_ST_Py_DDS_CI: # Ubuntu 2020, Static, Python, DDS, LibCI without executables + U20_ST_Py_DDS_RSUSB_CI: # Ubuntu 2020, Static, Python, DDS, RSUSB, LibCI without executables runs-on: ubuntu-20.04 timeout-minutes: 60 steps: @@ -381,10 +381,13 @@ jobs: 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=true -DBUILD_UNIT_TESTS=false -DCHECK_FOR_UPDATES=false -DBUILD_WITH_DDS=true -DBUILD_PYTHON_BINDINGS=true -DPYTHON_EXECUTABLE=$(which python3) + 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 cmake --build . -- -j4 - name: LibCI @@ -392,7 +395,7 @@ jobs: # 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" + python3 unit-tests/run-unit-tests.py --no-color --debug --stdout --not-live --context "dds linux" --tag dds #-------------------------------------------------------------------------------- diff --git a/.github/workflows/cppcheck-parse.py b/.github/workflows/cppcheck-parse.py index 882ce86628..2af406295a 100644 --- a/.github/workflows/cppcheck-parse.py +++ b/.github/workflows/cppcheck-parse.py @@ -28,7 +28,7 @@ def __init__( self, message, line=None ): global line_number, logfile if line is None: line = line_number - super().__init__( f'{logile}+{line}: {message}' ) + super().__init__( f'{logfile}+{line}: {message}' ) handle = open( logfile, "r" ) diff --git a/.github/workflows/cppcheck_run.log b/.github/workflows/cppcheck_run.log index 7498abd549..f01f00021e 100644 --- a/.github/workflows/cppcheck_run.log +++ b/.github/workflows/cppcheck_run.log @@ -41,6 +41,10 @@ + + + + @@ -59,10 +63,6 @@ - - - - @@ -97,18 +97,18 @@ - - - - - - + + + + + + @@ -227,6 +227,24 @@ + + + + + + + + + + + + + + + + + + @@ -245,12 +263,6 @@ - - - - - - @@ -271,18 +283,6 @@ - - - - - - - - - - - - @@ -290,9 +290,6 @@ - - - @@ -305,6 +302,9 @@ + + + @@ -430,9 +430,6 @@ - - - @@ -477,25 +474,25 @@ - + - + - + - + - + - + - + - + - + - + @@ -520,12 +517,12 @@ - + - - + + diff --git a/CMake/external_fastdds.cmake b/CMake/external_fastdds.cmake new file mode 100644 index 0000000000..a1162949ad --- /dev/null +++ b/CMake/external_fastdds.cmake @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 3.5) +include(FetchContent) + +# We use a function to enforce a scoped variables creation only for FastDDS build (i.e turn off BUILD_SHARED_LIBS which is used on LRS build as well) +function(get_fastdds) + + # Mark new options from FetchContent as advanced options + mark_as_advanced(FETCHCONTENT_QUIET) + mark_as_advanced(FETCHCONTENT_BASE_DIR) + mark_as_advanced(FETCHCONTENT_FULLY_DISCONNECTED) + mark_as_advanced(FETCHCONTENT_UPDATES_DISCONNECTED) + + message(CHECK_START "Fetching fastdds...") + list(APPEND CMAKE_MESSAGE_INDENT " ") # Indent outputs + + FetchContent_Declare( + fastdds + GIT_REPOSITORY https://github.com/eProsima/Fast-DDS.git + GIT_TAG v2.9.1 + GIT_SUBMODULES "" # Submodules will be cloned as part of the FastDDS cmake configure stage + GIT_SHALLOW ON # No history needed + SOURCE_DIR ${CMAKE_BINARY_DIR}/third-party/fastdds + ) + + # Set FastDDS internal variables + # We use cached variables so the default parameter inside the sub directory will not override the required values + # We add "FORCE" so that is a previous cached value is set our assignment will override it. + set(THIRDPARTY_Asio FORCE CACHE INTERNAL "" FORCE) + set(THIRDPARTY_fastcdr FORCE CACHE INTERNAL "" FORCE) + set(THIRDPARTY_TinyXML2 FORCE CACHE INTERNAL "" FORCE) + set(COMPILE_TOOLS OFF CACHE INTERNAL "" FORCE) + set(BUILD_TESTING OFF CACHE INTERNAL "" FORCE) + set(SQLITE3_SUPPORT OFF CACHE INTERNAL "" FORCE) + + # Set special values for FastDDS sub directory + set(BUILD_SHARED_LIBS OFF) + set(CMAKE_INSTALL_PREFIX ${CMAKE_BINARY_DIR}/fastdds/fastdds_install) + set(CMAKE_PREFIX_PATH ${CMAKE_BINARY_DIR}/fastdds/fastdds_install) + + # Get fastdds + FetchContent_MakeAvailable(fastdds) + + # Mark new options from FetchContent as advanced options + mark_as_advanced(FETCHCONTENT_SOURCE_DIR_FASTDDS) + mark_as_advanced(FETCHCONTENT_UPDATES_DISCONNECTED_FASTDDS) + + # place FastDDS project with other 3rd-party projects + set_target_properties(fastcdr fastrtps PROPERTIES + FOLDER "ExternalProjectTargets/fastdds") + + list(POP_BACK CMAKE_MESSAGE_INDENT) # Unindent outputs + + add_library(dds INTERFACE) + target_link_libraries(dds INTERFACE fastcdr fastrtps) + + add_definitions(-DBUILD_WITH_DDS) + + install(TARGETS dds EXPORT realsense2Targets) + message(CHECK_PASS "Done") +endfunction() + +# Trigger the FastDDS build +get_fastdds() + + diff --git a/CMake/external_foonathan_memory.cmake b/CMake/external_foonathan_memory.cmake new file mode 100644 index 0000000000..bcff6b4e3e --- /dev/null +++ b/CMake/external_foonathan_memory.cmake @@ -0,0 +1,83 @@ +cmake_minimum_required(VERSION 3.11) +include(FetchContent) + +# Mark new options from FetchContent as advanced options +mark_as_advanced(FETCHCONTENT_QUIET) +mark_as_advanced(FETCHCONTENT_BASE_DIR) +mark_as_advanced(FETCHCONTENT_FULLY_DISCONNECTED) +mark_as_advanced(FETCHCONTENT_UPDATES_DISCONNECTED) + +message(CHECK_START "Fetching & Installing foonathan_memory...") +list(APPEND CMAKE_MESSAGE_INDENT " ") + +FetchContent_Declare( + foonathan_memory + GIT_REPOSITORY https://github.com/foonathan/memory.git + GIT_TAG "v0.7-2" + GIT_SHALLOW ON # No history needed + SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/third-party/foonathan_memory +) + +# Remove unrequired targets +set(FOONATHAN_MEMORY_BUILD_VARS -DBUILD_SHARED_LIBS=OFF # explicit set static lib + -DFOONATHAN_MEMORY_BUILD_EXAMPLES=OFF + -DFOONATHAN_MEMORY_BUILD_TESTS=OFF + -DFOONATHAN_MEMORY_BUILD_TOOLS=ON) # this tool is needed during configure time only, FastDDS recommend turning it ON. + +# Align STATIC CRT definitions with LRS +if(BUILD_WITH_STATIC_CRT) + set(FOONATHAN_MEMORY_BUILD_VARS ${FOONATHAN_MEMORY_BUILD_VARS} + -DCMAKE_POLICY_DEFAULT_CMP0091:STRING=NEW + -DCMAKE_MSVC_RUNTIME_LIBRARY:STRING=MultiThreaded$<$:Debug>) +endif() + + +# Since `FastDDS` require foonathan_memory package installed during configure time, +# We download it build it and install it both in Release & Debug configuration since we need both available. +# We use `FetchContent_Populate` and not `FetchContent_MakeAvailable` for that reason, we want to manually configure and build it. +FetchContent_GetProperties(foonathan_memory) +if(NOT foonathan_memory_POPULATED) + FetchContent_Populate(foonathan_memory) +endif() + +# Mark new options from FetchContent as advanced options +mark_as_advanced(FETCHCONTENT_SOURCE_DIR_FOONATHAN_MEMORY) +mark_as_advanced(FETCHCONTENT_UPDATES_DISCONNECTED_FOONATHAN_MEMORY) + + +# Configure stage +execute_process(COMMAND "${CMAKE_COMMAND}" -G "${CMAKE_GENERATOR}" -DCMAKE_INSTALL_PREFIX=${CMAKE_CURRENT_BINARY_DIR}/fastdds/fastdds_install + ${FOONATHAN_MEMORY_BUILD_VARS} + . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/third-party/foonathan_memory" + OUTPUT_QUIET + RESULT_VARIABLE configure_ret +) + +# Build and install Debug version +execute_process(COMMAND "${CMAKE_COMMAND}" --build . --config Debug --target install + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/third-party/foonathan_memory" + OUTPUT_QUIET + RESULT_VARIABLE debug_build_ret +) + +# Build and install RelWithDeb version +execute_process(COMMAND "${CMAKE_COMMAND}" --build . --config RelWithDebInfo --target install + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/third-party/foonathan_memory" + OUTPUT_QUIET + RESULT_VARIABLE rel_with_deb_info_build_ret +) + +# Build and install Release version +execute_process(COMMAND "${CMAKE_COMMAND}" --build . --config Release --target install + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/third-party/foonathan_memory" + OUTPUT_QUIET + RESULT_VARIABLE release_build_ret +) + + if(configure_ret OR debug_build_ret OR release_build_ret OR rel_with_deb_info_build_ret) + message( FATAL_ERROR "Failed to build foonathan_memory") + endif() + +list(POP_BACK CMAKE_MESSAGE_INDENT) +message(CHECK_PASS "Done") diff --git a/CMake/external_json.cmake b/CMake/external_json.cmake new file mode 100644 index 0000000000..1f16025128 --- /dev/null +++ b/CMake/external_json.cmake @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.6) +include(ExternalProject) + + + +# We use a function to enforce a scoped variables creation only for the build +# (i.e turn off BUILD_SHARED_LIBS which is used on LRS build as well) +function(get_nlohmann_json) + + message( STATUS #CHECK_START + "Fetching nlohmann/json..." ) + #list( APPEND CMAKE_MESSAGE_INDENT " " ) # Indent outputs + + # We want to clone the pybind repo and build it here, during configuration, so we can use it. + # But ExternalProject_add is limited in that it only does its magic during build. + # This is possible in CMake 3.12+ with FetchContent and FetchContent_MakeAvailable in 3.14+ (meaning Ubuntu 20) + # but we need to adhere to CMake 3.10 (Ubuntu 18). + # So instead, we invoke a new CMake project just to download pybind: + configure_file( CMake/json-download.cmake.in + ${CMAKE_BINARY_DIR}/external-projects/json-download/CMakeLists.txt ) + execute_process( COMMAND "${CMAKE_COMMAND}" -G "${CMAKE_GENERATOR}" . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/external-projects/json-download" + OUTPUT_QUIET + RESULT_VARIABLE configure_ret ) + execute_process( COMMAND "${CMAKE_COMMAND}" --build . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/external-projects/json-download" + OUTPUT_QUIET + RESULT_VARIABLE build_ret ) + + if( configure_ret OR build_ret ) + message( FATAL_ERROR "Failed to download nlohmann/json" ) + endif() + + message( STATUS #CHECK_PASS + "Fetching nlohmann/json - Done" ) + #list( POP_BACK CMAKE_MESSAGE_INDENT ) # Unindent outputs (requires cmake 3.15) + +endfunction() + +# Trigger the build +get_nlohmann_json() diff --git a/CMake/external_pybind11.cmake b/CMake/external_pybind11.cmake index 57ceb21b8a..34e3a4fb7c 100644 --- a/CMake/external_pybind11.cmake +++ b/CMake/external_pybind11.cmake @@ -20,10 +20,16 @@ function(get_pybind11) ${CMAKE_BINARY_DIR}/external-projects/pybind11-download/CMakeLists.txt ) execute_process( COMMAND "${CMAKE_COMMAND}" -G "${CMAKE_GENERATOR}" . WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/external-projects/pybind11-download" - OUTPUT_QUIET ) + OUTPUT_QUIET + RESULT_VARIABLE configure_ret ) execute_process( COMMAND "${CMAKE_COMMAND}" --build . WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/external-projects/pybind11-download" - OUTPUT_QUIET ) + OUTPUT_QUIET + RESULT_VARIABLE build_ret ) + + if( configure_ret OR build_ret ) + message( FATAL_ERROR "Failed to download pybind11" ) + endif() # Now that it's available, we can refer to it with an actual ExternalProject_add (but notice we're not # downloading anything) @@ -75,8 +81,45 @@ function(get_pybind11) endfunction() + +# We also want a json-compatible pybind interface: +function(get_pybind11_json) + + message( STATUS #CHECK_START + "Fetching pybind11_json..." ) + #list( APPEND CMAKE_MESSAGE_INDENT " " ) # Indent outputs + + # We want to clone the pybind repo and build it here, during configuration, so we can use it. + # But ExternalProject_add is limited in that it only does its magic during build. + # This is possible in CMake 3.12+ with FetchContent and FetchContent_MakeAvailable in 3.14+ (meaning Ubuntu 20) + # but we need to adhere to CMake 3.10 (Ubuntu 18). + # So instead, we invoke a new CMake project just to download pybind: + configure_file( CMake/pybind11-json-download.cmake.in + ${CMAKE_BINARY_DIR}/external-projects/pybind11-json-download/CMakeLists.txt ) + execute_process( COMMAND "${CMAKE_COMMAND}" -G "${CMAKE_GENERATOR}" . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/external-projects/pybind11-json-download" + OUTPUT_QUIET + RESULT_VARIABLE configure_ret ) + execute_process( COMMAND "${CMAKE_COMMAND}" --build . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/external-projects/pybind11-json-download" + OUTPUT_QUIET + RESULT_VARIABLE build_ret ) + + if( configure_ret OR build_ret ) + message( FATAL_ERROR "Failed to download pybind11_json" ) + endif() + + # pybind11_add_module will add the directory automatically (see below) + + message( STATUS #CHECK_PASS + "Fetching pybind11_json - Done" ) + #list( POP_BACK CMAKE_MESSAGE_INDENT ) # Unindent outputs (requires cmake 3.15) + +endfunction() + # Trigger the build get_pybind11() +get_pybind11_json() # This function overrides "pybind11_add_module" function, arguments is same as "pybind11_add_module" arguments # pybind11_add_module( SHARED [file, file2, ...] ) @@ -93,5 +136,7 @@ function( pybind11_add_module project_name library_type ...) # (workaround for RS5-10582; see also https://github.com/pybind/pybind11/issues/2898) # NOTE: this workaround seems to be needed for debug compilations only target_compile_definitions( ${project_name} PRIVATE -DPYBIND11_COMPILER_TYPE=\"_${project_name}_abi\" ) - + + target_include_directories( ${project_name} PRIVATE "${CMAKE_BINARY_DIR}/third-party/pybind11-json/include" ) + endfunction() diff --git a/CMake/global_config.cmake b/CMake/global_config.cmake index 6292ae4b9e..6fea0025bc 100644 --- a/CMake/global_config.cmake +++ b/CMake/global_config.cmake @@ -80,7 +80,7 @@ macro(global_set_flags) add_definitions(-DCHECK_FOR_UPDATES) endif() endif() - + add_definitions(-D${BACKEND} -DUNICODE) endmacro() @@ -103,6 +103,5 @@ macro(global_target_config) ) - endmacro() diff --git a/CMake/json-download.cmake.in b/CMake/json-download.cmake.in new file mode 100644 index 0000000000..5fd3e71fbd --- /dev/null +++ b/CMake/json-download.cmake.in @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.6) +project(nlohmann-json-download NONE) + +include(ExternalProject) +ExternalProject_Add( + nlohmann_json + PREFIX . + GIT_REPOSITORY "https://github.com/nlohmann/json.git" + #GIT_TAG v3.11.2 + GIT_CONFIG advice.detachedHead=false # otherwise we'll get "You are in 'detached HEAD' state..." + SOURCE_DIR "${CMAKE_BINARY_DIR}/third-party/json" + GIT_SHALLOW 1 # No history needed (requires cmake 3.6) + # Override default steps with no action, we just want the clone step. + CONFIGURE_COMMAND "" + BUILD_COMMAND "" + INSTALL_COMMAND "" + ) + + diff --git a/CMake/lrs_options.cmake b/CMake/lrs_options.cmake index 468cb9e8fd..3cad5b602d 100644 --- a/CMake/lrs_options.cmake +++ b/CMake/lrs_options.cmake @@ -45,4 +45,5 @@ else() option(ENABLE_EASYLOGGINGPP_ASYNC "Switch Logger to Asynchronous Mode (set OFF for Synchronous Mode)" OFF) endif() option(BUILD_PC_STITCHING "Build pointcloud-stitching example" OFF) +option(BUILD_WITH_DDS "Use FastDDS to access camera devices through DDS topics" OFF) diff --git a/CMake/pybind11-json-download.cmake.in b/CMake/pybind11-json-download.cmake.in new file mode 100644 index 0000000000..0710d227de --- /dev/null +++ b/CMake/pybind11-json-download.cmake.in @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.6) +project(pybind11-json-download NONE) + +include(ExternalProject) +ExternalProject_Add( + pybind11_json + PREFIX . + GIT_REPOSITORY "https://github.com/pybind/pybind11_json.git" + GIT_TAG 0.2.13 + GIT_CONFIG advice.detachedHead=false # otherwise we'll get "You are in 'detached HEAD' state..." + SOURCE_DIR "${CMAKE_BINARY_DIR}/third-party/pybind11-json" + GIT_SHALLOW ON # No history needed (requires cmake 3.6) + # Override default steps with no action, we just want the clone step. + CONFIGURE_COMMAND "" + BUILD_COMMAND "" + INSTALL_COMMAND "" + ) + + diff --git a/CMakeLists.txt b/CMakeLists.txt index d12b7b6c22..cb0fbd4e9e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,5 @@ -# minimum required cmake version: 3.1.0 + +# Xenial requires 3.5, Bionic 3.10... cmake_minimum_required(VERSION 3.1.0) set( LRS_TARGET realsense2 ) @@ -59,8 +60,29 @@ include(include/CMakeLists.txt) include(src/CMakeLists.txt) include(third-party/CMakeLists.txt) +include(CMake/external_json.cmake) +target_include_directories( rsutils + PUBLIC + $ + ) + target_link_libraries( ${LRS_TARGET} PUBLIC rsutils ) +if(BUILD_WITH_DDS) + if (CMAKE_SYSTEM MATCHES "Windows" OR CMAKE_SYSTEM MATCHES "Linux") + + message(STATUS "Building with FastDDS") + include(CMake/external_foonathan_memory.cmake) + include(CMake/external_fastdds.cmake) + + target_link_libraries( ${LRS_TARGET} PRIVATE realdds ) + + else() + MESSAGE(STATUS "Turning off `BUILD_WITH_DDS` as it's only supported on Windows & Linux and not on ${CMAKE_SYSTEM}") + set(BUILD_WITH_DDS OFF) + endif() +endif() + # configure the project according to OS specific requirments # macro definition located at "CMake/_config.cmake" os_target_config() @@ -98,3 +120,4 @@ if(IMPORT_DEPTH_CAM_FW) endif() include(CMake/embedd_udev_rules.cmake) + diff --git a/common/device-model.h b/common/device-model.h index 1733fb9cef..18e7d50976 100644 --- a/common/device-model.h +++ b/common/device-model.h @@ -6,7 +6,7 @@ #include #include "notifications.h" #include "realsense-ui-advanced-mode.h" -#include +#include #include "sw-update/dev-updates-profile.h" #include #include "updates-model.h" diff --git a/common/rendering.h b/common/rendering.h index a1c68eee94..2663cb1d22 100644 --- a/common/rendering.h +++ b/common/rendering.h @@ -624,6 +624,18 @@ namespace rs2 } break; } + case RS2_FORMAT_COMBINED_MOTION: + { + auto & motion = *reinterpret_cast< const rs2_combined_motion * >( frame.get_data() ); + draw_motion_data( (float)motion.linear_acceleration.x, + (float)motion.linear_acceleration.y, + (float)motion.linear_acceleration.z ); + draw_motion_data( (float)motion.angular_velocity.x, + (float)motion.angular_velocity.y, + (float)motion.angular_velocity.z, + false ); // Don't clear previous draw + break; + } case RS2_FORMAT_Y16: case RS2_FORMAT_Y10BPACK: glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_LUMINANCE, GL_UNSIGNED_SHORT, data); @@ -799,7 +811,7 @@ namespace rs2 draw_text((int)(xy.x - w / 2), (int)xy.y, text); } - void draw_motion_data(float x, float y, float z) + void draw_motion_data(float x, float y, float z, bool clear=true) { glMatrixMode(GL_PROJECTION); glPushMatrix(); @@ -808,7 +820,8 @@ namespace rs2 glViewport(0, 0, 768, 768); glClearColor(0, 0, 0, 1); - glClear(GL_COLOR_BUFFER_BIT); + if( clear ) + glClear(GL_COLOR_BUFFER_BIT); glMatrixMode(GL_PROJECTION); glLoadIdentity(); @@ -823,11 +836,14 @@ namespace rs2 glRotatef(-135, 0.0f, 1.0f, 0.0f); - draw_axes(); + if( clear ) + { + draw_axes(); - draw_circle(1, 0, 0, 0, 1, 0); - draw_circle(0, 1, 0, 0, 0, 1); - draw_circle(1, 0, 0, 0, 0, 1); + draw_circle( 1, 0, 0, 0, 1, 0 ); + draw_circle( 0, 1, 0, 0, 0, 1 ); + draw_circle( 1, 0, 0, 0, 0, 1 ); + } const auto canvas_size = 230; const auto vec_threshold = 0.2f; diff --git a/common/rs-config.cpp b/common/rs-config.cpp index ade22597c7..53fcd6f178 100644 --- a/common/rs-config.cpp +++ b/common/rs-config.cpp @@ -3,7 +3,7 @@ #include "rs-config.h" -#include "../third-party/json.hpp" +#include #include "model-views.h" @@ -139,4 +139,4 @@ config_file& config_file::operator=(const config_file& other) bool config_file::operator==(const config_file& other) const { return _values == other._values; -} \ No newline at end of file +} diff --git a/common/subdevice-model.h b/common/subdevice-model.h index a6149016be..788a525a24 100644 --- a/common/subdevice-model.h +++ b/common/subdevice-model.h @@ -17,7 +17,7 @@ #include #include -#include "../third-party/json.hpp" +#include #include "objects-in-frame.h" #include "processing-block-model.h" diff --git a/common/sw-update/versions-db-manager.cpp b/common/sw-update/versions-db-manager.cpp index 2f48f8e0fe..3269985aec 100644 --- a/common/sw-update/versions-db-manager.cpp +++ b/common/sw-update/versions-db-manager.cpp @@ -5,7 +5,7 @@ #include #include -#include "json.hpp" +#include #include "versions-db-manager.h" #include #include diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 89cc66eec0..8281fb33cf 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,10 +1,7 @@ # License: Apache 2.0. See LICENSE file in root directory. # Copyright(c) 2019 Intel Corporation. All Rights Reserved. -# minimum required cmake version: 3.1.0 cmake_minimum_required(VERSION 3.1.0) -project(RealsenseExamples) - # Save the command line compile commands in the build output set(CMAKE_EXPORT_COMPILE_COMMANDS 1) # View the makefile commands during build @@ -40,3 +37,4 @@ add_subdirectory(record-playback) add_subdirectory(motion) add_subdirectory(gl) add_subdirectory(hdr) + diff --git a/include/CMakeLists.txt b/include/CMakeLists.txt index eff94cfa96..3d82c53b5a 100644 --- a/include/CMakeLists.txt +++ b/include/CMakeLists.txt @@ -1,6 +1,6 @@ # License: Apache 2.0. See LICENSE file in root directory. # Copyright(c) 2019 Intel Corporation. All Rights Reserved. -target_sources(${LRS_TARGET} +target_sources( ${PROJECT_NAME} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/librealsense2/rs.hpp" "${CMAKE_CURRENT_LIST_DIR}/librealsense2/rs.h" diff --git a/include/librealsense2/h/rs_context.h b/include/librealsense2/h/rs_context.h index c7eb47c3e2..78c1c3d04f 100644 --- a/include/librealsense2/h/rs_context.h +++ b/include/librealsense2/h/rs_context.h @@ -22,6 +22,21 @@ extern "C" { */ rs2_context* rs2_create_context(int api_version, rs2_error** error); +/** +* \brief Creates RealSense context that is required for the rest of the API, and accepts a settings JSON. +* \param[in] api_version Users are expected to pass their version of \c RS2_API_VERSION to make sure they are running the correct librealsense version. +* \param[in] json_settings Pointer to a string containing a JSON configuration to use, or null if none +* Possible settings: +* dds-discovery - (bool) false to disable DDS discovery; defaults to true (requires BUILD_WITH_DDS) +* dds-domain - (int) the number of the domain discovery is on (requires BUILD_WITH_DDS) +* use-basic-formats - (bool) true to make sensors stream only "basic" types without converting to formats +* other then the raw camera formats; defaults to false. +* Convert only interleaved formats (Y8I, Y12I), no colored infrared. +* \param[out] error If non-null, receives any error that occurs during this call, otherwise, errors are ignored. +* \return Context object +*/ +rs2_context* rs2_create_context_ex(int api_version, const char * json_settings, rs2_error** error); + /** * \brief Frees the relevant context object. * \param[in] context Object that is no longer needed @@ -95,6 +110,7 @@ rs2_device_list* rs2_query_devices(const rs2_context* context, rs2_error** error #define RS2_PRODUCT_LINE_SR300 0x04 #define RS2_PRODUCT_LINE_L500 0x08 #define RS2_PRODUCT_LINE_T200 0x10 +#define RS2_PRODUCT_LINE_SW_ONLY 0x100 // enable to return only SW devices, including playback #define RS2_PRODUCT_LINE_DEPTH (RS2_PRODUCT_LINE_L500 | RS2_PRODUCT_LINE_SR300 | RS2_PRODUCT_LINE_D400) #define RS2_PRODUCT_LINE_TRACKING RS2_PRODUCT_LINE_T200 diff --git a/include/librealsense2/h/rs_sensor.h b/include/librealsense2/h/rs_sensor.h index 4f10daf436..ff8b74ce22 100644 --- a/include/librealsense2/h/rs_sensor.h +++ b/include/librealsense2/h/rs_sensor.h @@ -51,6 +51,7 @@ typedef enum rs2_stream RS2_STREAM_GPIO , /**< Signals from external device connected through GPIO */ RS2_STREAM_POSE , /**< 6 Degrees of Freedom pose data, calculated by RealSense device */ RS2_STREAM_CONFIDENCE , /**< 4 bit per-pixel depth confidence level */ + RS2_STREAM_MOTION , /**< Native stream of combined motion data (incl. accel & gyro) */ RS2_STREAM_COUNT } rs2_stream; const char* rs2_stream_to_string(rs2_stream stream); @@ -89,6 +90,7 @@ typedef enum rs2_format RS2_FORMAT_Z16H , /**< DEPRECATED! - Variable-length Huffman-compressed 16-bit depth values. */ RS2_FORMAT_FG , /**< 16-bit per-pixel frame grabber format. */ RS2_FORMAT_Y411 , /**< 12-bit per-pixel. */ + RS2_FORMAT_COMBINED_MOTION , /**< Combined motion data, as in the combined_motion structure */ RS2_FORMAT_COUNT /**< Number of enumeration values. Not a valid input: intended to be used in for-loops. */ } rs2_format; const char* rs2_format_to_string(rs2_format format); @@ -100,6 +102,14 @@ typedef struct rs2_extrinsics float translation[3]; /**< Three-element translation vector, in meters */ } rs2_extrinsics; +/** \brief RS2_STREAM_MOTION / RS2_FORMAT_COMBINED_MOTION content is similar to ROS2's Imu message */ +typedef struct rs2_combined_motion +{ + struct { double x, y, z, w; } orientation; + struct { double x, y, z; } angular_velocity; + struct { double x, y, z; } linear_acceleration; +} rs2_combined_motion; + /** * Deletes sensors list, any sensors created from this list will remain unaffected * \param[in] info_list list to delete diff --git a/include/librealsense2/hpp/rs_context.hpp b/include/librealsense2/hpp/rs_context.hpp index 3d105aa0aa..e1c604add3 100644 --- a/include/librealsense2/hpp/rs_context.hpp +++ b/include/librealsense2/hpp/rs_context.hpp @@ -96,14 +96,24 @@ namespace rs2 class context { public: - context() + enum uninitialized_t { uninitialized }; + context( uninitialized_t ) + { + } + context( char const * json_settings = nullptr ) { rs2_error* e = nullptr; _context = std::shared_ptr( - rs2_create_context(RS2_API_VERSION, &e), + rs2_create_context_ex( RS2_API_VERSION, json_settings, &e ), rs2_delete_context); error::handle(e); } + context( std::string const & json_settings ) + : context( json_settings.c_str() ) + { + } + + operator bool() const { return !! _context; } /** * create a static snapshot of all connected devices at the time of the call diff --git a/include/librealsense2/hpp/rs_device.hpp b/include/librealsense2/hpp/rs_device.hpp index e8acf46411..615636c5b7 100644 --- a/include/librealsense2/hpp/rs_device.hpp +++ b/include/librealsense2/hpp/rs_device.hpp @@ -7,6 +7,7 @@ #include "rs_types.hpp" #include "rs_sensor.hpp" #include +#include namespace rs2 { @@ -117,6 +118,12 @@ namespace rs2 { return _dev; } + bool operator<( device const & other ) const + { + return ( + std::strcmp( get_info( RS2_CAMERA_INFO_SERIAL_NUMBER ), other.get_info( RS2_CAMERA_INFO_SERIAL_NUMBER ) ) + < 0 ); + } template bool is() const diff --git a/scripts/pr_check.sh b/scripts/pr_check.sh index ee7e6ef53e..79e9084db7 100755 --- a/scripts/pr_check.sh +++ b/scripts/pr_check.sh @@ -121,13 +121,14 @@ if [[ $1 == *"help"* ]]; then echo " --fix Try to auto-fix defects" exit 0 fi - + cd .. check_folder CMake $1 check_folder common $1 check_folder include $1 check_folder src $1 check_folder examples $1 +check_folder third-party/realdds $1 check_folder third-party/rsutils $1 check_folder tools $1 cd scripts diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 6081467a60..39af781021 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -139,3 +139,12 @@ target_sources(${LRS_TARGET} "${CMAKE_CURRENT_LIST_DIR}/small-heap.h" "${CMAKE_CURRENT_LIST_DIR}/basics.h" ) + +if(BUILD_WITH_DDS) + file( GLOB_RECURSE RS_DDS_SOURCE_FILES + LIST_DIRECTORIES false + RELATIVE ${PROJECT_SOURCE_DIR} + "${CMAKE_CURRENT_LIST_DIR}/dds/*" + ) + target_sources( ${LRS_TARGET} PRIVATE ${RS_DDS_SOURCE_FILES} ) +endif() diff --git a/src/archive.h b/src/archive.h index 2d09c1f82d..9170bfe9fd 100644 --- a/src/archive.h +++ b/src/archive.h @@ -17,7 +17,7 @@ namespace librealsense public: virtual callback_invocation_holder begin_callback() = 0; - virtual frame_interface* alloc_and_track(const size_t size, const frame_additional_data& additional_data, bool requires_memory) = 0; + virtual frame_interface* alloc_and_track(const size_t size, frame_additional_data && additional_data, bool requires_memory) = 0; virtual std::shared_ptr get_md_parsers() const = 0; diff --git a/src/context.cpp b/src/context.cpp index ead9b376fd..20c6471261 100644 --- a/src/context.cpp +++ b/src/context.cpp @@ -21,44 +21,98 @@ #include "proc/color-formats-converter.h" +#ifdef BUILD_WITH_DDS +#include "dds/rs-dds-device-info.h" + +#include +#include +#include +#include +#include +#include +#include + +// We manage one participant and device-watcher per domain: +// Two contexts with the same domain-id will share the same participant and watcher, while a third context on a +// different domain will have its own. +// +struct dds_domain_context +{ + rsutils::shared_ptr_singleton< realdds::dds_participant > participant; + rsutils::shared_ptr_singleton< realdds::dds_device_watcher > device_watcher; +}; +// +// Domains are mapped by ID: +// Two contexts with the same participant name on different domain-ids are using two different participants! +// +static std::map< realdds::dds_domain_id, dds_domain_context > dds_domain_context_by_id; + +#endif // BUILD_WITH_DDS + +#include +using json = nlohmann::json; + +namespace { + +template< class T > +bool contains( const T & first, const T & second ) +{ + return first == second; +} + template<> -bool contains(const std::shared_ptr& first, - const std::shared_ptr& second) +bool contains( const std::shared_ptr< librealsense::device_info > & first, + const std::shared_ptr< librealsense::device_info > & second ) { auto first_data = first->get_device_data(); auto second_data = second->get_device_data(); - for (auto&& uvc : first_data.uvc_devices) + for( auto && uvc : first_data.uvc_devices ) { - if (std::find(second_data.uvc_devices.begin(), - second_data.uvc_devices.end(), uvc) == - second_data.uvc_devices.end()) + if( std::find( second_data.uvc_devices.begin(), second_data.uvc_devices.end(), uvc ) + == second_data.uvc_devices.end() ) return false; } - for (auto&& usb : first_data.usb_devices) + for( auto && usb : first_data.usb_devices ) { - if (std::find(second_data.usb_devices.begin(), - second_data.usb_devices.end(), usb) == - second_data.usb_devices.end()) + if( std::find( second_data.usb_devices.begin(), second_data.usb_devices.end(), usb ) + == second_data.usb_devices.end() ) return false; } - for (auto&& hid : first_data.hid_devices) + for( auto && hid : first_data.hid_devices ) { - if (std::find(second_data.hid_devices.begin(), - second_data.hid_devices.end(), hid) == - second_data.hid_devices.end()) + if( std::find( second_data.hid_devices.begin(), second_data.hid_devices.end(), hid ) + == second_data.hid_devices.end() ) return false; } - for (auto&& pd : first_data.playback_devices) + for( auto && pd : first_data.playback_devices ) { - if (std::find(second_data.playback_devices.begin(), - second_data.playback_devices.end(), pd) == - second_data.playback_devices.end()) + if( std::find( second_data.playback_devices.begin(), second_data.playback_devices.end(), pd ) + == second_data.playback_devices.end() ) return false; } return true; } +template< class T > +std::vector< std::shared_ptr< T > > subtract_sets( const std::vector< std::shared_ptr< T > > & first, + const std::vector< std::shared_ptr< T > > & second ) +{ + std::vector< std::shared_ptr< T > > results; + std::for_each( first.begin(), first.end(), [&]( std::shared_ptr< T > data ) { + if( std::find_if( second.begin(), + second.end(), + [&]( std::shared_ptr< T > new_dev ) { return contains( data, new_dev ); } ) + == second.end() ) + { + results.push_back( data ); + } + } ); + return results; +} + +} // namespace + namespace librealsense { const std::map platform_color_fourcc_to_rs2_format = { @@ -72,22 +126,92 @@ namespace librealsense {rs_fourcc('M','J','P','G'), RS2_STREAM_COLOR}, }; - context::context( backend_type type ) - : _devices_changed_callback(nullptr, [](rs2_devices_changed_callback*){}) + + context::context() + : _devices_changed_callback( nullptr, []( rs2_devices_changed_callback* ) {} ) { - static bool version_logged=false; - if (!version_logged) + static bool version_logged = false; + if( ! version_logged ) { version_logged = true; LOG_DEBUG( "Librealsense VERSION: " << RS2_API_VERSION_STR ); } + } + + context::context( backend_type type ) + : context() + { _backend = platform::create_backend(); +#ifdef BUILD_WITH_DDS + { + realdds::dds_domain_id domain_id = 0; + auto & domain = dds_domain_context_by_id[domain_id]; + _dds_participant = domain.participant.instance(); + if( ! _dds_participant->is_valid() ) + _dds_participant->init( domain_id, rsutils::os::executable_name() ); + _dds_watcher = domain.device_watcher.instance( _dds_participant ); + } +#endif //BUILD_WITH_DDS + + environment::get_instance().set_time_service(_backend->create_time_service()); + + _device_watcher = _backend->create_device_watcher(); + assert(_device_watcher->is_stopped()); + } + + + context::context( json const & settings ) + : context() + { + _settings = settings; + + _backend = platform::create_backend(); // standard type + + environment::get_instance().set_time_service( _backend->create_time_service() ); + + _device_watcher = _backend->create_device_watcher(); + assert( _device_watcher->is_stopped() ); - environment::get_instance().set_time_service(_backend->create_time_service()); +#ifdef BUILD_WITH_DDS + if( rsutils::json::get< bool >( settings, std::string( "dds-discovery", 13 ), true ) ) + { + realdds::dds_domain_id domain_id + = rsutils::json::get< int >( settings, std::string( "dds-domain", 10 ), 0 ); + std::string participant_name = rsutils::json::get< std::string >( settings, + std::string( "dds-participant-name", 20 ), + rsutils::os::executable_name() ); + + auto & domain = dds_domain_context_by_id[domain_id]; + _dds_participant = domain.participant.instance(); + if( ! _dds_participant->is_valid() ) + { + _dds_participant->init( domain_id, participant_name ); + } + else if( rsutils::json::has_value( settings, std::string( "dds-participant-name", 20 ) ) + && participant_name != _dds_participant->name() ) + { + throw std::runtime_error( + rsutils::string::from() + << "A DDS participant '" << _dds_participant->name() + << "' already exists in domain " << domain_id << "; cannot create '" << participant_name << "'" ); + } + _dds_watcher = domain.device_watcher.instance( _dds_participant ); - _device_watcher = _backend->create_device_watcher(); - assert(_device_watcher->is_stopped()); + // The DDS device watcher should always be on + if( _dds_watcher && _dds_watcher->is_stopped() ) + { + start_dds_device_watcher( + rsutils::json::get< size_t >( settings, std::string( "dds-message-timeout-ms", 22 ), 5000 ) ); + } + } +#endif //BUILD_WITH_DDS + } + + + context::context( char const * json_settings ) + : context( json_settings ? json::parse( json_settings ) : json() ) + { } @@ -276,19 +400,32 @@ namespace librealsense context::~context() { - _device_watcher->stop(); //ensure that the device watcher will stop before the _devices_changed_callback will be deleted + //ensure that the device watchers will stop before the _devices_changed_callback will be deleted + + if ( _device_watcher ) + _device_watcher->stop(); +#ifdef BUILD_WITH_DDS + if( _dds_watcher ) + _dds_watcher->stop(); +#endif //BUILD_WITH_DDS } std::vector> context::query_devices(int mask) const { - - platform::backend_device_group devices(_backend->query_uvc_devices(), _backend->query_usb_devices(), _backend->query_hid_devices()); - return create_devices(devices, _playback_devices, mask); + platform::backend_device_group devices; + if( ! ( mask & RS2_PRODUCT_LINE_SW_ONLY ) ) + { + devices.uvc_devices = _backend->query_uvc_devices(); + devices.usb_devices = _backend->query_usb_devices(); + devices.hid_devices = _backend->query_hid_devices(); + } + return create_devices( devices, _playback_devices, mask ); } - std::vector> context::create_devices(platform::backend_device_group devices, - const std::map>& playback_devices, - int mask) const + std::vector< std::shared_ptr< device_info > > + context::create_devices( platform::backend_device_group devices, + const std::map< std::string, std::weak_ptr< device_info > > & playback_devices, + int mask ) const { std::vector> list; @@ -296,12 +433,48 @@ namespace librealsense // to allow them to modify context later on auto ctx = t->shared_from_this(); - if (mask & RS2_PRODUCT_LINE_D400) + if( mask & RS2_PRODUCT_LINE_D400 ) { auto d400_devices = d400_info::pick_d400_devices(ctx, devices); std::copy(begin(d400_devices), end(d400_devices), std::back_inserter(list)); } +#ifdef BUILD_WITH_DDS + if( _dds_watcher ) + _dds_watcher->foreach_device( + [&]( std::shared_ptr< realdds::dds_device > const & dev ) -> bool + { + if( ! dev->is_running() ) + { + LOG_DEBUG( "device '" << dev->device_info().topic_root << "' is not yet running" ); + return true; + } + if( dev->device_info().product_line == "D400" ) + { + if( ! ( mask & RS2_PRODUCT_LINE_D400 ) ) + return true; + } + else if( dev->device_info().product_line == "L500" ) + { + if( ! ( mask & RS2_PRODUCT_LINE_L500 ) ) + return true; + } + else if( dev->device_info().product_line == "SR300" ) + { + if( ! ( mask & RS2_PRODUCT_LINE_SR300 ) ) + return true; + } + else if( ! ( mask & RS2_PRODUCT_LINE_NON_INTEL ) ) + { + return true; + } + + std::shared_ptr< device_info > info = std::make_shared< dds_device_info >( ctx, dev ); + list.push_back( info ); + return true; + } ); +#endif //BUILD_WITH_DDS + // Supported recovery devices if (mask & RS2_PRODUCT_LINE_D400) { @@ -309,7 +482,7 @@ namespace librealsense std::copy(begin(recovery_devices), end(recovery_devices), std::back_inserter(list)); } - if (mask & RS2_PRODUCT_LINE_NON_INTEL) + if( mask & RS2_PRODUCT_LINE_NON_INTEL ) { auto uvc_devices = platform_camera_info::pick_uvc_devices(ctx, devices.uvc_devices); std::copy(begin(uvc_devices), end(uvc_devices), std::back_inserter(list)); @@ -322,7 +495,7 @@ namespace librealsense } if (list.size()) - LOG_INFO( "Found " << list.size() << " RealSense devices (mask 0x" << std::hex << mask << ")" ); + LOG_INFO( "Found " << list.size() << " RealSense devices (mask 0x" << std::hex << mask << ")" << std::dec ); return list; } @@ -355,28 +528,35 @@ namespace librealsense LOG_DEBUG("\nDevice connected:\n\n" << std::string(devices_info_added[i]->get_device_data())); } - std::map devices_changed_callbacks; + invoke_devices_changed_callbacks( rs2_devices_info_removed, rs2_devices_info_added ); + } + } + + void context::invoke_devices_changed_callbacks( std::vector & rs2_devices_info_removed, + std::vector & rs2_devices_info_added ) + { + std::map devices_changed_callbacks; + { + std::lock_guard lock( _devices_changed_callbacks_mtx ); + devices_changed_callbacks = _devices_changed_callbacks; + } + + for( auto & kvp : devices_changed_callbacks ) + { + try { - std::lock_guard lock(_devices_changed_callbacks_mtx); - devices_changed_callbacks = _devices_changed_callbacks; + kvp.second->on_devices_changed( new rs2_device_list( { shared_from_this(), rs2_devices_info_removed } ), + new rs2_device_list( { shared_from_this(), rs2_devices_info_added } ) ); } - - for (auto& kvp : devices_changed_callbacks) + catch( ... ) { - try - { - kvp.second->on_devices_changed(new rs2_device_list({ shared_from_this(), rs2_devices_info_removed }), - new rs2_device_list({ shared_from_this(), rs2_devices_info_added })); - } - catch (...) - { - LOG_ERROR("Exception thrown from user callback handler"); - } + LOG_ERROR( "Exception thrown from user callback handler" ); } - - raise_devices_changed(rs2_devices_info_removed, rs2_devices_info_added); } + + raise_devices_changed( rs2_devices_info_removed, rs2_devices_info_added ); } + void context::raise_devices_changed(const std::vector& removed, const std::vector& added) { if (_devices_changed_callback) @@ -401,6 +581,29 @@ namespace librealsense }); } +#ifdef BUILD_WITH_DDS + void context::start_dds_device_watcher( size_t message_timeout_ms ) + { + _dds_watcher->on_device_added( [this, message_timeout_ms]( std::shared_ptr< realdds::dds_device > const & dev ) { + dev->run( message_timeout_ms ); + + std::vector rs2_device_info_added; + std::vector rs2_device_info_removed; + std::shared_ptr< device_info > info = std::make_shared< dds_device_info >( shared_from_this(), dev ); + rs2_device_info_added.push_back( { shared_from_this(), info } ); + invoke_devices_changed_callbacks( rs2_device_info_removed, rs2_device_info_added ); + } ); + _dds_watcher->on_device_removed( [this]( std::shared_ptr< realdds::dds_device > const & dev ) { + std::vector rs2_device_info_added; + std::vector rs2_device_info_removed; + std::shared_ptr< device_info > info = std::make_shared< dds_device_info >( shared_from_this(), dev ); + rs2_device_info_removed.push_back( { shared_from_this(), info } ); + invoke_devices_changed_callbacks( rs2_device_info_removed, rs2_device_info_added ); + } ); + _dds_watcher->start(); + } +#endif //BUILD_WITH_DDS + uint64_t context::register_internal_device_callback(devices_changed_callback_ptr callback) { std::lock_guard lock(_devices_changed_callbacks_mtx); @@ -411,7 +614,6 @@ namespace librealsense { start_device_watcher(); } - return callback_id; } @@ -506,7 +708,7 @@ namespace librealsense auto prev_playback_devices = _playback_devices; _playback_devices[file] = dinfo; on_device_changed({}, {}, prev_playback_devices, _playback_devices); - return std::move(dinfo); + return dinfo; } void context::add_software_device(std::shared_ptr dev) diff --git a/src/context.h b/src/context.h index d454666dde..e40f0dbd3f 100644 --- a/src/context.h +++ b/src/context.h @@ -8,6 +8,7 @@ #include "core/streaming.h" #include +#include #include "media/playback/playback_device.h" namespace librealsense @@ -34,6 +35,15 @@ struct rs2_stream_profile std::shared_ptr clone; }; + +#ifdef BUILD_WITH_DDS +namespace realdds { + class dds_device_watcher; + class dds_participant; +} // namespace realdds +#endif + + namespace librealsense { class device; @@ -102,9 +112,13 @@ namespace librealsense class context : public std::enable_shared_from_this { + context(); public: explicit context( backend_type type ); + explicit context( nlohmann::json const & ); + explicit context( char const * json_settings ); + void stop() { _device_watcher->stop(); } ~context(); std::vector> query_devices(int mask) const; @@ -121,20 +135,31 @@ namespace librealsense void remove_device(const std::string& file); void add_software_device(std::shared_ptr software_device); + + const nlohmann::json & get_settings() const { return _settings; } private: void on_device_changed(platform::backend_device_group old, platform::backend_device_group curr, const std::map>& old_playback_devices, const std::map>& new_playback_devices); + void invoke_devices_changed_callbacks( std::vector & rs2_devices_info_removed, + std::vector & rs2_devices_info_added ); void raise_devices_changed(const std::vector& removed, const std::vector& added); void start_device_watcher(); - std::shared_ptr _backend; std::shared_ptr _device_watcher; std::map> _playback_devices; std::map _devices_changed_callbacks; +#ifdef BUILD_WITH_DDS + std::shared_ptr< realdds::dds_participant > _dds_participant; + std::shared_ptr< realdds::dds_device_watcher > _dds_watcher; + + void start_dds_device_watcher( size_t message_timeout_ms ); +#endif + + nlohmann::json _settings; // Save operation settings devices_changed_callback_ptr _devices_changed_callback; std::map> _streams; diff --git a/src/dds/rs-dds-color-sensor-proxy.h b/src/dds/rs-dds-color-sensor-proxy.h new file mode 100644 index 0000000000..c9b9806af7 --- /dev/null +++ b/src/dds/rs-dds-color-sensor-proxy.h @@ -0,0 +1,26 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "rs-dds-sensor-proxy.h" + + +namespace librealsense { + +// For cases when checking if this is< color_sensor > (like realsense-viewer::subdevice_model) +class dds_color_sensor_proxy + : public dds_sensor_proxy + , public color_sensor +{ +public: + dds_color_sensor_proxy( std::string const & sensor_name, + software_device * owner, + std::shared_ptr< realdds::dds_device > const & dev ) + : dds_sensor_proxy( sensor_name, owner, dev ) + { + } +}; + + +} // namespace librealsense diff --git a/src/dds/rs-dds-depth-sensor-proxy.h b/src/dds/rs-dds-depth-sensor-proxy.h new file mode 100644 index 0000000000..c5d4079983 --- /dev/null +++ b/src/dds/rs-dds-depth-sensor-proxy.h @@ -0,0 +1,39 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "rs-dds-sensor-proxy.h" + + +namespace librealsense { + +// For cases when checking if this is< depth_sensor > (like realsense-viewer::subdevice_model) +class dds_depth_sensor_proxy + : public dds_sensor_proxy + , public depth_sensor +{ +public: + dds_depth_sensor_proxy( std::string const & sensor_name, + software_device * owner, + std::shared_ptr< realdds::dds_device > const & dev ) + : dds_sensor_proxy( sensor_name, owner, dev ) + { + } + + // Needed by abstract interfaces + float get_depth_scale() const override { return get_option( RS2_OPTION_DEPTH_UNITS ).query(); } + + void create_snapshot( std::shared_ptr< depth_sensor > & snapshot ) const override + { + snapshot = std::make_shared< depth_sensor_snapshot >( get_depth_scale() ); + } + + void enable_recording( std::function< void( const depth_sensor & ) > recording_function ) override + { + // does not change over time + } +}; + + +} // namespace librealsense diff --git a/src/dds/rs-dds-device-info.cpp b/src/dds/rs-dds-device-info.cpp new file mode 100644 index 0000000000..2762fb8b0e --- /dev/null +++ b/src/dds/rs-dds-device-info.cpp @@ -0,0 +1,27 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. + +#include "rs-dds-device-info.h" +#include "rs-dds-device-proxy.h" + +#include +#include + + +namespace librealsense { + + +std::shared_ptr< device_interface > dds_device_info::create( std::shared_ptr< context > ctx, + bool register_device_notifications ) const +{ + return std::make_shared< dds_device_proxy >( ctx, _dev ); +} + + +platform::backend_device_group dds_device_info::get_device_data() const +{ + return platform::backend_device_group{ { platform::playback_device_info{ _dev->device_info().topic_root } } }; +} + + +} // namespace librealsense diff --git a/src/dds/rs-dds-device-info.h b/src/dds/rs-dds-device-info.h new file mode 100644 index 0000000000..c54ed21c68 --- /dev/null +++ b/src/dds/rs-dds-device-info.h @@ -0,0 +1,46 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. + +#pragma once + +#include // device_info, context + +#include + + +namespace realdds { +class dds_device; +} // namespace realdds + + +namespace librealsense { + + +class dds_device_proxy; +class device_interface; + + +// Factory class for dds_device_proxy. +// +// This is what's created when context::query_devices() is used, or when a watcher detects a new DDS device: it just +// points to a (possibly not running) DDS device that is only instantiated when create() is called. +// +class dds_device_info : public device_info +{ + std::shared_ptr< realdds::dds_device > _dev; + +public: + dds_device_info( std::shared_ptr< context > const & ctx, std::shared_ptr< realdds::dds_device > const & dev ) + : device_info( ctx ) + , _dev( dev ) + { + } + + std::shared_ptr< device_interface > create( std::shared_ptr< context > ctx, + bool register_device_notifications ) const override; + + platform::backend_device_group get_device_data() const override; +}; + + +} // namespace librealsense diff --git a/src/dds/rs-dds-device-proxy.cpp b/src/dds/rs-dds-device-proxy.cpp new file mode 100644 index 0000000000..039bd3e230 --- /dev/null +++ b/src/dds/rs-dds-device-proxy.cpp @@ -0,0 +1,416 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. + +#include "rs-dds-device-proxy.h" +#include "rs-dds-color-sensor-proxy.h" +#include "rs-dds-depth-sensor-proxy.h" + +#include +#include +#include + +#include + +#include +#include + +#include + + +namespace librealsense { + + +// Constants for Json lookups +static const std::string stream_name_key( "stream-name", 11 ); + + +static rs2_stream to_rs2_stream_type( std::string const & type_string ) +{ + static const std::map< std::string, rs2_stream > type_to_rs2 = { + { "depth", RS2_STREAM_DEPTH }, + { "color", RS2_STREAM_COLOR }, + { "ir", RS2_STREAM_INFRARED }, + { "motion", RS2_STREAM_MOTION }, + { "confidence", RS2_STREAM_CONFIDENCE }, + }; + auto it = type_to_rs2.find( type_string ); + if( it == type_to_rs2.end() ) + throw invalid_value_exception( "unknown stream type '" + type_string + "'" ); + + return it->second; +} + + +static rs2_video_stream to_rs2_video_stream( rs2_stream const stream_type, + sid_index const & sidx, + std::shared_ptr< realdds::dds_video_stream_profile > const & profile, + const std::set< realdds::video_intrinsics > & intrinsics ) +{ + rs2_video_stream prof = {}; + prof.type = stream_type; + prof.index = sidx.index; + prof.uid = sidx.sid; + prof.width = profile->width(); + prof.height = profile->height(); + prof.fps = profile->frequency(); + prof.fmt = static_cast< rs2_format >( profile->format().to_rs2() ); + + // Handle intrinsics + auto intr = std::find_if( intrinsics.begin(), + intrinsics.end(), + [profile]( const realdds::video_intrinsics & intr ) + { return profile->width() == intr.width && profile->height() == intr.height; } ); + if( intr != intrinsics.end() ) // Some profiles don't have intrinsics + { + prof.intrinsics.width = intr->width; + prof.intrinsics.height = intr->height; + prof.intrinsics.ppx = intr->principal_point_x; + prof.intrinsics.ppy = intr->principal_point_y; + prof.intrinsics.fx = intr->focal_lenght_x; + prof.intrinsics.fy = intr->focal_lenght_y; + prof.intrinsics.model = static_cast< rs2_distortion >( intr->distortion_model ); + memcpy( prof.intrinsics.coeffs, intr->distortion_coeffs.data(), sizeof( prof.intrinsics.coeffs ) ); + } + + return prof; +} + + +static rs2_motion_stream to_rs2_motion_stream( rs2_stream const stream_type, + sid_index const & sidx, + std::shared_ptr< realdds::dds_motion_stream_profile > const & profile, + const realdds::motion_intrinsics & intrinsics ) +{ + rs2_motion_stream prof; + prof.type = stream_type; + prof.index = sidx.index; + prof.uid = sidx.sid; + prof.fps = profile->frequency(); + prof.fmt = RS2_FORMAT_COMBINED_MOTION; + + memcpy( prof.intrinsics.data, intrinsics.data.data(), sizeof( prof.intrinsics.data ) ); + memcpy( prof.intrinsics.noise_variances, + intrinsics.noise_variances.data(), + sizeof( prof.intrinsics.noise_variances ) ); + memcpy( prof.intrinsics.bias_variances, + intrinsics.bias_variances.data(), + sizeof( prof.intrinsics.bias_variances ) ); + + return prof; +} + + +static rs2_extrinsics to_rs2_extrinsics( const std::shared_ptr< realdds::extrinsics > & dds_extrinsics ) +{ + rs2_extrinsics rs2_extr; + + memcpy( rs2_extr.rotation, dds_extrinsics->rotation.data(), sizeof( rs2_extr.rotation ) ); + memcpy( rs2_extr.translation, dds_extrinsics->translation.data(), sizeof( rs2_extr.translation ) ); + + return rs2_extr; +} + + +dds_device_proxy::dds_device_proxy( std::shared_ptr< context > ctx, std::shared_ptr< realdds::dds_device > const & dev ) + : software_device( ctx ) + , _dds_dev( dev ) +{ + 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_SERIAL_NUMBER, dev->device_info().serial ); + register_info( RS2_CAMERA_INFO_PRODUCT_LINE, dev->device_info().product_line ); + register_info( RS2_CAMERA_INFO_PRODUCT_ID, dev->device_info().product_id ); + register_info( RS2_CAMERA_INFO_PHYSICAL_PORT, dev->device_info().topic_root ); + register_info( RS2_CAMERA_INFO_CAMERA_LOCKED, dev->device_info().locked ? "YES" : "NO" ); + + // Assumes dds_device initialization finished + struct sensor_info + { + std::shared_ptr< dds_sensor_proxy > proxy; + int sensor_index = 0; + }; + std::map< std::string, sensor_info > sensor_name_to_info; + + // 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 ) + { + auto & sensor_info = sensor_name_to_info[stream->sensor_name()]; + if( ! sensor_info.proxy ) + { + // This is a new sensor we haven't seen yet + sensor_info.proxy = create_sensor( stream->sensor_name() ); + sensor_info.sensor_index = add_sensor( sensor_info.proxy ); + assert( sensor_info.sensor_index == _software_sensors.size() ); + _software_sensors.push_back( sensor_info.proxy ); + } + auto stream_type = to_rs2_stream_type( stream->type_string() ); + int index = get_index_from_stream_name( stream->name() ); + sid_index sidx( environment::get_instance().generate_stream_id(), index); + sid_index type_and_index( stream_type, index ); + _stream_name_to_librs_stream[stream->name()] + = 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() << " " << stream->sensor_name() << " : " << stream->name() ); + + software_sensor & sensor = get_software_sensor( sensor_info.sensor_index ); + auto video_stream = std::dynamic_pointer_cast< realdds::dds_video_stream >( stream ); + auto motion_stream = std::dynamic_pointer_cast< realdds::dds_motion_stream >( stream ); + auto & profiles = stream->profiles(); + auto const & default_profile = profiles[stream->default_profile_index()]; + for( auto & profile : profiles ) + { + LOG_DEBUG( " " << profile->details_to_string() ); + if( video_stream ) + { + auto video_profile = std::static_pointer_cast< realdds::dds_video_stream_profile >( profile ); + auto raw_stream_profile = sensor.add_video_stream( + to_rs2_video_stream( stream_type, sidx, video_profile, video_stream->get_intrinsics() ), + profile == default_profile ); + _stream_name_to_profiles[stream->name()].push_back( raw_stream_profile ); + } + else if( motion_stream ) + { + auto motion_profile = std::static_pointer_cast< realdds::dds_motion_stream_profile >( profile ); + auto raw_stream_profile = sensor.add_motion_stream( + to_rs2_motion_stream( stream_type, sidx, motion_profile, motion_stream->get_gyro_intrinsics() ), + profile == default_profile ); + _stream_name_to_profiles[stream->name()].push_back( raw_stream_profile ); + } + // NOTE: the raw profile will be cloned and overriden by the format converter! + } + + auto & options = stream->options(); + for( auto & option : options ) + { + sensor_info.proxy->add_option( option ); + } + + auto & recommended_filters = stream->recommended_filters(); + for( auto & filter_name : recommended_filters ) + { + sensor_info.proxy->add_processing_block( filter_name ); + } + } ); // End foreach_stream lambda + + for( auto & sensor_info : sensor_name_to_info ) + { + LOG_DEBUG( sensor_info.first ); + sensor_info.second.proxy->initialization_done(); + + // 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() ) + { + 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 stream_iter = streams.find( sidx ); + if( stream_iter == streams.end() ) + { + LOG_DEBUG( " no dds stream" ); + continue; + } + + 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. + // The raw profile intrinsics will be overriden to call the target's intrinsics function (which by default + // calls the raw again, creating an infinite loop), so we must override the target: + set_profile_intrinsics( profile, stream_iter->second ); + + _stream_name_to_profiles.at( stream_iter->second->name() ).push_back( profile ); // For extrinsics + + tag_default_profile_of_stream( profile, stream_iter->second ); + } + } + + if( _dds_dev->supports_metadata() ) + { + _dds_dev->on_metadata_available( + [this]( nlohmann::json && dds_md ) + { + std::string stream_name = rsutils::json::get< std::string >( dds_md, stream_name_key ); + auto it = _stream_name_to_owning_sensor.find( stream_name ); + if( it != _stream_name_to_owning_sensor.end() ) + it->second->handle_new_metadata( stream_name, std::move( dds_md ) ); + } ); + } + + // According to extrinsics_graph (in environment.h) we need 3 steps: + + // 1. Register streams with extrinsics between them + if( _dds_dev->has_extrinsics() ) + { + for( auto & from_stream : _stream_name_to_librs_stream ) + { + for( auto & to_stream : _stream_name_to_librs_stream ) + { + if( from_stream.first != to_stream.first ) + { + 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 ); + continue; + } + rs2_extrinsics extr = to_rs2_extrinsics( dds_extr ); + environment::get_instance().get_extrinsics_graph().register_extrinsics( *from_stream.second, + *to_stream.second, + extr ); + } + } + } + } + + // 2. Register all profiles + for( auto & it : _stream_name_to_profiles ) + { + for( auto profile : it.second ) + { + environment::get_instance().get_extrinsics_graph().register_profile( *profile ); + } + } + + // 3. Link profile to it's stream + for( auto & it : _stream_name_to_librs_stream ) + { + for( auto & profile : _stream_name_to_profiles[it.first] ) + { + environment::get_instance().get_extrinsics_graph().register_same_extrinsics( *it.second, *profile ); + } + } + // TODO - need to register extrinsics group in dev? +} + + +int dds_device_proxy::get_index_from_stream_name( const std::string & name ) const +{ + int index = 0; + + auto delimiter_pos = name.find( '_' ); + if( delimiter_pos != std::string::npos ) + { + std::string index_as_string = name.substr( delimiter_pos + 1, name.size() ); + index = std::stoi( index_as_string ); + } + + return index; +} + + +void dds_device_proxy::set_profile_intrinsics( std::shared_ptr< stream_profile_interface > & profile, + const std::shared_ptr< realdds::dds_stream > & stream ) const +{ + if( auto video_stream = std::dynamic_pointer_cast< realdds::dds_video_stream >( stream ) ) + { + set_video_profile_intrinsics( profile, video_stream ); + } + else if( auto motion_stream = std::dynamic_pointer_cast< realdds::dds_motion_stream >( stream ) ) + { + set_motion_profile_intrinsics( profile, motion_stream ); + } +} + + +void dds_device_proxy::set_video_profile_intrinsics( std::shared_ptr< stream_profile_interface > profile, + std::shared_ptr< realdds::dds_video_stream > stream ) const +{ + auto vsp = std::dynamic_pointer_cast< video_stream_profile >( profile ); + auto & stream_intrinsics = stream->get_intrinsics(); + auto it = std::find_if( stream_intrinsics.begin(), + stream_intrinsics.end(), + [vsp]( const realdds::video_intrinsics & intr ) + { return vsp->get_width() == intr.width && vsp->get_height() == intr.height; } ); + + if( it != stream_intrinsics.end() ) // Some profiles don't have intrinsics + { + rs2_intrinsics intr; + intr.width = it->width; + intr.height = it->height; + intr.ppx = it->principal_point_x; + intr.ppy = it->principal_point_y; + intr.fx = it->focal_lenght_x; + intr.fy = it->focal_lenght_y; + intr.model = static_cast< rs2_distortion >( it->distortion_model ); + memcpy( intr.coeffs, it->distortion_coeffs.data(), sizeof( intr.coeffs ) ); + vsp->set_intrinsics( [intr]() { return intr; } ); + } +} + + +void dds_device_proxy::set_motion_profile_intrinsics( std::shared_ptr< stream_profile_interface > profile, + std::shared_ptr< realdds::dds_motion_stream > stream ) const +{ + auto msp = std::dynamic_pointer_cast< motion_stream_profile >( profile ); + auto & stream_intrinsics = stream->get_gyro_intrinsics(); + rs2_motion_device_intrinsic intr; + memcpy( intr.data, stream_intrinsics.data.data(), sizeof( intr.data ) ); + memcpy( intr.noise_variances, stream_intrinsics.noise_variances.data(), sizeof( intr.noise_variances ) ); + memcpy( intr.bias_variances, stream_intrinsics.bias_variances.data(), sizeof( intr.bias_variances ) ); + msp->set_intrinsics( [intr]() { return intr; } ); +} + + +std::shared_ptr< dds_sensor_proxy > dds_device_proxy::create_sensor( const std::string & sensor_name ) +{ + if( sensor_name.compare( "RGB Camera" ) == 0 ) + return std::make_shared< dds_color_sensor_proxy >( sensor_name, this, _dds_dev ); + else if( sensor_name.compare( "Stereo Module" ) == 0 ) + return std::make_shared< dds_depth_sensor_proxy >( sensor_name, this, _dds_dev ); + + return std::make_shared< dds_sensor_proxy >( sensor_name, this, _dds_dev ); +} + + +// Tagging converted profiles. dds_sensor_proxy::add_video/motion_stream tagged the raw profiles. +void dds_device_proxy::tag_default_profile_of_stream( + const std::shared_ptr< stream_profile_interface > & profile, + const std::shared_ptr< const realdds::dds_stream > & stream ) const +{ + auto const & dds_default_profile = stream->default_profile(); + + if( profile->get_stream_type() == to_rs2_stream_type( stream->type_string() ) && + profile->get_framerate() == dds_default_profile->frequency() ) + { + auto vsp = std::dynamic_pointer_cast< video_stream_profile >( profile ); + auto dds_vsp = std::dynamic_pointer_cast< realdds::dds_video_stream_profile >( dds_default_profile ); + if( vsp && dds_vsp ) + { + if( vsp->get_width() != dds_vsp->width() || vsp->get_height() != dds_vsp->height() + || vsp->get_format() != dds_vsp->format().to_rs2() ) + return; // Video profiles of incompatible resolutions + } + + profile->tag_profile( PROFILE_TAG_DEFAULT ); + } +} + + +void dds_device_proxy::tag_profiles( stream_profiles profiles ) const +{ + //Do nothing. PROFILE_TAG_DEFAULT is already added in tag_default_profile_of_stream. +} + + +} // namespace librealsense \ No newline at end of file diff --git a/src/dds/rs-dds-device-proxy.h b/src/dds/rs-dds-device-proxy.h new file mode 100644 index 0000000000..f74ca771c4 --- /dev/null +++ b/src/dds/rs-dds-device-proxy.h @@ -0,0 +1,64 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. + +#pragma once + +#include +#include "sid_index.h" + +#include +#include + + +namespace realdds { +class dds_device; +class dds_stream; +class dds_video_stream; +class dds_motion_stream; +class dds_stream_profile; +} // namespace realdds + + +namespace librealsense { + + +class stream; +class dds_sensor_proxy; +class stream_profile_interface; + + +// This is the rs2 device; it proxies to an actual DDS device that does all the actual +// work. For example: +// auto dev_list = ctx.query_devices(); +// auto dev1 = dev_list[0]; +// auto dev2 = dev_list[0]; +// dev1 and dev2 are two different rs2 devices, but they both go to the same DDS source! +// +class dds_device_proxy : public software_device +{ + std::shared_ptr< realdds::dds_device > _dds_dev; + std::map< std::string, std::vector< std::shared_ptr< stream_profile_interface > > > _stream_name_to_profiles; + std::map< std::string, std::shared_ptr< librealsense::stream > > _stream_name_to_librs_stream; + std::map< std::string, std::shared_ptr< dds_sensor_proxy > > _stream_name_to_owning_sensor; + + int get_index_from_stream_name( const std::string & name ) const; + void set_profile_intrinsics( std::shared_ptr< stream_profile_interface > & profile, + const std::shared_ptr< realdds::dds_stream > & stream ) const; + void set_video_profile_intrinsics( std::shared_ptr< stream_profile_interface > profile, + std::shared_ptr< realdds::dds_video_stream > stream ) const; + void set_motion_profile_intrinsics( std::shared_ptr< stream_profile_interface > profile, + std::shared_ptr< realdds::dds_motion_stream > stream ) const; + +public: + dds_device_proxy( std::shared_ptr< context > ctx, std::shared_ptr< realdds::dds_device > const & dev ); + + void tag_default_profile_of_stream( const std::shared_ptr< stream_profile_interface > & profile, + const std::shared_ptr< const realdds::dds_stream > & stream ) const; + + std::shared_ptr< dds_sensor_proxy > create_sensor( const std::string & sensor_name ); + + void tag_profiles( stream_profiles profiles ) const override; +}; + + +} // namespace librealsense \ No newline at end of file diff --git a/src/dds/rs-dds-internal-data.cpp b/src/dds/rs-dds-internal-data.cpp new file mode 100644 index 0000000000..c7b6a72cc4 --- /dev/null +++ b/src/dds/rs-dds-internal-data.cpp @@ -0,0 +1,178 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. + +#include "rs-dds-internal-data.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace librealsense { + +static const std::string RS405_PID = "0B5B"; +static const std::string RS415_PID = "0AD3"; +static const std::string RS435_RGB_PID = "0B07"; +static const std::string RS435I_PID = "0B3A"; +static const std::string RS455_PID = "0B5C"; +static const std::string RS457_PID = "ABCD"; + + +std::vector< rs2_format > target_formats( rs2_format source_format ) +{ + // Mapping from source color format to all of the compatible target color formats. + std::vector< rs2_format > formats = { RS2_FORMAT_RGB8, RS2_FORMAT_RGBA8, RS2_FORMAT_BGR8, RS2_FORMAT_BGRA8 }; + + switch( source_format ) + { + case RS2_FORMAT_YUYV: + formats.push_back( RS2_FORMAT_YUYV ); + formats.push_back( RS2_FORMAT_Y16 ); + formats.push_back( RS2_FORMAT_Y8 ); + break; + case RS2_FORMAT_UYVY: + formats.push_back( RS2_FORMAT_UYVY ); + break; + default: + throw std::runtime_error( "Format is not supported for mapping" ); + } + + return formats; +} + +std::vector< tagged_profile > dds_rs_internal_data::get_profiles_tags( const std::string & product_id, + const std::string & product_line ) +{ + std::vector< tagged_profile > tags; + + if( product_id == RS405_PID ) + { + tags.push_back( { RS2_STREAM_COLOR, -1, 848, 480, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + tags.push_back( { RS2_STREAM_DEPTH, -1, 848, 480, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + tags.push_back( { RS2_STREAM_INFRARED, -1, 848, 480, RS2_FORMAT_Y8, 30, profile_tag::PROFILE_TAG_SUPERSET } ); + } + else if( product_id == RS415_PID ) + { + tags.push_back( { RS2_STREAM_COLOR, -1, 1280, 720, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + tags.push_back( { RS2_STREAM_DEPTH, -1, 1280, 720, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + tags.push_back( { RS2_STREAM_INFRARED, -1, 1280, 720, RS2_FORMAT_Y8, 30, profile_tag::PROFILE_TAG_SUPERSET } ); + } + else if( product_id == RS435_RGB_PID ) + { + tags.push_back( { RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + tags.push_back( { RS2_STREAM_DEPTH, -1, 848, 480, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + tags.push_back( { RS2_STREAM_INFRARED, -1, 848, 480, RS2_FORMAT_Y8, 30, profile_tag::PROFILE_TAG_SUPERSET } ); + } + else if( product_id == RS435I_PID ) + { + tags.push_back( { RS2_STREAM_COLOR, -1, 1280, 720, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + tags.push_back( { RS2_STREAM_DEPTH, -1, 848, 480, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + tags.push_back( { RS2_STREAM_INFRARED, -1, 848, 480, RS2_FORMAT_Y8, 30, profile_tag::PROFILE_TAG_SUPERSET } ); + tags.push_back( { RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + tags.push_back( { RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 63, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + tags.push_back( { RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 100, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + } + else if( product_id == RS455_PID ) + { + tags.push_back( { RS2_STREAM_COLOR, -1, 1280, 720, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + tags.push_back( { RS2_STREAM_DEPTH, -1, 848, 480, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + tags.push_back( { RS2_STREAM_INFRARED, -1, 848, 480, RS2_FORMAT_Y8, 30, profile_tag::PROFILE_TAG_SUPERSET } ); + tags.push_back( { RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + tags.push_back( { RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 63, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + tags.push_back( { RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 100, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + } + else if( product_id == RS457_PID ) + { + tags.push_back( { RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + tags.push_back( { RS2_STREAM_DEPTH, -1, 640, 480, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + } + else if( product_line == "D400" ) + { + tags.push_back( { RS2_STREAM_DEPTH, -1, 1280, 720, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + tags.push_back( { RS2_STREAM_INFRARED, 1, 1280, 720, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); + tags.push_back( { RS2_STREAM_INFRARED, 2, 1280, 720, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET } ); + } + + // For other devices empty tags will be returned, no default profile defined. + + return tags; +} + + +std::vector< processing_block_factory > dds_rs_internal_data::get_profile_converters( const std::string & product_id, + const std::string & product_line ) +{ + std::vector< processing_block_factory > factories; + std::vector< processing_block_factory > tmp; + + if( product_line == "D400" ) + { + if( product_id == RS457_PID ) + { + tmp = processing_block_factory::create_pbf_vector< uyvy_converter >( RS2_FORMAT_UYVY, + target_formats( RS2_FORMAT_UYVY ), + RS2_STREAM_COLOR ); + for( auto & it : tmp ) + factories.push_back( std::move( it ) ); + tmp = processing_block_factory::create_pbf_vector< uyvy_converter >( RS2_FORMAT_YUYV, + target_formats( RS2_FORMAT_YUYV ), + RS2_STREAM_COLOR ); + for( auto & it : tmp ) + factories.push_back( std::move( it ) ); + factories.push_back( { { { RS2_FORMAT_Y12I } }, + { { RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 1 }, + { RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 2 } }, + []() { return std::make_shared< y12i_to_y16y16_mipi >(); } } ); + } + else + { + tmp = processing_block_factory::create_pbf_vector< yuy2_converter >( RS2_FORMAT_YUYV, + target_formats( RS2_FORMAT_YUYV ), + RS2_STREAM_COLOR ); + for( auto & it : tmp ) + factories.push_back( std::move( it ) ); + factories.push_back( processing_block_factory::create_id_pbf( RS2_FORMAT_RAW16, RS2_STREAM_COLOR ) ); + factories.push_back( { { { RS2_FORMAT_Y12I } }, + { { RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 1 }, + { RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 2 } }, + []() { return std::make_shared< y12i_to_y16y16 >(); } } ); + } + tmp = processing_block_factory::create_pbf_vector< uyvy_converter >( + RS2_FORMAT_UYVY, + target_formats( RS2_FORMAT_UYVY ), + RS2_STREAM_INFRARED ); + for( auto & it : tmp ) + factories.push_back( std::move( it ) ); + factories.push_back( processing_block_factory::create_id_pbf( RS2_FORMAT_Y8, RS2_STREAM_INFRARED, 1 ) ); + factories.push_back( processing_block_factory::create_id_pbf( RS2_FORMAT_Z16, RS2_STREAM_DEPTH ) ); + factories.push_back( { { { RS2_FORMAT_W10 } }, + { { RS2_FORMAT_RAW10, RS2_STREAM_INFRARED, 1 } }, + []() { return std::make_shared< w10_converter >( RS2_FORMAT_RAW10 ); } } ); + factories.push_back( { { { RS2_FORMAT_W10 } }, + { { RS2_FORMAT_Y10BPACK, RS2_STREAM_INFRARED, 1 } }, + []() { return std::make_shared< w10_converter >( RS2_FORMAT_Y10BPACK ); } } ); + factories.push_back( { { { RS2_FORMAT_Y8I } }, + { { RS2_FORMAT_Y8, RS2_STREAM_INFRARED, 1 }, + { RS2_FORMAT_Y8, RS2_STREAM_INFRARED, 2 } }, + []() { return std::make_shared< y8i_to_y8y8 >(); } } ); + + // Motion convertion is done on the camera side. + // Intrinsics table is being read from the camera flash and it is too much to set here, this file is ment as a + // temporary solution, the camera should send all this data in a designated topic. + factories.push_back( { { { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL } }, + { { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL } }, + []() { return std::make_shared< identity_processing_block >(); } } ); + factories.push_back( { { { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO } }, + { { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO } }, + []() { return std::make_shared< identity_processing_block >(); } } ); + } + + // For other devices empty factories will be returned, raw profiles will be used. + + return factories; +} + +} // namespace librealsense diff --git a/src/dds/rs-dds-internal-data.h b/src/dds/rs-dds-internal-data.h new file mode 100644 index 0000000000..4c3e13f196 --- /dev/null +++ b/src/dds/rs-dds-internal-data.h @@ -0,0 +1,28 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. + +#pragma once + +#include +#include + + +namespace librealsense { + + +// librealsense initializes each device type with a device specific information - what are the device raw formats and +// to what profiles we can convert them, what are the default profiles, intrinsics information etc... +// This data is stored in the specific device and sensor classes constructor or initialization functions. +// Currently we did not want to send it as a realdds supported topic, so this class stores all the data needed for DDS +// devices in one place. +class dds_rs_internal_data +{ +public: + static std::vector< tagged_profile > get_profiles_tags( const std::string & product_id, + const std::string & product_line ); + static std::vector< processing_block_factory > get_profile_converters( const std::string & product_id, + const std::string & product_line ); +}; + + +} // namespace librealsense diff --git a/src/dds/rs-dds-option.cpp b/src/dds/rs-dds-option.cpp new file mode 100644 index 0000000000..fe1b81b4ef --- /dev/null +++ b/src/dds/rs-dds-option.cpp @@ -0,0 +1,52 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "rs-dds-option.h" + +#include + + +namespace librealsense { + + +rs_dds_option::rs_dds_option( const std::shared_ptr< realdds::dds_option > & dds_opt, + set_option_callback set_opt_cb, + query_option_callback query_opt_cb ) + : option_base( { dds_opt->get_range().min, + dds_opt->get_range().max, + dds_opt->get_range().step, + dds_opt->get_range().default_value } ) + , _dds_opt( dds_opt ) + , _set_opt_cb( set_opt_cb ) + , _query_opt_cb( query_opt_cb ) +{ +} + + +void rs_dds_option::set( float value ) +{ + if( ! _set_opt_cb ) + throw std::runtime_error( "Set option callback is not set for option " + _dds_opt->get_name() ); + + _set_opt_cb( _dds_opt->get_name(), value ); +} + + +float rs_dds_option::query() const +{ + if( ! _query_opt_cb ) + throw std::runtime_error( "Query option callback is not set for option " + _dds_opt->get_name() ); + + return _query_opt_cb( _dds_opt->get_name() ); +} + + +const char * rs_dds_option::get_description() const +{ + return _dds_opt->get_description().c_str(); +} + + +} // namespace librealsense diff --git a/src/dds/rs-dds-option.h b/src/dds/rs-dds-option.h new file mode 100644 index 0000000000..f4428bf1a7 --- /dev/null +++ b/src/dds/rs-dds-option.h @@ -0,0 +1,47 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. + +#pragma once + +#include + +#include +#include + + +namespace realdds { +class dds_option; +} // namespace realdds + + +namespace librealsense { + + +// A facade for a realdds::dds_option exposing librealsense interface +class rs_dds_option : public option_base +{ + std::shared_ptr< realdds::dds_option > _dds_opt; + +public: + typedef std::function< void( const std::string & name, float value ) > set_option_callback; + typedef std::function< float( const std::string & name ) > query_option_callback; + +private: + set_option_callback _set_opt_cb; + query_option_callback _query_opt_cb; + +public: + rs_dds_option( const std::shared_ptr< realdds::dds_option > & dds_opt, + set_option_callback set_opt_cb, + query_option_callback query_opt_cb ); + + void set( float value ) override; + + float query() const override; + + bool is_enabled() const override { return true; } + const char * get_description() const override; +}; + + +} // namespace librealsense diff --git a/src/dds/rs-dds-sensor-proxy.cpp b/src/dds/rs-dds-sensor-proxy.cpp new file mode 100644 index 0000000000..c2aff68cdb --- /dev/null +++ b/src/dds/rs-dds-sensor-proxy.cpp @@ -0,0 +1,631 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. + +#include "rs-dds-sensor-proxy.h" +#include "rs-dds-option.h" +#include "rs-dds-internal-data.h" + +#include +#include + +#include +#include +#include + +#include + +// Processing blocks for DDS SW sensors +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + + +namespace librealsense { + + +// Constants for Json lookups +static const std::string frame_number_key( "frame-number", 12 ); +static const std::string metadata_key( "metadata", 8 ); +static const std::string timestamp_key( "timestamp", 9 ); +static const std::string timestamp_domain_key( "timestamp-domain", 16 ); +static const std::string depth_units_key( "depth-units", 11 ); +static const std::string metadata_header_key( "header", 6 ); + + +dds_sensor_proxy::dds_sensor_proxy( std::string const & sensor_name, + software_device * owner, + std::shared_ptr< realdds::dds_device > const & dev ) + : software_sensor( sensor_name, owner ) + , _dev( dev ) + , _name( sensor_name ) + , _md_enabled( dev->supports_metadata() ) +{ +} + + +void dds_sensor_proxy::add_dds_stream( sid_index sidx, std::shared_ptr< realdds::dds_stream > const & stream ) +{ + auto & s = _streams[sidx]; + if( s ) + { + LOG_ERROR( "stream at " << sidx.to_string() << " already exists for sensor '" << get_name() << "'" ); + return; + } + s = stream; +} + + +std::shared_ptr< stream_profile_interface > dds_sensor_proxy::add_video_stream( rs2_video_stream video_stream, + bool is_default ) +{ + auto profile = std::make_shared< video_stream_profile >( platform::stream_profile{ (uint32_t)video_stream.width, + (uint32_t)video_stream.height, + (uint32_t)video_stream.fps, + 0 } ); + profile->set_dims( video_stream.width, video_stream.height ); + profile->set_format( video_stream.fmt ); + profile->set_framerate( video_stream.fps ); + profile->set_stream_index( video_stream.index ); + profile->set_stream_type( video_stream.type ); + profile->set_unique_id( video_stream.uid ); + profile->set_intrinsics( [=]() { // + return video_stream.intrinsics; + } ); + if( is_default ) + profile->tag_profile( profile_tag::PROFILE_TAG_DEFAULT ); + _raw_rs_profiles.push_back( profile ); + + return profile; +} + + +std::shared_ptr< stream_profile_interface > dds_sensor_proxy::add_motion_stream( rs2_motion_stream motion_stream, + bool is_default ) +{ + auto profile = std::make_shared< motion_stream_profile >( platform::stream_profile{ 0, + 0, + (uint32_t)motion_stream.fps, + 0 } ); + profile->set_format( motion_stream.fmt ); + profile->set_framerate( motion_stream.fps ); + profile->set_stream_index( motion_stream.index ); + profile->set_stream_type( motion_stream.type ); + profile->set_unique_id( motion_stream.uid ); + profile->set_intrinsics( [=]() { return motion_stream.intrinsics; } ); + if( is_default ) + profile->tag_profile( profile_tag::PROFILE_TAG_DEFAULT ); + _raw_rs_profiles.push_back( profile ); + + return profile; +} + + +void dds_sensor_proxy::initialization_done() +{ + register_basic_converters(); + _profiles = _formats_converter.get_all_possible_profiles( _raw_rs_profiles ); +} + + +void dds_sensor_proxy::register_basic_converters() +{ + std::vector< librealsense::processing_block_factory > converters; + std::vector< processing_block_factory > tmp; + + // Color + converters.push_back( processing_block_factory::create_id_pbf( RS2_FORMAT_YUYV, RS2_STREAM_COLOR ) ); + converters.push_back( processing_block_factory::create_id_pbf( RS2_FORMAT_UYVY, RS2_STREAM_COLOR ) ); + converters.push_back( processing_block_factory::create_id_pbf( RS2_FORMAT_RGB8, RS2_STREAM_COLOR ) ); + converters.push_back( processing_block_factory::create_id_pbf( RS2_FORMAT_RGBA8, RS2_STREAM_COLOR ) ); + converters.push_back( processing_block_factory::create_id_pbf( RS2_FORMAT_BGR8, RS2_STREAM_COLOR ) ); + converters.push_back( processing_block_factory::create_id_pbf( RS2_FORMAT_BGRA8, RS2_STREAM_COLOR ) ); + + converters.push_back( { { { RS2_FORMAT_UYVY } }, { { RS2_FORMAT_RGB8, RS2_STREAM_COLOR } }, + []() { return std::make_shared< uyvy_converter >( RS2_FORMAT_RGB8 ); } } ); + converters.push_back( { { { RS2_FORMAT_YUYV } }, { { RS2_FORMAT_RGB8, RS2_STREAM_COLOR } }, + []() { return std::make_shared< yuy2_converter >( RS2_FORMAT_RGB8 ); } } ); + + // Depth + converters.push_back( processing_block_factory::create_id_pbf( RS2_FORMAT_Z16, RS2_STREAM_DEPTH ) ); + + // Infrared (converter source needs type to be handled properly by formats_converter) + converters.push_back( { { { RS2_FORMAT_Y8, RS2_STREAM_INFRARED } }, + { { RS2_FORMAT_Y8, RS2_STREAM_INFRARED, 0 }, + { RS2_FORMAT_Y8, RS2_STREAM_INFRARED, 1 }, + { RS2_FORMAT_Y8, RS2_STREAM_INFRARED, 2 } }, + []() { return std::make_shared< identity_processing_block >(); } } ); + converters.push_back( { { { RS2_FORMAT_Y16, RS2_STREAM_INFRARED } }, + { { RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 1 }, + { RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 2 } }, + []() { return std::make_shared< identity_processing_block >(); } } ); + + // Motion + converters.push_back( { { { RS2_FORMAT_COMBINED_MOTION, RS2_STREAM_MOTION } }, + { { RS2_FORMAT_COMBINED_MOTION, RS2_STREAM_MOTION } }, + []() { return std::make_shared< identity_processing_block >(); } } ); + + // Confidence + converters.push_back( { { { RS2_FORMAT_RAW8, RS2_STREAM_CONFIDENCE } }, + { { RS2_FORMAT_RAW8, RS2_STREAM_CONFIDENCE } }, + []() { return std::make_shared< identity_processing_block >(); } } ); + + _formats_converter.register_converters( converters ); +} + + +std::shared_ptr< realdds::dds_video_stream_profile > +dds_sensor_proxy::find_profile( sid_index sidx, realdds::dds_video_stream_profile const & profile ) const +{ + auto it = _streams.find( sidx ); + if( it == _streams.end() ) + { + LOG_ERROR( "Invalid stream index " << sidx.to_string() << " in rs2 profile for sensor '" << get_name() << "'" ); + } + else + { + auto & stream = it->second; + for( auto & sp : stream->profiles() ) + { + auto vsp = std::static_pointer_cast< realdds::dds_video_stream_profile >( sp ); + if( profile.width() == vsp->width() && profile.height() == vsp->height() + && profile.format() == vsp->format() && profile.frequency() == vsp->frequency() ) + { + return vsp; + } + } + } + return std::shared_ptr< realdds::dds_video_stream_profile >(); +} + + +std::shared_ptr< realdds::dds_motion_stream_profile > +dds_sensor_proxy::find_profile( sid_index sidx, realdds::dds_motion_stream_profile const & profile ) const +{ + auto it = _streams.find( sidx ); + if( it == _streams.end() ) + { + LOG_ERROR( "Invalid stream index " << sidx.to_string() << " in rs2 profile for sensor '" << get_name() << "'" ); + } + else + { + auto & stream = it->second; + for( auto & sp : stream->profiles() ) + { + auto msp = std::static_pointer_cast< realdds::dds_motion_stream_profile >( sp ); + if( profile.frequency() == msp->frequency() ) + { + return msp; + } + } + } + return std::shared_ptr< realdds::dds_motion_stream_profile >(); +} + + +void dds_sensor_proxy::open( const stream_profiles & profiles ) +{ + _formats_converter.prepare_to_convert( profiles ); + + const auto & source_profiles = _formats_converter.get_active_source_profiles(); + // TODO - register processing block options? + + realdds::dds_stream_profiles realdds_profiles; + for( size_t i = 0; i < source_profiles.size(); ++i ) + { + auto & sp = source_profiles[i]; + sid_index sidx( sp->get_unique_id(), sp->get_stream_index() ); + if( Is< 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(), + realdds::dds_stream_format::from_rs2( sp->get_format() ), + vsp->get_width(), + vsp->get_height() ) ); + if( video_profile ) + realdds_profiles.push_back( video_profile ); + else + LOG_ERROR( "no profile found in stream for rs2 profile " << vsp ); + } + else if( Is< motion_stream_profile >( sp ) ) + { + auto motion_profile = find_profile( + sidx, + realdds::dds_motion_stream_profile( source_profiles[i]->get_framerate() ) ); + if( motion_profile ) + realdds_profiles.push_back( motion_profile ); + else + LOG_ERROR( "no profile found in stream for rs2 profile " << sp ); + } + else + { + LOG_ERROR( "unknown stream profile type for rs2 profile " << sp ); + } + } + + if( source_profiles.size() > 0 ) + { + _dev->open( realdds_profiles ); + } + + software_sensor::open( source_profiles ); +} + + +void dds_sensor_proxy::handle_video_data( realdds::topics::image_msg && dds_frame, + const std::shared_ptr< stream_profile_interface > & profile, + streaming_impl & streaming ) +{ + frame_additional_data data; // with NO metadata by default! + data.timestamp // in ms + = static_cast< rs2_time_t >( realdds::time_to_double( dds_frame.timestamp ) * 1e3 ); + 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 + + 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 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 ); + + if( _md_enabled ) + { + streaming.syncer.enqueue_frame( dds_frame.timestamp.to_ns(), streaming.syncer.hold( new_frame ) ); + } + else + { + invoke_new_frame( new_frame, + nullptr, // pixels are already inside new_frame->data + nullptr ); // so no deleter is necessary + } +} + + +void dds_sensor_proxy::handle_motion_data( realdds::topics::imu_msg && imu, + const std::shared_ptr< stream_profile_interface > & profile, + streaming_impl & streaming ) +{ + frame_additional_data data; // with NO metadata by default! + data.timestamp // in ms + = static_cast< rs2_time_t >( realdds::time_to_double( imu.timestamp() ) * 1e3 ); + data.timestamp_domain; // leave default (hardware domain) + data.last_frame_number = streaming.last_frame_number.fetch_add( 1 ); + data.frame_number = data.last_frame_number + 1; + + auto new_frame_interface = allocate_new_frame( RS2_EXTENSION_MOTION_FRAME, profile.get(), std::move( data ) ); + if( ! new_frame_interface ) + return; + + auto new_frame = static_cast< frame * >( new_frame_interface ); + new_frame->data.resize( sizeof( rs2_combined_motion ) ); + rs2_combined_motion * m = reinterpret_cast< rs2_combined_motion * >( new_frame->data.data() ); + m->orientation.x = imu.imu_data().orientation().x(); + m->orientation.y = imu.imu_data().orientation().y(); + m->orientation.z = imu.imu_data().orientation().z(); + m->orientation.w = imu.imu_data().orientation().w(); + m->angular_velocity.x = imu.gyro_data().x(); // should be in rad/sec + m->angular_velocity.y = imu.gyro_data().y(); + m->angular_velocity.z = imu.gyro_data().z(); + m->linear_acceleration.x = imu.accel_data().x(); // should be in m/s^2 + m->linear_acceleration.y = imu.accel_data().y(); + m->linear_acceleration.z = imu.accel_data().z(); + + // No metadata for motion streams, therefore no syncer + invoke_new_frame( new_frame, + nullptr, // pixels are already inside new_frame->data + nullptr ); // so no deleter is necessary +} + + +void dds_sensor_proxy::handle_new_metadata( std::string const & stream_name, nlohmann::json && dds_md ) +{ + if( ! _md_enabled ) + return; + + auto it = _streaming_by_name.find( stream_name ); + if( it != _streaming_by_name.end() ) + it->second.syncer.enqueue_metadata( + rsutils::json::get< realdds::dds_nsec >( dds_md[metadata_header_key], timestamp_key ), + std::move( dds_md ) ); + else + throw std::runtime_error( "Stream '" + stream_name + "' received metadata but does not enable metadata" ); +} + + +void dds_sensor_proxy::add_frame_metadata( frame * const f, nlohmann::json && dds_md, streaming_impl & streaming ) +{ + if( dds_md.empty() ) + { + // Without MD, we have no way of knowing the frame-number - we assume it's one higher than + // the last + f->additional_data.last_frame_number = streaming.last_frame_number.fetch_add( 1 ); + f->additional_data.frame_number = f->additional_data.last_frame_number + 1; + // the frame should already have empty metadata, so no need to do anything else + return; + } + + nlohmann::json const & md_header = dds_md[metadata_header_key]; + nlohmann::json const & md = dds_md[metadata_key]; + + // A frame number is "optional". If the server supplies it, we try to use it for the simple fact that, + // otherwise, we have no way of detecting drops without some advanced heuristic tracking the FPS and + // timestamps. If not supplied, we use an increasing counter. + // Note that if we have no metadata, we have no frame-numbers! So we need a way of generating them + if( rsutils::json::get_ex( md_header, frame_number_key, &f->additional_data.frame_number ) ) + { + f->additional_data.last_frame_number = streaming.last_frame_number.exchange( f->additional_data.frame_number ); + if( f->additional_data.frame_number != f->additional_data.last_frame_number + 1 + && f->additional_data.last_frame_number ) + { + LOG_DEBUG( "frame drop? expecting " << f->additional_data.last_frame_number + 1 << "; got " + << f->additional_data.frame_number ); + } + } + else + { + f->additional_data.last_frame_number = streaming.last_frame_number.fetch_add( 1 ); + f->additional_data.frame_number = f->additional_data.last_frame_number + 1; + } + + // Timestamp is already set in the frame - must be communicated in the metadata, but only for syncing + // purposes, so we ignore here. The domain is optional, and really only rs-dds-adapter communicates it + // because the source is librealsense... + f->additional_data.timestamp; + rsutils::json::get_ex( md_header, timestamp_domain_key, &f->additional_data.timestamp_domain ); + + // Expected metadata for all depth images + rsutils::json::get_ex( md_header, depth_units_key, &f->additional_data.depth_units ); + + // Other metadata fields. Metadata fields that are present but unknown by librealsense will be ignored. + auto & metadata = reinterpret_cast< metadata_array & >( f->additional_data.metadata_blob ); + for( size_t i = 0; i < static_cast< size_t >( RS2_FRAME_METADATA_COUNT ); ++i ) + { + auto key = static_cast< rs2_frame_metadata_value >( i ); + std::string const & keystr = librealsense::get_string( key ); + try + { + metadata[key] = { true, rsutils::json::get< rs2_metadata_type >( md, keystr ) }; + } + catch( std::runtime_error const & ) + { + // The metadata key doesn't exist or the value isn't the right type... we ignore it! + // (all metadata is not there when we create the frame, so no need to erase) + } + } +} + + +template +frame_callback_ptr make_callback( T callback ) +{ + return { + new internal_frame_callback( callback ), + []( rs2_frame_callback * p ) { p->release(); } + }; +} + + +void dds_sensor_proxy::start( frame_callback_ptr callback ) +{ + for( auto & profile : sensor_base::get_active_streams() ) + { + auto streamit = _streams.find( sid_index( profile->get_unique_id(), profile->get_stream_index() ) ); + if( streamit == _streams.end() ) + { + LOG_ERROR( "Profile (" << profile->get_unique_id() << "," << profile->get_stream_index() << ") not found in streams!"); + continue; + } + auto const & dds_stream = streamit->second; + // Opening it will start streaming on the server side automatically + dds_stream->open( "rt/" + _dev->device_info().topic_root + '_' + dds_stream->name(), _dev->subscriber() ); + auto & streaming = _streaming_by_name[dds_stream->name()]; + streaming.syncer.on_frame_release( frame_releaser ); + streaming.syncer.on_frame_ready( + [this, &streaming]( syncer_type::frame_holder && fh, nlohmann::json && md ) + { + if( _is_streaming ) // stop was not called + { + add_frame_metadata( static_cast< frame * >( fh.get() ), std::move( md ), streaming ); + invoke_new_frame( static_cast< frame * >( fh.release() ), nullptr, nullptr ); + } + } ); + + if( auto dds_video_stream = std::dynamic_pointer_cast< realdds::dds_video_stream >( dds_stream ) ) + { + dds_video_stream->on_data_available( + [profile, this, &streaming]( realdds::topics::image_msg && dds_frame ) + { + if( _is_streaming ) + handle_video_data( std::move( dds_frame ), profile, streaming ); + } ); + } + else if( auto dds_motion_stream = std::dynamic_pointer_cast< realdds::dds_motion_stream >( dds_stream ) ) + { + dds_motion_stream->on_data_available( + [profile, this, &streaming]( realdds::topics::imu_msg && imu ) + { + if( _is_streaming ) + handle_motion_data( std::move( imu ), profile, streaming ); + } ); + } + else + throw std::runtime_error( "Unsupported stream type" ); + + dds_stream->start_streaming(); + } + + _formats_converter.set_frames_callback( callback ); + const auto && process_cb = make_callback( [&, this]( frame_holder f ) { + _formats_converter.convert_frame( f ); + } ); + + software_sensor::start( process_cb ); +} + + +void dds_sensor_proxy::stop() +{ + for( auto & profile : sensor_base::get_active_streams() ) + { + auto streamit = _streams.find( sid_index( profile->get_unique_id(), profile->get_stream_index() ) ); + if( streamit == _streams.end() ) + { + LOG_ERROR( "Profile (" << profile->get_unique_id() << "," << profile->get_stream_index() << ") not found in streams!" ); + continue; + } + auto const & dds_stream = streamit->second; + + dds_stream->stop_streaming(); + dds_stream->close(); + + _streaming_by_name[dds_stream->name()].syncer.on_frame_ready( nullptr ); + + if( auto dds_video_stream = std::dynamic_pointer_cast< realdds::dds_video_stream >( dds_stream ) ) + { + dds_video_stream->on_data_available( nullptr ); + } + else if( auto dds_motion_stream = std::dynamic_pointer_cast< realdds::dds_motion_stream >( dds_stream ) ) + { + dds_motion_stream->on_data_available( nullptr ); + } + else + throw std::runtime_error( "Unsupported stream type" ); + } + + // Resets frame source. Nullify streams on_data_available before calling stop. + software_sensor::stop(); + + // Must be done after dds_stream->stop_streaming or we will need to add validity checks to on_data_available, + // and after software_sensor::stop cause to make sure _is_streaming is false + _streaming_by_name.clear(); +} + + +void dds_sensor_proxy::add_option( std::shared_ptr< realdds::dds_option > option ) +{ + // Convert name to rs2_option type + rs2_option option_id = RS2_OPTION_COUNT; + for( size_t i = 0; i < static_cast< size_t >( RS2_OPTION_COUNT ); i++ ) + { + if( option->get_name().compare( get_string( static_cast< rs2_option >( i ) ) ) == 0 ) + { + option_id = static_cast< rs2_option >( i ); + break; + } + } + + if( option_id == RS2_OPTION_COUNT ) + throw librealsense::invalid_value_exception( "Option " + option->get_name() + " type not found" ); + + auto opt = std::make_shared< rs_dds_option >( + option, + [&]( const std::string & name, float value ) { set_option( name, value ); }, + [&]( const std::string & name ) -> float { return query_option( name ); } ); + register_option( option_id, opt ); +} + + +void dds_sensor_proxy::set_option( const std::string & name, float value ) const +{ + // Sensor is setting the option for all supporting streams (with same value) + for( auto & stream : _streams ) + { + for( auto & dds_opt : stream.second->options() ) + { + if( dds_opt->get_name().compare( name ) == 0 ) + { + _dev->set_option_value( dds_opt, value ); + break; + } + } + } +} + + +float dds_sensor_proxy::query_option( const std::string & name ) const +{ + for( auto & stream : _streams ) + { + for( auto & dds_opt : stream.second->options() ) + { + if( dds_opt->get_name().compare( name ) == 0 ) + { + // Assumes value is same for all relevant streams in the sensor, values are always set together + return _dev->query_option_value( dds_opt ); + } + } + } + + throw std::runtime_error( "Could not find a stream that supports option " + name ); +} + + +void dds_sensor_proxy::add_processing_block( std::string filter_name ) +{ + auto & current_filters = get_software_recommended_proccesing_blocks(); + + if( processing_block_exists( current_filters.get_recommended_processing_blocks(), filter_name ) ) + return; // Already created by another stream of this sensor + + create_processing_block( filter_name ); +} + + +bool dds_sensor_proxy::processing_block_exists( processing_blocks const & blocks, std::string const & block_name ) const +{ + for( auto & block : blocks ) + if( block_name.compare( block->get_info( RS2_CAMERA_INFO_NAME ) ) == 0 ) + return true; + + return false; +} + + +void dds_sensor_proxy::create_processing_block( std::string & filter_name ) +{ + auto & current_filters = get_software_recommended_proccesing_blocks(); + + if( filter_name.compare( "Decimation Filter" ) == 0 ) + // sensor.cpp sets format option based on sensor type, but the filter does not use it and selects the + // appropriate decimation algorithm based on processed frame profile format. + current_filters.add_processing_block( std::make_shared< decimation_filter >() ); + else if( filter_name.compare( "HDR Merge" ) == 0 ) + current_filters.add_processing_block( std::make_shared< hdr_merge >() ); + else if( filter_name.compare( "Filter By Sequence id" ) == 0 ) + current_filters.add_processing_block( std::make_shared< sequence_id_filter >() ); + else if( filter_name.compare( "Threshold Filter" ) == 0 ) + current_filters.add_processing_block( std::make_shared< threshold >() ); + else if( filter_name.compare( "Depth to Disparity" ) == 0 ) + current_filters.add_processing_block( std::make_shared< disparity_transform >( true ) ); + else if( filter_name.compare( "Disparity to Depth" ) == 0 ) + current_filters.add_processing_block( std::make_shared< disparity_transform >( false ) ); + else if( filter_name.compare( "Spatial Filter" ) == 0 ) + current_filters.add_processing_block( std::make_shared< spatial_filter >() ); + else if( filter_name.compare( "Temporal Filter" ) == 0 ) + current_filters.add_processing_block( std::make_shared< temporal_filter >() ); + else if( filter_name.compare( "Hole Filling Filter" ) == 0 ) + current_filters.add_processing_block( std::make_shared< hole_filling_filter >() ); + else + throw std::runtime_error( "Unsupported processing block '" + filter_name + "' received" ); +} + + +} // namespace librealsense diff --git a/src/dds/rs-dds-sensor-proxy.h b/src/dds/rs-dds-sensor-proxy.h new file mode 100644 index 0000000000..ab33290589 --- /dev/null +++ b/src/dds/rs-dds-sensor-proxy.h @@ -0,0 +1,110 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. + +#pragma once + + +#include "sid_index.h" +#include +#include + +#include + +#include +#include +#include + + +namespace realdds { +class dds_device; +class dds_stream; +class dds_option; +class dds_video_stream_profile; +class dds_motion_stream_profile; +namespace topics { +class image_msg; +class imu_msg; +} // namespace topics +} // namespace realdds + + +namespace librealsense { + + +class dds_device_proxy; + + +class dds_sensor_proxy : public software_sensor +{ + std::shared_ptr< realdds::dds_device > const _dev; + std::string const _name; + bool const _md_enabled; + + typedef realdds::dds_metadata_syncer syncer_type; + static void frame_releaser( syncer_type::frame_type * f ) { static_cast< frame * >( f )->release(); } + + struct streaming_impl + { + syncer_type syncer; + std::atomic< unsigned long long > last_frame_number{ 0 }; + }; + + std::map< sid_index, std::shared_ptr< realdds::dds_stream > > _streams; + std::map< std::string, streaming_impl > _streaming_by_name; + + formats_converter _formats_converter; + // DDS profiles are stored in _streams, _raw_rs_profiles stores librealsense representation of them, + // software_device::_profiles stores librealsense profiles after conversion from raw to a user friendly format. + stream_profiles _raw_rs_profiles; + +public: + dds_sensor_proxy( std::string const & sensor_name, + software_device * owner, + std::shared_ptr< realdds::dds_device > const & dev ); + + const std::string & get_name() const { return _name; } + + void add_dds_stream( sid_index sidx, std::shared_ptr< realdds::dds_stream > const & stream ); + std::shared_ptr add_video_stream( rs2_video_stream video_stream, bool is_default ) override; + std::shared_ptr add_motion_stream( rs2_motion_stream motion_stream, bool is_default ) override; + // Not adding streams or profiles after this + void initialization_done(); + + void open( const stream_profiles & profiles ) override; + void start( frame_callback_ptr callback ) override; + void stop(); + + void add_option( std::shared_ptr< realdds::dds_option > option ); + void set_option( const std::string & name, float value ) const; + float query_option( const std::string & name ) const; + + void add_processing_block( std::string filter_name ); + bool processing_block_exists( processing_blocks const & blocks, std::string const & block_name ) const; + void create_processing_block( std::string & filter_name ); + + const std::map< sid_index, std::shared_ptr< realdds::dds_stream > > & streams() const { return _streams; } + +private: + void register_basic_converters(); + + std::shared_ptr< realdds::dds_video_stream_profile > + find_profile( sid_index sidx, realdds::dds_video_stream_profile const & profile ) const; + + std::shared_ptr< realdds::dds_motion_stream_profile > + find_profile( sid_index sidx, realdds::dds_motion_stream_profile const & profile ) const; + + void handle_video_data( realdds::topics::image_msg && dds_frame, + const std::shared_ptr< stream_profile_interface > &, + streaming_impl & streaming ); + void handle_motion_data( realdds::topics::imu_msg &&, + const std::shared_ptr< stream_profile_interface > &, + streaming_impl & ); + void handle_new_metadata( std::string const & stream_name, nlohmann::json && metadata ); + + void add_frame_metadata( frame * const, nlohmann::json && metadata, streaming_impl & ); + + friend class dds_device_proxy; // Currently calls handle_new_metadata +}; + + +} // namespace librealsense diff --git a/src/dds/sid_index.h b/src/dds/sid_index.h new file mode 100644 index 0000000000..82ad5ac4fd --- /dev/null +++ b/src/dds/sid_index.h @@ -0,0 +1,35 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. + +#pragma once + +#include + +namespace librealsense { + + +struct sid_index +{ + int sid; // Stream ID; assigned based on an atomic counter + int index; // Used to distinguish similar streams like IR L / R, 0 otherwise + + sid_index( int sid_, int index_ ) + : sid( sid_ ) + , index( index_ ) + { + } + + sid_index() = default; + sid_index( sid_index const & ) = default; + sid_index( sid_index && ) = default; + + std::string to_string() const { return '(' + std::to_string( sid ) + '.' + std::to_string( index ) + ')'; } + + inline bool operator<( sid_index const & r ) const + { + return this->sid < r.sid || this->sid == r.sid && this->index < r.index; + } +}; + + +} diff --git a/src/device.cpp b/src/device.cpp index 6cc93aea00..60e9e05ee5 100644 --- a/src/device.cpp +++ b/src/device.cpp @@ -5,6 +5,7 @@ #include "core/video.h" #include "core/motion.h" #include "device.h" + #include using namespace librealsense; @@ -167,7 +168,7 @@ matcher_factory::create_timestamp_composite_matcher( std::vector< std::shared_pt } device::device(std::shared_ptr ctx, - const platform::backend_device_group group, + const platform::backend_device_group & group, bool device_changed_notifications) : _context(ctx), _group(group), _is_valid(true), _device_changed_notifications(device_changed_notifications), diff --git a/src/device.h b/src/device.h index 3f7096bc38..3c0d145d04 100644 --- a/src/device.h +++ b/src/device.h @@ -95,7 +95,7 @@ class device : public virtual device_interface, public info_container std::vector map_supported_color_formats(rs2_format source_format); explicit device(std::shared_ptr ctx, - const platform::backend_device_group group, + const platform::backend_device_group & group, bool device_changed_notifications = true); std::map>> _extrinsics; diff --git a/src/ds/advanced_mode/json_loader.hpp b/src/ds/advanced_mode/json_loader.hpp index 35546a1f00..a927ae9ac1 100644 --- a/src/ds/advanced_mode/json_loader.hpp +++ b/src/ds/advanced_mode/json_loader.hpp @@ -12,7 +12,7 @@ #include #include -#include "../../../third-party/json.hpp" +#include #include #include "types.h" #include "presets.h" diff --git a/src/ds/d400/d400-auto-calibration.cpp b/src/ds/d400/d400-auto-calibration.cpp index 499196cc4f..de97390c55 100644 --- a/src/ds/d400/d400-auto-calibration.cpp +++ b/src/ds/d400/d400-auto-calibration.cpp @@ -2,7 +2,7 @@ // Copyright(c) 2016 Intel Corporation. All Rights Reserved. #include -#include "../third-party/json.hpp" +#include #include "d400-device.h" #include "d400-private.h" #include "d400-thermal-monitor.h" diff --git a/src/ds/d400/d400-device.cpp b/src/ds/d400/d400-device.cpp index 6fbfeae585..30debc3a6b 100644 --- a/src/ds/d400/d400-device.cpp +++ b/src/ds/d400/d400-device.cpp @@ -1,34 +1,34 @@ // License: Apache 2.0. See LICENSE file in root directory. // Copyright(c) 2016 Intel Corporation. All Rights Reserved. -#include -#include - -#include "device.h" -#include "context.h" -#include "image.h" -#include "metadata-parser.h" +#include +#include +#include +#include #include "d400-device.h" #include "d400-private.h" #include "d400-options.h" #include "ds/ds-timestamp.h" -#include "stream.h" -#include "environment.h" +#include +#include #include "d400-color.h" #include "d400-nonmonochrome.h" -#include "proc/depth-formats-converter.h" -#include "proc/y8i-to-y8y8.h" -#include "proc/y12i-to-y16y16.h" -#include "proc/y12i-to-y16y16-mipi.h" -#include "proc/color-formats-converter.h" +#include +#include +#include +#include +#include -#include "hdr-config.h" +#include #include "d400-thermal-monitor.h" -#include "../common/fw/firmware-version.h" -#include "fw-update/fw-update-unsigned.h" -#include "../third-party/json.hpp" +#include +#include +#include + +#include +#include #ifdef HWM_OVER_XU constexpr bool hw_mon_over_xu = true; @@ -1152,13 +1152,11 @@ namespace librealsense platform::usb_spec d400_device::get_usb_spec() const { - if(!supports_info(RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR)) - return platform::usb_undefined; - auto str = get_info(RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR); - for (auto u : platform::usb_spec_names) + if( supports_info( RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR ) ) { - if (u.second.compare(str) == 0) - return u.first; + auto it = platform::usb_name_to_spec.find( get_info( RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR ) ); + if( it != platform::usb_name_to_spec.end() ) + return it->second; } return platform::usb_undefined; } diff --git a/src/ds/d400/d400-private.cpp b/src/ds/d400/d400-private.cpp index 839e775159..86aca2f1b3 100644 --- a/src/ds/d400/d400-private.cpp +++ b/src/ds/d400/d400-private.cpp @@ -257,7 +257,7 @@ namespace librealsense RS2_DISTORTION_INVERSE_BROWN_CONRADY // The coefficients shall be use for undistort }; librealsense::copy(calc_intrinsic.coeffs, table->distortion, sizeof(table->distortion)); - LOG_DEBUG(endl << array2str((float_4&)(calc_intrinsic.fx, calc_intrinsic.fy, calc_intrinsic.ppx, calc_intrinsic.ppy)) << endl); + //LOG_DEBUG(endl << array2str((float_4&)(calc_intrinsic.fx, calc_intrinsic.fy, calc_intrinsic.ppx, calc_intrinsic.ppy)) << endl); static rs2_intrinsics ref{}; if (memcmp(&calc_intrinsic, &ref, sizeof(rs2_intrinsics))) diff --git a/src/ds/ds-private.h b/src/ds/ds-private.h index 12b6ad1a39..b7d3c44a64 100644 --- a/src/ds/ds-private.h +++ b/src/ds/ds-private.h @@ -292,10 +292,10 @@ namespace librealsense throw invalid_value_exception("Calibration data CRC error, parsing aborted!"); } - LOG_DEBUG("Loaded Valid Table: version [mjr.mnr]: 0x" << - hex << setfill('0') << setw(4) << header->version << dec - << ", type " << header->table_type << ", size " << header->table_size - << ", CRC: " << hex << header->crc32); + //LOG_DEBUG("Loaded Valid Table: version [mjr.mnr]: 0x" << + // hex << setfill('0') << setw(4) << header->version << dec + // << ", type " << header->table_type << ", size " << header->table_size + // << ", CRC: " << hex << header->crc32 << dec ); return table; } @@ -619,4 +619,4 @@ namespace librealsense 0x4, 0x1, 0, 0x1, 0, 0, 0, 0, 0, 0x4, 0x1, 0, 0x1, 0, 0x1, 0, 0, 0 }; } // librealsense::ds -} // namespace librealsense \ No newline at end of file +} // namespace librealsense diff --git a/src/frame-archive.h b/src/frame-archive.h index 5ca2052dc9..af758bfaee 100644 --- a/src/frame-archive.h +++ b/src/frame-archive.h @@ -26,7 +26,7 @@ namespace librealsense std::shared_ptr get_sensor() const override { return _sensor.lock(); } void set_sensor(std::shared_ptr s) override { _sensor = s; } - T alloc_frame(const size_t size, const frame_additional_data& additional_data, bool requires_memory) + T alloc_frame(const size_t size, frame_additional_data && additional_data, bool requires_memory) { T backbuffer; //const size_t size = modes[stream].get_image_size(stream); @@ -59,7 +59,7 @@ namespace librealsense { backbuffer.data.resize(size, 0); // TODO: Allow users to provide a custom allocator for frame buffers } - backbuffer.additional_data = additional_data; + backbuffer.additional_data = std::move( additional_data ); return backbuffer; } @@ -160,9 +160,9 @@ namespace librealsense ref->release(); } - frame_interface* alloc_and_track(const size_t size, const frame_additional_data& additional_data, bool requires_memory) override + frame_interface* alloc_and_track(const size_t size, frame_additional_data && additional_data, bool requires_memory) override { - auto frame = alloc_frame(size, additional_data, requires_memory); + auto frame = alloc_frame( size, std::move( additional_data ), requires_memory ); return track_frame(frame); } diff --git a/src/frame.h b/src/frame.h index c41b658d37..55b3e1a75c 100644 --- a/src/frame.h +++ b/src/frame.h @@ -8,6 +8,8 @@ #include "core/extension.h" #include #include +#include +#include #include namespace librealsense { @@ -31,6 +33,8 @@ struct metadata_array_value }; #pragma pack( pop ) +typedef std::array< metadata_array_value, RS2_FRAME_METADATA_ACTUAL_COUNT > metadata_array; + static_assert( sizeof( metadata_array_value ) == sizeof( rs2_metadata_type ) + 1, "unexpected size for metadata array members" ); @@ -69,7 +73,7 @@ struct frame_additional_data : frame_header { uint32_t metadata_size = 0; bool fisheye_ae_mode = false; // TODO: remove in future release - std::array< uint8_t, RS2_FRAME_METADATA_ACTUAL_COUNT * sizeof( metadata_array_value ) > metadata_blob; + std::array< uint8_t, sizeof( metadata_array ) > metadata_blob = {}; rs2_time_t last_timestamp = 0; unsigned long long last_frame_number = 0; bool is_blocking = false; // when running from recording, this bit indicates @@ -83,6 +87,12 @@ struct frame_additional_data : frame_header frame_additional_data() {} + frame_additional_data( metadata_array const & metadata ) + { + metadata_size = (uint32_t) sizeof( metadata ); + memcpy( metadata_blob.data(), metadata.data(), metadata_size ); + } + frame_additional_data( rs2_time_t in_timestamp, unsigned long long in_frame_number, rs2_time_t in_system_time, diff --git a/src/hw-monitor.h b/src/hw-monitor.h index a3b87965ea..07df7a8b1c 100644 --- a/src/hw-monitor.h +++ b/src/hw-monitor.h @@ -357,7 +357,7 @@ namespace librealsense { T rv = 0; if (index + sizeof(T) >= data.size()) - throw new std::runtime_error("get_gvd_field - index out of bounds, buffer size: " + + throw std::runtime_error("get_gvd_field - index out of bounds, buffer size: " + std::to_string(data.size()) + ", index: " + std::to_string(index)); for (int i = 0; i < sizeof(T); i++) rv += data[index + i] << (i * 8); diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index a605e30b46..f813f57afc 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -487,10 +487,9 @@ namespace librealsense if(!(std::ifstream(path + "/version") >> val)) throw linux_backend_exception("Failed to read usb version specification"); - auto kvp = std::find_if(usb_spec_names.begin(),usb_spec_names.end(), - [&val](const std::pair& kpv){ return (std::string::npos !=val.find(kpv.second));}); - if (kvp != std::end(usb_spec_names)) - res = kvp->first; + auto it = usb_name_to_spec.find( val ); + if( it != usb_name_to_spec.end() ) + res = it->second; } return res; } diff --git a/src/media/ros/ros_reader.cpp b/src/media/ros/ros_reader.cpp index 102f1cf24c..cab861014d 100644 --- a/src/media/ros/ros_reader.cpp +++ b/src/media/ros/ros_reader.cpp @@ -437,9 +437,11 @@ namespace librealsense get_frame_metadata(m_file, info_topic, stream_id, image_data, additional_data); } - frame_interface* frame = m_frame_source->alloc_frame( + frame_interface * frame = m_frame_source->alloc_frame( frame_source::stream_to_frame_types(stream_id.stream_type), - msg->data.size(), additional_data, true); + msg->data.size(), + std::move( additional_data ), + true ); if (frame == nullptr) { LOG_WARNING("Failed to allocate new frame"); @@ -488,7 +490,10 @@ namespace librealsense get_frame_metadata(m_file, info_topic, stream_id, motion_data, additional_data); } - frame_interface* frame = m_frame_source->alloc_frame(RS2_EXTENSION_MOTION_FRAME, 3 * sizeof(float), additional_data, true); + frame_interface * frame = m_frame_source->alloc_frame( RS2_EXTENSION_MOTION_FRAME, + 3 * sizeof( float ), + std::move( additional_data ), + true ); if (frame == nullptr) { LOG_WARNING("Failed to allocate new frame"); @@ -634,7 +639,7 @@ namespace librealsense additional_data.timestamp = timestamp_ms.count(); - frame_interface* new_frame = m_frame_source->alloc_frame(frame_type, frame_size, additional_data, true); + frame_interface* new_frame = m_frame_source->alloc_frame(frame_type, frame_size, std::move( additional_data ), true); if (new_frame == nullptr) { LOG_WARNING("Failed to allocate new frame"); diff --git a/src/metadata-parser.h b/src/metadata-parser.h index 8e6b1e9862..6c7d752b70 100644 --- a/src/metadata-parser.h +++ b/src/metadata-parser.h @@ -58,20 +58,21 @@ namespace librealsense bool try_get(const frame& frm, rs2_metadata_type& result) const { const uint8_t* pos = frm.additional_data.metadata_blob.data(); - while (pos <= frm.additional_data.metadata_blob.data() + frm.additional_data.metadata_blob.size()) + while( pos <= frm.additional_data.metadata_blob.data() + frm.additional_data.metadata_blob.size() ) { - const rs2_frame_metadata_value* type = reinterpret_cast(pos); - pos += sizeof(rs2_frame_metadata_value); - if (_type == *type) + const rs2_frame_metadata_value* type = reinterpret_cast< const rs2_frame_metadata_value* >( pos ); + pos += sizeof( rs2_frame_metadata_value ); + if( _type == *type ) { - const rs2_metadata_type* value = reinterpret_cast(pos); - memcpy((void*)&result, (const void*)value, sizeof(*value)); + const rs2_metadata_type* value = reinterpret_cast< const rs2_metadata_type* >( pos ); + memcpy( (void*)&result, (const void*)value, sizeof( *value ) ); return true; } - pos += sizeof(rs2_metadata_type); + pos += sizeof( rs2_metadata_type ); } return false; } + rs2_frame_metadata_value _type; }; diff --git a/src/mf/mf-hid.cpp b/src/mf/mf-hid.cpp index 2607bc2eb5..be211473e6 100644 --- a/src/mf/mf-hid.cpp +++ b/src/mf/mf-hid.cpp @@ -375,7 +375,7 @@ namespace librealsense CHECK_HR(CoCreateInstance(CLSID_SensorManager, NULL, CLSCTX_INPROC_SERVER, IID_PPV_ARGS(&pSensorManager))); /* Retrieves a collection containing all sensors associated with category SENSOR_CATEGORY_ALL */ - LOG_HR(res=pSensorManager->GetSensorsByCategory(SENSOR_CATEGORY_ALL, &pSensorCollection)); + res=pSensorManager->GetSensorsByCategory(SENSOR_CATEGORY_ALL, &pSensorCollection); if (SUCCEEDED(res)) { /* Retrieves the count of sensors in the collection */ @@ -485,6 +485,9 @@ namespace librealsense safe_release(pSensor); } } + // ERROR_NOT_FOUND is normal if no sensors are available + else if( res != HRESULT_FROM_WIN32( ERROR_NOT_FOUND ) ) + LOG_HR_STR( "pSensorManager->GetSensorsByCategory(SENSOR_CATEGORY_ALL)", res ); } catch (...) { diff --git a/src/mf/mf-uvc.cpp b/src/mf/mf-uvc.cpp index ed6254b5e4..675501a82f 100644 --- a/src/mf/mf-uvc.cpp +++ b/src/mf/mf-uvc.cpp @@ -871,7 +871,12 @@ namespace librealsense //enable source CHECK_HR(MFCreateDeviceSource(_device_attrs, &_source)); LOG_HR(_source->QueryInterface(__uuidof(IAMCameraControl), reinterpret_cast(&_camera_control))); - LOG_HR(_source->QueryInterface(__uuidof(IAMVideoProcAmp), reinterpret_cast(&_video_proc))); + // The IAMVideoProcAmp interface adjusts the qualities of an incoming video signal, such as brightness, + // contrast, hue, saturation, gamma, and sharpness. + auto hr = _source->QueryInterface( __uuidof( IAMVideoProcAmp ), reinterpret_cast< void ** >( &_video_proc ) ); + // E_NOINTERFACE is expected... especially when no video camera + if( hr != E_NOINTERFACE ) + LOG_HR_STR( "QueryInterface(IAMVideoProcAmp)", hr ); //enable reader CHECK_HR(MFCreateSourceReaderFromMediaSource(_source, _reader_attrs, &_reader)); diff --git a/src/proc/formats-converter.cpp b/src/proc/formats-converter.cpp index dac63073aa..df1b2a8dfa 100644 --- a/src/proc/formats-converter.cpp +++ b/src/proc/formats-converter.cpp @@ -8,8 +8,8 @@ namespace librealsense { void formats_converter::register_converter( const std::vector< stream_profile > & source, - const std::vector< stream_profile > & target, - std::function< std::shared_ptr< processing_block >( void ) > generate_func ) + const std::vector< stream_profile > & target, + std::function< std::shared_ptr< processing_block >( void ) > generate_func ) { _pb_factories.push_back( std::make_shared< processing_block_factory >( source, target, generate_func ) ); } @@ -21,10 +21,42 @@ void formats_converter::register_converter( const processing_block_factory & pbf void formats_converter::register_converters( const std::vector< processing_block_factory > & pbfs ) { - for( auto && pbf : pbfs ) + for( auto & pbf : pbfs ) register_converter( pbf ); } +void formats_converter::clear_registered_converters() +{ + _pb_factories.clear(); +} + +void formats_converter::drop_non_basic_formats() +{ + for( size_t i = 0; i < _pb_factories.size(); ++i ) + { + const auto & source = _pb_factories[i]->get_source_info(); + const auto & target = _pb_factories[i]->get_target_info(); + if( target.size() == 1 && + source[0].format == target[0].format ) + { + // Identity, does not actually convert, keep this converter, unless it is colored infrared. + bool colored_infrared = target[0].stream == RS2_STREAM_INFRARED && source[0].format == RS2_FORMAT_UYVY; + + if( ! colored_infrared ) + continue; + } + + if( source[0].format == RS2_FORMAT_Y8I || source[0].format == RS2_FORMAT_Y12I ) + continue; // Convert interleaved formats. + + // Remove unwanted converters. Move to last element in vector and pop it out. + if( i != ( _pb_factories.size() -1 ) ) + std::swap( _pb_factories[i], _pb_factories.back() ); + _pb_factories.pop_back(); + --i; // Don't advance the counter because we reduce the vector size. + } +} + stream_profiles formats_converter::get_all_possible_profiles( const stream_profiles & raw_profiles ) { // For each profile that can be used as input check all registered factories if they can create @@ -35,42 +67,43 @@ stream_profiles formats_converter::get_all_possible_profiles( const stream_profi for( auto & raw_profile : raw_profiles ) { - for( auto && pbf : _pb_factories ) + for( auto & pbf : _pb_factories ) { - const auto && sources = pbf->get_source_info(); - for( auto && source : sources ) + const auto & sources = pbf->get_source_info(); + for( auto & source : sources ) { if( source.format == raw_profile->get_format() && ( source.stream == raw_profile->get_stream_type() || source.stream == RS2_STREAM_ANY ) ) { - const auto & targets = pbf->get_target_info(); - // targets are saved with format and type only. Updating fps and resolution before using as key - for( auto target : targets ) + // targets are saved with format, type and sometimes index. Updating fps and resolution before using as key + for( const auto & target : pbf->get_target_info() ) { - target.fps = raw_profile->get_framerate(); + // When interleaved streams are seperated to two distinct streams (e.g. sent as DDS streams), + // same converters are registered for both stream. We handle the relevant one based on index. + // Currently for infrared streams only. + if( source.stream == RS2_STREAM_INFRARED && raw_profile->get_stream_index() != target.index ) + continue; - auto && cloned_profile = clone_profile( raw_profile ); + auto cloned_profile = clone_profile( raw_profile ); cloned_profile->set_format( target.format ); - cloned_profile->set_stream_index( target.index ); //TODO - shouldn't be from_profile.index? + cloned_profile->set_stream_index( target.index ); cloned_profile->set_stream_type( target.stream ); - auto && cloned_vsp = As< video_stream_profile, stream_profile_interface >( cloned_profile ); + auto cloned_vsp = As< video_stream_profile, stream_profile_interface >( cloned_profile ); if( cloned_vsp ) { // Converter may rotate the image, invoke stream_resolution function to get actual result const auto res = target.stream_resolution( { cloned_vsp->get_width(), cloned_vsp->get_height() } ); - target.height = res.height; - target.width = res.width; - cloned_vsp->set_dims( target.width, target.height ); + cloned_vsp->set_dims( res.width, res.height ); } // Cache pbf supported profiles for efficiency in find_pbf_matching_most_profiles _pbf_supported_profiles[pbf.get()].push_back( cloned_profile ); // Cache mapping of each target profile to profiles it is converting from. - _target_profiles_to_raw_profiles[target].push_back( raw_profile ); + _target_profiles_to_raw_profiles[cloned_profile].push_back( raw_profile ); - // TODO - Duplicates in the list happen when 2 from_profiles have conversion to same target. + // TODO - Duplicates in the list happen when 2 raw_profiles have conversion to same target. // In this case it is faster to check if( _target_to_source_profiles_map[target].size() > 1 ) // rather then if( is_profile_in_list( cloned_profile, to_profiles ) ), but L500 unit-tests // fail if we change. Need to understand difference @@ -139,7 +172,7 @@ bool formats_converter::is_profile_in_list( const std::shared_ptr< stream_profil const stream_profiles & profiles ) const { // Converting to stream_profile to avoid dynamic casting to video/motion_stream_profile - const auto && is_duplicate_predicate = [&profile]( const std::shared_ptr< stream_profile_interface > & spi ) { + auto is_duplicate_predicate = [&profile]( const std::shared_ptr< stream_profile_interface > & spi ) { return to_profile( spi.get() ) == to_profile( profile.get() ); }; @@ -173,11 +206,11 @@ void formats_converter::prepare_to_convert( stream_profiles from_profiles ) } // Retrieve source profile from cached map and generate the relevant processing block. - std::unordered_set> current_resolved_reqs; + std::unordered_set< std::shared_ptr< stream_profile_interface > > current_resolved_reqs; auto best_pb = factory_of_best_match->generate(); for( const auto & from_profile : from_profiles_of_best_match ) { - auto & mapped_raw_profiles = _target_profiles_to_raw_profiles[to_profile( from_profile.get() )]; + auto & mapped_raw_profiles = _target_profiles_to_raw_profiles[from_profile]; for( const auto & raw_profile : mapped_raw_profiles ) { @@ -199,7 +232,7 @@ void formats_converter::update_target_profiles_data( const stream_profiles & fro { for( auto & from_profile : from_profiles ) { - for( auto & raw_profile : _target_profiles_to_raw_profiles[to_profile( from_profile.get() )] ) + for( auto & raw_profile : _target_profiles_to_raw_profiles[from_profile] ) { raw_profile->set_stream_index( from_profile->get_stream_index() ); raw_profile->set_unique_id( from_profile->get_unique_id() ); @@ -226,7 +259,7 @@ void formats_converter::update_target_profiles_data( const stream_profiles & fro void formats_converter::cache_from_profiles( const stream_profiles & from_profiles ) { - for( auto && from_profile : from_profiles ) + for( auto & from_profile : from_profiles ) { _format_mapping_to_from_profiles[from_profile->get_format()].push_back( from_profile ); } @@ -280,7 +313,7 @@ formats_converter::find_pbf_matching_most_profiles( const stream_profiles & from for( auto & pbf : _pb_factories ) { - stream_profiles && satisfied_req = pbf->find_satisfied_requests( from_profiles, _pbf_supported_profiles[pbf.get()] ); + stream_profiles satisfied_req = pbf->find_satisfied_requests( from_profiles, _pbf_supported_profiles[pbf.get()] ); size_t satisfied_count = satisfied_req.size(); size_t source_size = pbf->get_source_info().size(); if( satisfied_count > max_satisfied_count || @@ -308,7 +341,7 @@ void formats_converter::set_frames_callback( frame_callback_ptr callback ) _converted_frames_callback = callback; // After processing callback - const auto && output_cb = make_callback( [&]( frame_holder f ) { + auto output_cb = make_callback( [&]( frame_holder f ) { std::vector< frame_interface * > frames_to_be_processed; frames_to_be_processed.push_back( f.frame ); @@ -322,7 +355,7 @@ void formats_converter::set_frames_callback( frame_callback_ptr callback ) } // Process only frames which aren't composite. - for( auto && fr : frames_to_be_processed ) + for( auto & fr : frames_to_be_processed ) { if( ! dynamic_cast< composite_frame * >( fr ) ) { @@ -363,7 +396,7 @@ void formats_converter::convert_frame( frame_holder & f ) return; auto & converters = _raw_profile_to_converters[f->get_stream()]; - for( auto && converter : converters ) + for( auto & converter : converters ) { f->acquire(); converter->invoke( f.frame ); diff --git a/src/proc/formats-converter.h b/src/proc/formats-converter.h index c21d5361b3..17c8eb9ab3 100644 --- a/src/proc/formats-converter.h +++ b/src/proc/formats-converter.h @@ -27,6 +27,11 @@ namespace librealsense std::function< std::shared_ptr< processing_block >( void ) > generate_func ); void register_converter( const processing_block_factory & pbf ); void register_converters( const std::vector< processing_block_factory > & pbfs ); + void clear_registered_converters(); + + // Don't convert to types other then the raw camera formats (use only identity formats) + // Convert only interleaved formats (Y8I, Y12I), no colored infrared. + void drop_non_basic_formats(); stream_profiles get_all_possible_profiles( const stream_profiles & raw_profiles ); void prepare_to_convert( stream_profiles to_profiles ); @@ -49,11 +54,11 @@ namespace librealsense std::pair< std::shared_ptr< processing_block_factory >, stream_profiles > find_pbf_matching_most_profiles( const stream_profiles & profiles ); - std::shared_ptr find_cached_profile_for_frame( const frame_interface * f ); + std::shared_ptr< stream_profile_interface > find_cached_profile_for_frame( const frame_interface * f ); std::vector< std::shared_ptr< processing_block_factory > > _pb_factories; std::unordered_map< processing_block_factory *, stream_profiles > _pbf_supported_profiles; - std::unordered_map< stream_profile, stream_profiles > _target_profiles_to_raw_profiles; + std::unordered_map< std::shared_ptr< stream_profile_interface >, stream_profiles > _target_profiles_to_raw_profiles; std::unordered_map< std::shared_ptr< stream_profile_interface >, std::unordered_set< std::shared_ptr< processing_block > > > _raw_profile_to_converters; diff --git a/src/proc/processing-blocks-factory.cpp b/src/proc/processing-blocks-factory.cpp index 7d953d7267..c704bc2ea9 100644 --- a/src/proc/processing-blocks-factory.cpp +++ b/src/proc/processing-blocks-factory.cpp @@ -50,16 +50,16 @@ namespace librealsense bool processing_block_factory::operator==(const processing_block_factory & rhs) const { - const auto&& rhs_src = rhs.get_source_info(); - for (auto&& src : _source_info) + const auto & rhs_src = rhs.get_source_info(); + for (auto & src : _source_info) { auto equals = [&src](const stream_profile& prof) { return prof == src; }; if (std::none_of(begin(rhs_src), end(rhs_src), equals)) return false; } - const auto&& rhs_tgt = rhs.get_target_info(); - for (auto&& tgt : _target_info) + const auto & rhs_tgt = rhs.get_target_info(); + for (auto & tgt : _target_info) { auto equals = [&tgt](const stream_profile& prof) { return prof == tgt; }; if (std::none_of(begin(rhs_tgt), end(rhs_tgt), equals)) diff --git a/src/proc/processing-blocks-factory.h b/src/proc/processing-blocks-factory.h index 9e0a4407e9..59e588978f 100644 --- a/src/proc/processing-blocks-factory.h +++ b/src/proc/processing-blocks-factory.h @@ -25,8 +25,8 @@ namespace librealsense bool operator==(const processing_block_factory& rhs) const; - std::vector get_source_info() const { return _source_info; } - std::vector get_target_info() const { return _target_info; } + const std::vector & get_source_info() const { return _source_info; } + const std::vector & get_target_info() const { return _target_info; } std::shared_ptr generate(); static processing_block_factory create_id_pbf(rs2_format format, rs2_stream stream, int idx = 0); diff --git a/src/proc/synthetic-stream.cpp b/src/proc/synthetic-stream.cpp index a5829118a4..4bfa937eac 100644 --- a/src/proc/synthetic-stream.cpp +++ b/src/proc/synthetic-stream.cpp @@ -341,7 +341,11 @@ namespace librealsense data.system_time = _actual_source.get_time(); data.is_blocking = original->is_blocking(); - auto res = _actual_source.alloc_frame(frame_type, vid_stream->get_width() * vid_stream->get_height() * sizeof(float) * 5, data, true); + auto res + = _actual_source.alloc_frame( frame_type, + vid_stream->get_width() * vid_stream->get_height() * sizeof( float ) * 5, + std::move( data ), + true ); if (!res) throw wrong_api_call_sequence_exception("Out of frame resources!"); res->set_sensor(original->get_sensor()); res->set_stream(stream); @@ -402,7 +406,7 @@ namespace librealsense auto of = dynamic_cast(original); frame_additional_data data = of->additional_data; - auto res = _actual_source.alloc_frame(frame_type, stride * height, data, true); + auto res = _actual_source.alloc_frame( frame_type, stride * height, std::move( data ), true ); if (!res) throw wrong_api_call_sequence_exception("Out of frame resources!"); vf = dynamic_cast(res); vf->metadata_parsers = of->metadata_parsers; @@ -425,7 +429,7 @@ namespace librealsense { auto of = dynamic_cast(original); frame_additional_data data = of->additional_data; - auto res = _actual_source.alloc_frame(frame_type, of->get_frame_data_size(), data, true); + auto res = _actual_source.alloc_frame( frame_type, of->get_frame_data_size(), std::move( data ), true ); if (!res) throw wrong_api_call_sequence_exception("Out of frame resources!"); auto mf = dynamic_cast(res); mf->metadata_parsers = of->metadata_parsers; @@ -471,7 +475,10 @@ namespace librealsense for (auto&& f : holders) req_size += get_embeded_frames_size(f.frame); - auto res = _actual_source.alloc_frame(RS2_EXTENSION_COMPOSITE_FRAME, req_size * sizeof(rs2_frame*), d, true); + auto res = _actual_source.alloc_frame( RS2_EXTENSION_COMPOSITE_FRAME, + req_size * sizeof( rs2_frame * ), + std::move( d ), + true ); if (!res) return nullptr; auto cf = static_cast(res); diff --git a/src/realsense.def b/src/realsense.def index ab2d168aaf..17bdb49197 100644 --- a/src/realsense.def +++ b/src/realsense.def @@ -2,6 +2,7 @@ LIBRARY EXPORTS rs2_create_context + rs2_create_context_ex rs2_delete_context rs2_create_recording_context rs2_create_mock_context diff --git a/src/rs.cpp b/src/rs.cpp index 7d7d8f5c73..1c5c6e3f01 100644 --- a/src/rs.cpp +++ b/src/rs.cpp @@ -176,6 +176,15 @@ rs2_context* rs2_create_context(int api_version, rs2_error** error) BEGIN_API_CA } HANDLE_EXCEPTIONS_AND_RETURN(nullptr, api_version) +rs2_context* rs2_create_context_ex(int api_version, const char * json_settings, rs2_error** error) BEGIN_API_CALL +{ + verify_version_compatibility(api_version); + + return new rs2_context{ + std::make_shared< librealsense::context >( json_settings ) }; +} +HANDLE_EXCEPTIONS_AND_RETURN(nullptr, api_version, json_settings) + void rs2_delete_context(rs2_context* context) BEGIN_API_CALL { VALIDATE_NOT_NULL(context); diff --git a/src/sensor.cpp b/src/sensor.cpp index 0fb49518d8..f531627ec0 100644 --- a/src/sensor.cpp +++ b/src/sensor.cpp @@ -13,6 +13,7 @@ #include "device-calibration.h" #include +#include #include #include @@ -454,7 +455,7 @@ void log_callback_end( uint32_t fps, frame_holder fh = _source.alloc_frame( frame_source::stream_to_frame_types( req_profile_base->get_stream_type() ), expected_size, - fr->additional_data, + std::move( fr->additional_data ), true ); auto diff = environment::get_instance().get_time_service()->get_time() - system_time; if( diff > 10 ) @@ -998,7 +999,8 @@ void log_callback_end( uint32_t fps, last_frame_number = frame_counter; last_timestamp = timestamp; - frame_holder frame = _source.alloc_frame(RS2_EXTENSION_MOTION_FRAME, data_size, fr->additional_data, true); + frame_holder frame + = _source.alloc_frame( RS2_EXTENSION_MOTION_FRAME, data_size, std::move( fr->additional_data ), true ); memcpy( (void *)frame->get_frame_data(), sensor_data.fo.pixels, sizeof( byte ) * sensor_data.fo.frame_size ); @@ -1353,8 +1355,13 @@ void log_callback_end( uint32_t fps, stream_profiles synthetic_sensor::init_stream_profiles() { - stream_profiles from_profiles = _raw_sensor->get_stream_profiles( PROFILE_TAG_ANY | PROFILE_TAG_DEBUG ); - stream_profiles result_profiles = _formats_converter.get_all_possible_profiles( from_profiles ); + stream_profiles raw_profiles = _raw_sensor->get_stream_profiles( PROFILE_TAG_ANY | PROFILE_TAG_DEBUG ); + if( should_use_basic_formats() ) + { + _formats_converter.drop_non_basic_formats(); + } + + stream_profiles result_profiles = _formats_converter.get_all_possible_profiles( raw_profiles ); _owner->tag_profiles( result_profiles ); sort_profiles( &result_profiles ); @@ -1362,12 +1369,12 @@ void log_callback_end( uint32_t fps, return result_profiles; } - void synthetic_sensor::open(const stream_profiles& requests) + void synthetic_sensor::open(const stream_profiles & requests) { std::lock_guard lock(_synthetic_configure_lock); _formats_converter.prepare_to_convert( requests ); - + const auto & resolved_req = _formats_converter.get_active_source_profiles(); std::vector< std::shared_ptr< processing_block > > active_pbs = _formats_converter.get_active_converters(); for( auto & pb : active_pbs ) @@ -1376,7 +1383,7 @@ void log_callback_end( uint32_t fps, _raw_sensor->set_source_owner(this); try { - _raw_sensor->open(resolved_req); + _raw_sensor->open( resolved_req ); } catch (const std::runtime_error& e) { @@ -1426,11 +1433,10 @@ void log_callback_end( uint32_t fps, set_frames_callback(callback); _formats_converter.set_frames_callback( callback ); - // Invoke processing blocks callback - const auto&& process_cb = make_callback([&, this](frame_holder f) { + auto process_cb = make_callback( [&, this]( frame_holder f ) { _formats_converter.convert_frame( f ); - }); + } ); // Call the processing block on the frame _raw_sensor->start(process_cb); @@ -1518,4 +1524,14 @@ void log_callback_end( uint32_t fps, { snapshot = std::make_shared(); } + + bool synthetic_sensor::should_use_basic_formats() const + { + if( _owner->get_context() ) + { + return rsutils::json::get< bool >( _owner->get_context()->get_settings(), std::string( "use-basic-formats", 17 ), false ); + } + + return false; + } } diff --git a/src/sensor.h b/src/sensor.h index aa2ccbfd34..5185dabd4a 100644 --- a/src/sensor.h +++ b/src/sensor.h @@ -238,16 +238,13 @@ namespace librealsense bool is_streaming() const override; bool is_opened() const override; - protected: - void add_source_profiles_missing_data(); - private: - stream_profiles resolve_requests(const stream_profiles& requests); - std::shared_ptr filter_frame_by_requests(const frame_interface* f); void sort_profiles(stream_profiles * profiles); void register_processing_block_options(const processing_block& pb); void unregister_processing_block_options(const processing_block& pb); + bool should_use_basic_formats() const; + std::mutex _synthetic_configure_lock; frame_callback_ptr _post_process_callback; diff --git a/src/serialized-utilities.h b/src/serialized-utilities.h index 42ac6ff781..55d04fccf2 100644 --- a/src/serialized-utilities.h +++ b/src/serialized-utilities.h @@ -1,11 +1,11 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2021 Intel Corporation. All Rights Reserved. +// Copyright(c) 2022 Intel Corporation. All Rights Reserved. #pragma once #include #include #include -#include <../third-party/json.hpp> +#include namespace librealsense diff --git a/src/software-device.cpp b/src/software-device.cpp index 62c3ff0da7..9e1586ec3e 100644 --- a/src/software-device.cpp +++ b/src/software-device.cpp @@ -5,15 +5,24 @@ #include "stream.h" #include +#include +#include + +using rsutils::deferred; namespace librealsense { software_device::software_device() - : device(std::make_shared(backend_type::standard), {}, false), - _user_destruction_callback() + : device( std::make_shared< context >( nlohmann::json( { { "dds-discovery", false } } ) ), {}, false ) + , _user_destruction_callback() + { + register_info( RS2_CAMERA_INFO_NAME, "Software-Device" ); + } + + software_device::software_device( std::shared_ptr< context > ctx ) + : device( ctx, {}, false ) { - register_info(RS2_CAMERA_INFO_NAME, "Software-Device"); } librealsense::software_device::~software_device() @@ -52,7 +61,7 @@ namespace librealsense _user_destruction_callback = std::move(callback); } - software_sensor& software_device::get_software_sensor(int index) + software_sensor& software_device::get_software_sensor( size_t index) { if (index >= _software_sensors.size()) { @@ -111,14 +120,6 @@ namespace librealsense std::shared_ptr software_sensor::add_video_stream(rs2_video_stream video_stream, bool is_default) { - - auto currProfile = find_profile_by_uid(video_stream.uid); - if (currProfile) - { - //LOG_WARNING("Video stream unique ID already exist!"); - //throw rs2::error("Stream unique ID already exist!"); - } - auto profile = std::make_shared( platform::stream_profile{ (uint32_t)video_stream.width, (uint32_t)video_stream.height, (uint32_t)video_stream.fps, 0 }); profile->set_dims(video_stream.width, video_stream.height); @@ -136,13 +137,6 @@ namespace librealsense std::shared_ptr software_sensor::add_motion_stream(rs2_motion_stream motion_stream, bool is_default) { - auto currProfile = find_profile_by_uid(motion_stream.uid); - if (currProfile) - { - LOG_WARNING("Motion stream unique ID already exist!"); - throw rs2::error("Stream unique ID already exist!"); - } - auto profile = std::make_shared( platform::stream_profile{ 0, 0, (uint32_t)motion_stream.fps, 0 }); profile->set_format(motion_stream.fmt); @@ -159,13 +153,6 @@ namespace librealsense std::shared_ptr software_sensor::add_pose_stream(rs2_pose_stream pose_stream, bool is_default) { - auto currProfile = find_profile_by_uid(pose_stream.uid); - if (currProfile) - { - LOG_WARNING("Pose stream unique ID already exist!"); - throw rs2::error("Stream unique ID already exist!"); - } - auto profile = std::make_shared( platform::stream_profile{ 0, 0, (uint32_t)pose_stream.fps, 0 }); if (!profile) @@ -182,20 +169,6 @@ namespace librealsense return std::move(profile); } - std::shared_ptr software_sensor::find_profile_by_uid(int uid) - { - auto filtFunc = [&](std::shared_ptr profile) - { - return profile->get_unique_id() == uid; - }; - - auto profile = std::find_if(_profiles.begin(), _profiles.end(), filtFunc); - if ( profile != _profiles.end() ) { - return *profile; - } else { - return std::shared_ptr(); - } - } bool software_sensor::extend_to(rs2_extension extension_type, void ** ptr) { @@ -276,93 +249,115 @@ namespace librealsense } - void software_sensor::on_video_frame( rs2_software_video_frame const & software_frame ) + void software_sensor::erase_metadata( rs2_frame_metadata_value key ) { - if (!_is_streaming) { - software_frame.deleter(software_frame.pixels); - return; - } - - frame_additional_data data; - data.timestamp = software_frame.timestamp; - data.timestamp_domain = software_frame.domain; - data.frame_number = software_frame.frame_number; - data.depth_units = software_frame.depth_units; + _metadata_map[key].is_valid = false; + } - data.metadata_size = (uint32_t)( _metadata_map.size() * sizeof( metadata_array_value ) ); - memcpy( data.metadata_blob.data(), _metadata_map.data(), data.metadata_size ); - rs2_extension extension = software_frame.profile->profile->get_stream_type() == RS2_STREAM_DEPTH ? - RS2_EXTENSION_DEPTH_FRAME : RS2_EXTENSION_VIDEO_FRAME; + frame_interface * software_sensor::allocate_new_frame( rs2_extension extension, + stream_profile_interface * profile, + frame_additional_data && data ) + { + auto frame = _source.alloc_frame( extension, 0, std::move( data ), false ); + if( ! frame ) + { + LOG_WARNING( "Failed to allocate frame " << data.frame_number << " type " << extension ); + } + else + { + frame->set_stream( std::dynamic_pointer_cast< stream_profile_interface >( profile->shared_from_this() ) ); + } + return frame; + } - auto frame = _source.alloc_frame(extension, 0, data, false); - if (!frame) + + frame_interface * software_sensor::allocate_new_video_frame( video_stream_profile_interface * profile, + int stride, + int bpp, + frame_additional_data && data ) + { + auto frame = allocate_new_frame( profile->get_stream_type() == RS2_STREAM_DEPTH ? RS2_EXTENSION_DEPTH_FRAME + : RS2_EXTENSION_VIDEO_FRAME, + profile, + std::move( data ) ); + if( frame ) { - LOG_WARNING("Dropped video frame. alloc_frame(...) returned nullptr"); - return; + auto vid_frame = dynamic_cast< video_frame * >( frame ); + vid_frame->assign( profile->get_width(), profile->get_height(), stride, bpp * 8 ); + auto sd = dynamic_cast< software_device * >( _owner ); + sd->register_extrinsic( *profile ); } - auto vid_profile = dynamic_cast(software_frame.profile->profile); - auto vid_frame = dynamic_cast(frame); - vid_frame->assign(vid_profile->get_width(), vid_profile->get_height(), software_frame.stride, software_frame.bpp * 8); + return frame; + } - frame->set_stream(std::dynamic_pointer_cast(software_frame.profile->profile->shared_from_this())); - frame->attach_continuation(frame_continuation{ [=]() { - software_frame.deleter(software_frame.pixels); - }, software_frame.pixels }); - auto sd = dynamic_cast(_owner); - sd->register_extrinsic(*vid_profile); - _source.invoke_callback(frame); + void software_sensor::invoke_new_frame( frame_holder && frame, + void const * pixels, + std::function< void() > on_release ) + { + // The frame pixels/data are stored in the continuation object! + if( pixels ) + frame->attach_continuation( frame_continuation( on_release, pixels ) ); + _source.invoke_callback( std::move( frame ) ); } - void software_sensor::on_motion_frame( rs2_software_motion_frame const & software_frame ) + + void software_sensor::on_video_frame( rs2_software_video_frame const & software_frame ) { - if (!_is_streaming) return; + deferred on_release( [deleter = software_frame.deleter, data = software_frame.pixels]() { deleter( data ); } ); + + stream_profile_interface * profile = software_frame.profile->profile; + auto vid_profile = dynamic_cast< video_stream_profile_interface * >( profile ); + if( ! vid_profile ) + throw invalid_value_exception( "Non-video profile provided to on_video_frame" ); - frame_additional_data data; + if( ! _is_streaming ) + return; + + frame_additional_data data( _metadata_map ); data.timestamp = software_frame.timestamp; data.timestamp_domain = software_frame.domain; data.frame_number = software_frame.frame_number; + data.depth_units = software_frame.depth_units; - data.metadata_size = (uint32_t) (_metadata_map.size() * sizeof( metadata_array_value )); - memcpy( data.metadata_blob.data(), _metadata_map.data(), data.metadata_size ); + auto frame + = allocate_new_video_frame( vid_profile, software_frame.stride, software_frame.bpp, std::move( data ) ); + if( frame ) + invoke_new_frame( frame, software_frame.pixels, on_release.detach() ); + } - auto frame = _source.alloc_frame(RS2_EXTENSION_MOTION_FRAME, 0, data, false); - if (!frame) - { - LOG_WARNING("Dropped motion frame. alloc_frame(...) returned nullptr"); + void software_sensor::on_motion_frame( rs2_software_motion_frame const & software_frame ) + { + deferred on_release( [deleter = software_frame.deleter, data = software_frame.data]() { deleter( data ); } ); + if( ! _is_streaming ) return; - } - frame->set_stream(std::dynamic_pointer_cast(software_frame.profile->profile->shared_from_this())); - frame->attach_continuation(frame_continuation{ [=]() { - software_frame.deleter(software_frame.data); - }, software_frame.data }); - _source.invoke_callback(frame); + + frame_additional_data data( _metadata_map ); + data.timestamp = software_frame.timestamp; + data.timestamp_domain = software_frame.domain; + data.frame_number = software_frame.frame_number; + + auto frame + = allocate_new_frame( RS2_EXTENSION_MOTION_FRAME, software_frame.profile->profile, std::move( data ) ); + if( frame ) + invoke_new_frame( frame, software_frame.data, on_release.detach() ); } void software_sensor::on_pose_frame( rs2_software_pose_frame const & software_frame ) { - if (!_is_streaming) return; + deferred on_release( [deleter = software_frame.deleter, data = software_frame.data]() { deleter( data ); } ); + if( ! _is_streaming ) + return; - frame_additional_data data; + frame_additional_data data( _metadata_map ); data.timestamp = software_frame.timestamp; data.timestamp_domain = software_frame.domain; data.frame_number = software_frame.frame_number; - data.metadata_size = (uint32_t) (_metadata_map.size() * sizeof( metadata_array_value )); - memcpy( data.metadata_blob.data(), _metadata_map.data(), data.metadata_size ); - - auto frame = _source.alloc_frame(RS2_EXTENSION_POSE_FRAME, 0, data, false); - if (!frame) - { - LOG_WARNING("Dropped pose frame. alloc_frame(...) returned nullptr"); - return; - } - frame->set_stream(std::dynamic_pointer_cast(software_frame.profile->profile->shared_from_this())); - frame->attach_continuation(frame_continuation{ [=]() { - software_frame.deleter(software_frame.data); - }, software_frame.data }); - _source.invoke_callback(frame); + auto frame = allocate_new_frame( RS2_EXTENSION_POSE_FRAME, software_frame.profile->profile, std::move( data ) ); + if( frame ) + invoke_new_frame( frame, software_frame.data, on_release.detach() ); } void software_sensor::on_notification( rs2_software_notification const & notif ) @@ -370,7 +365,6 @@ namespace librealsense notification n{ notif.category, notif.type, notif.severity, notif.description }; n.serialized_data = notif.serialized_data; _notifications_processor->raise_notification(n); - } void software_sensor::add_read_only_option(rs2_option option, float val) @@ -393,5 +387,14 @@ namespace librealsense register_option(option, (is_writable? std::make_shared(range) : std::make_shared(range))); } -} + + void software_recommended_proccesing_blocks::add_processing_block( std::shared_ptr< processing_block_interface > const & block ) + { + if( ! block ) + throw invalid_value_exception( "trying to add an empty software processing block" ); + + _blocks.push_back( block ); + } + +} // namespace librealsense diff --git a/src/software-device.h b/src/software-device.h index e36302acab..a09af6d484 100644 --- a/src/software-device.h +++ b/src/software-device.h @@ -11,16 +11,18 @@ namespace librealsense { class software_sensor; class software_device_info; + class video_stream_profile_interface; class software_device : public device { public: software_device(); + software_device( std::shared_ptr< context > ctx ); virtual ~software_device(); software_sensor& add_software_sensor(const std::string& name); - software_sensor& get_software_sensor(int index); + software_sensor& get_software_sensor( size_t index); void set_matcher_type(rs2_matchers matcher); @@ -37,7 +39,7 @@ namespace librealsense void register_destruction_callback(software_device_destruction_callback_ptr); - private: + protected: std::vector> _software_sensors; librealsense::software_device_destruction_callback_ptr _user_destruction_callback; rs2_matchers _matcher = RS2_MATCHER_DEFAULT; @@ -83,6 +85,8 @@ namespace librealsense } ~software_recommended_proccesing_blocks() override {} + void add_processing_block( std::shared_ptr< processing_block_interface > const & block ); + private: processing_blocks _blocks; }; @@ -92,9 +96,9 @@ namespace librealsense public: software_sensor(std::string name, software_device* owner); - std::shared_ptr add_video_stream(rs2_video_stream video_stream, bool is_default=false); - std::shared_ptr add_motion_stream(rs2_motion_stream motion_stream, bool is_default = false); - std::shared_ptr add_pose_stream(rs2_pose_stream pose_stream, bool is_default = false); + virtual std::shared_ptr add_video_stream(rs2_video_stream video_stream, bool is_default = false); + virtual std::shared_ptr add_motion_stream(rs2_motion_stream motion_stream, bool is_default = false); + virtual std::shared_ptr add_pose_stream(rs2_pose_stream pose_stream, bool is_default = false); bool extend_to(rs2_extension extension_type, void** ptr) override; @@ -113,11 +117,27 @@ namespace librealsense void add_read_only_option(rs2_option option, float val); void update_read_only_option(rs2_option option, float val); void add_option(rs2_option option, option_range range, bool is_writable); - void set_metadata(rs2_frame_metadata_value key, rs2_metadata_type value); + void set_metadata( rs2_frame_metadata_value key, rs2_metadata_type value ); + void erase_metadata( rs2_frame_metadata_value key ); + + protected: + frame_interface * allocate_new_frame( rs2_extension, stream_profile_interface *, frame_additional_data && ); + frame_interface * allocate_new_video_frame( video_stream_profile_interface *, int stride, int bpp, frame_additional_data && ); + void invoke_new_frame( frame_holder &&, void const * pixels, std::function< void() > on_release ); + + metadata_array _metadata_map; + + processing_blocks get_recommended_processing_blocks() const override + { + return _pbs.get_recommended_processing_blocks(); + } + + software_recommended_proccesing_blocks & get_software_recommended_proccesing_blocks() { return _pbs; } + + stream_profiles _profiles; + private: friend class software_device; - stream_profiles _profiles; - std::array< metadata_array_value, RS2_FRAME_METADATA_ACTUAL_COUNT > _metadata_map; uint64_t _unique_id; class stereo_extension : public depth_stereo_sensor @@ -161,8 +181,6 @@ namespace librealsense lazy _depth_extension; software_recommended_proccesing_blocks _pbs; - - std::shared_ptr find_profile_by_uid(int uid); }; MAP_EXTENSION(RS2_EXTENSION_SOFTWARE_SENSOR, software_sensor); MAP_EXTENSION(RS2_EXTENSION_SOFTWARE_DEVICE, software_device); diff --git a/src/source.cpp b/src/source.cpp index 4d6dd035a5..eca8b56c0a 100644 --- a/src/source.cpp +++ b/src/source.cpp @@ -88,11 +88,15 @@ namespace librealsense _metadata_parsers.reset(); } - frame_interface* frame_source::alloc_frame(rs2_extension type, size_t size, frame_additional_data additional_data, bool requires_memory) const + frame_interface * frame_source::alloc_frame( rs2_extension type, + size_t size, + frame_additional_data && additional_data, + bool requires_memory ) const { auto it = _archive.find(type); - if (it == _archive.end()) throw wrong_api_call_sequence_exception("Requested frame type is not supported!"); - return it->second->alloc_and_track(size, additional_data, requires_memory); + if( it == _archive.end() ) + throw wrong_api_call_sequence_exception( "Requested frame type is not supported!" ); + return it->second->alloc_and_track( size, std::move( additional_data ), requires_memory ); } void frame_source::set_sensor(const std::shared_ptr& s) diff --git a/src/source.h b/src/source.h index 61febc92f0..120cfa2767 100644 --- a/src/source.h +++ b/src/source.h @@ -25,7 +25,10 @@ namespace librealsense std::shared_ptr