diff --git a/CMake/global_config.cmake b/CMake/global_config.cmake index 7a7155b1a7..0be63670a9 100644 --- a/CMake/global_config.cmake +++ b/CMake/global_config.cmake @@ -69,6 +69,11 @@ macro(global_set_flags) include(libusb_config) endif() + if(BUILD_NETWORK_DEVICE) + add_definitions(-DNET_DEVICE) + set(LRS_NET_TARGET realsense2-net) + endif() + add_definitions(-D${BACKEND} -DUNICODE) endmacro() diff --git a/CMake/lrs_options.cmake b/CMake/lrs_options.cmake index d3100a4a8c..2341870d53 100644 --- a/CMake/lrs_options.cmake +++ b/CMake/lrs_options.cmake @@ -27,6 +27,7 @@ option(BUILD_OPENNI2_BINDINGS "Build OpenNI bindings" OFF) option(IMPORT_DEPTH_CAM_FW "Download the latest firmware for the depth cameras" ON) option(BUILD_CV_KINFU_EXAMPLE "Build OpenCV KinectFusion example" OFF) option(FORCE_RSUSB_BACKEND "Use RS USB backend, mandatory for Win7/MacOS/Android, optional for Linux" OFF) +option(BUILD_NETWORK_DEVICE "Build Network Device support" OFF) option(FORCE_LIBUVC "Explicitly turn-on libuvc backend - deprecated, use FORCE_RSUSB_BACKEND instead" OFF) option(FORCE_WINUSB_UVC "Explicitly turn-on winusb_uvc (for win7) backend - deprecated, use FORCE_RSUSB_BACKEND instead" OFF) option(ANDROID_USB_HOST_UVC "Build UVC backend for Android - deprecated, use FORCE_RSUSB_BACKEND instead" OFF) \ No newline at end of file diff --git a/CMake/realsense2-netConfig.cmake.in b/CMake/realsense2-netConfig.cmake.in new file mode 100644 index 0000000000..2832e89cdd --- /dev/null +++ b/CMake/realsense2-netConfig.cmake.in @@ -0,0 +1,12 @@ +@PACKAGE_INIT@ + +set(realsense2-net_VERSION_MAJOR "@REALSENSE_VERSION_MAJOR@") +set(realsense2-net_VERSION_MINOR "@REALSENSE_VERSION_MINOR@") +set(realsense2-net_VERSION_PATCH "@REALSENSE_VERSION_PATCH@") + +set(realsense2-net_VERSION ${realsense2-net_VERSION_MAJOR}.${realsense2-net_VERSION_MINOR}.${realsense2-gl_VERSION_PATCH}) + +set_and_check(realsense2-net_INCLUDE_DIR "@PACKAGE_CMAKE_INSTALL_INCLUDEDIR@") + +include("${CMAKE_CURRENT_LIST_DIR}/realsense2-netTargets.cmake") +set(realsense2-net_LIBRARY realsense2-net::realsense2-net) diff --git a/CMake/unix_config.cmake b/CMake/unix_config.cmake index 2ecd1ed02a..0de80e38a9 100644 --- a/CMake/unix_config.cmake +++ b/CMake/unix_config.cmake @@ -10,6 +10,7 @@ macro(os_set_flags) if(${MACHINE} MATCHES "arm-linux-gnueabihf") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -mfpu=neon -mfloat-abi=hard -ftree-vectorize -latomic") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -mfloat-abi=hard -ftree-vectorize -latomic") + add_definitions(-DRASPBERRY_PI) elseif(${MACHINE} MATCHES "aarch64-linux-gnu") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -mstrict-align -ftree-vectorize") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mstrict-align -ftree-vectorize") diff --git a/CMakeLists.txt b/CMakeLists.txt index 16ced1bbc8..a60838a3ed 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -87,6 +87,11 @@ if(BUILD_UNIT_TESTS) add_subdirectory(unit-tests) endif() +if(BUILD_NETWORK_DEVICE) + add_subdirectory(src/ethernet) + add_subdirectory(src/compression) +endif() + if(BUILD_WITH_TM2) add_tm2() endif() diff --git a/common/model-views.cpp b/common/model-views.cpp index 0d4c1206a2..25567ca16e 100644 --- a/common/model-views.cpp +++ b/common/model-views.cpp @@ -5339,8 +5339,9 @@ namespace rs2 // draw device header //////////////////////////////////////// const bool is_playback_device = dev.is(); + bool is_ip_device = dev.supports(RS2_CAMERA_INFO_IP_ADDRESS); auto header_h = panel_height; - if (is_playback_device) header_h += 15; + if (is_playback_device || is_ip_device) header_h += 15; ImColor device_header_background_color = title_color; const float left_space = 3.f; @@ -5372,28 +5373,39 @@ namespace rs2 std::stringstream ss; if(dev.supports(RS2_CAMERA_INFO_NAME)) ss << dev.get_info(RS2_CAMERA_INFO_NAME); - ImGui::Text(" %s", ss.str().c_str()); - if (dev.supports(RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR)) + if(is_ip_device) { - std::string desc = dev.get_info(RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR); - ss.str(""); - ss << " " << textual_icons::usb_type << " " << desc; - ImGui::SameLine(); - if (!starts_with(desc, "3.")) ImGui::PushStyleColor(ImGuiCol_Text, yellow); - else ImGui::PushStyleColor(ImGuiCol_Text, light_grey); - ImGui::Text(" %s", ss.str().c_str()); - ImGui::PopStyleColor(); - ss.str(""); - ss << "The camera was detected by the OS as connected to a USB " << desc << " port"; + ImGui::Text(" %s", ss.str().substr(0, ss.str().find("\n IP Device")).c_str()); + ImGui::PushFont(window.get_font()); - ImGui::PushStyleColor(ImGuiCol_Text, light_grey); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip(" %s", ss.str().c_str()); - ImGui::PopStyleColor(); + ImGui::Text("\tNetwork Device at %s", dev.get_info(RS2_CAMERA_INFO_IP_ADDRESS)); ImGui::PopFont(); } + else + { + ImGui::Text(" %s", ss.str().c_str()); - + if (dev.supports(RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR)) + { + std::string desc = dev.get_info(RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR); + ss.str(""); + ss << " " << textual_icons::usb_type << " " << desc; + ImGui::SameLine(); + if (!starts_with(desc, "3.")) ImGui::PushStyleColor(ImGuiCol_Text, yellow); + else ImGui::PushStyleColor(ImGuiCol_Text, light_grey); + ImGui::Text(" %s", ss.str().c_str()); + ImGui::PopStyleColor(); + ss.str(""); + ss << "The camera was detected by the OS as connected to a USB " << desc << " port"; + ImGui::PushFont(window.get_font()); + ImGui::PushStyleColor(ImGuiCol_Text, light_grey); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(" %s", ss.str().c_str()); + ImGui::PopStyleColor(); + ImGui::PopFont(); + } + } + //ImGui::Text(" %s", dev.get_info(RS2_CAMERA_INFO_NAME)); ImGui::PopFont(); diff --git a/common/model-views.h b/common/model-views.h index 728beb1aac..4a9fb8629d 100644 --- a/common/model-views.h +++ b/common/model-views.h @@ -172,6 +172,8 @@ namespace rs2 static const char* show_map_ruler { "viewer_model.show_map_ruler" }; static const char* show_stream_details { "viewer_model.show_stream_details" }; static const char* metric_system { "viewer_model.metric_system" }; + + static const char* last_ip { "viewer_model.last_ip" }; } namespace window { diff --git a/config/librealsense-net.pc.in b/config/librealsense-net.pc.in new file mode 100644 index 0000000000..18a204f8a2 --- /dev/null +++ b/config/librealsense-net.pc.in @@ -0,0 +1,17 @@ +prefix=@CMAKE_INSTALL_PREFIX@ +exec_prefix=${prefix} +includedir=${prefix}/include +#TODO: libdir=${exec_prefix}/lib@MULTI_ARCH_SUFFIX@ +libdir= ${prefix}/lib/x86_64-linux-gnu + +Name: realsense2-gl +Description: Intel(R) RealSense(tm) Network Device Extension Module +Version: @REALSENSE-NET_VERSION_STRING@ +URL: https://github.com/IntelRealSense/librealsense +Requires.private: @LRS_LIB_NAME@ +Libs: -L${libdir} -l@LRS_LIB_NAME@ +Libs.private: @LRS_PKG_LIBS@ +Cflags: -I${includedir} + +#TODO check -Wl -Bdynamic +#Libs: -L${libdir} -Wl,-Bdynamic -lrealsense diff --git a/include/librealsense2-net/rs_net.h b/include/librealsense2-net/rs_net.h new file mode 100644 index 0000000000..d07b77f248 --- /dev/null +++ b/include/librealsense2-net/rs_net.h @@ -0,0 +1,29 @@ +/* License: Apache 2.0. See LICENSE file in root directory. + Copyright(c) 2020 Intel Corporation. All Rights Reserved. */ + +/** \file rs_net.h +* \ +* Exposes RealSense network device functionality for C compilers +*/ + +#ifndef LIBREALSENSE_RS2_NET_H +#define LIBREALSENSE_RS2_NET_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "librealsense2/rs.h" + +/** + * Net device is a rs2_device that can be stream and be contolled remotely over network + * \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] address remote devce ip address. should be the address of the hosting device + * \param[out] error if non-null, receives any error that occurs during this call, otherwise, errors are ignored + */ +rs2_device* rs2_create_net_device(int api_version, const char* address, rs2_error** error); + +#ifdef __cplusplus +} +#endif +#endif diff --git a/include/librealsense2-net/rs_net.hpp b/include/librealsense2-net/rs_net.hpp new file mode 100644 index 0000000000..77279f2257 --- /dev/null +++ b/include/librealsense2-net/rs_net.hpp @@ -0,0 +1,47 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#ifndef LIBREALSENSE_RS2_NET_HPP +#define LIBREALSENSE_RS2_NET_HPP + +#include +#include "rs_net.h" + +#include + +namespace rs2 +{ + class net_device : public rs2::device + { + public: + net_device(const std::string& address) : rs2::device(init(address)) { } + + /** + * Add network device to existing context. + * Any future queries on the context will return this device. + * This operation cannot be undone (except for destroying the context) + * + * \param[in] ctx context to add the device to + */ + void add_to(context& ctx) + { + rs2_error* e = nullptr; + rs2_context_add_software_device(((std::shared_ptr)ctx).get(), _dev.get(), &e); + error::handle(e); + } + + + private: + std::shared_ptr init(const std::string& address) + { + rs2_error* e = nullptr; + auto dev = std::shared_ptr( + rs2_create_net_device(RS2_API_VERSION, address.c_str(), &e), + rs2_delete_device); + error::handle(e); + + return dev; + } + }; +} +#endif // LIBREALSENSE_RS2_NET_HPP diff --git a/include/librealsense2/h/rs_sensor.h b/include/librealsense2/h/rs_sensor.h index abbdce9cba..c02c02edb6 100644 --- a/include/librealsense2/h/rs_sensor.h +++ b/include/librealsense2/h/rs_sensor.h @@ -33,6 +33,7 @@ typedef enum rs2_camera_info { RS2_CAMERA_INFO_PRODUCT_LINE , /**< Device product line D400/SR300/L500/T200 */ RS2_CAMERA_INFO_ASIC_SERIAL_NUMBER , /**< ASIC serial number */ RS2_CAMERA_INFO_FIRMWARE_UPDATE_ID , /**< Firmware update ID */ + RS2_CAMERA_INFO_IP_ADDRESS , /**< IP address for remote camera. */ RS2_CAMERA_INFO_COUNT /**< Number of enumeration values. Not a valid input: intended to be used in for-loops. */ } rs2_camera_info; const char* rs2_camera_info_to_string(rs2_camera_info info); diff --git a/include/librealsense2/hpp/rs_internal.hpp b/include/librealsense2/hpp/rs_internal.hpp index dc135ee07f..076a49c6a0 100644 --- a/include/librealsense2/hpp/rs_internal.hpp +++ b/include/librealsense2/hpp/rs_internal.hpp @@ -259,23 +259,25 @@ namespace rs2 class software_device : public device { - std::shared_ptr create_device_ptr() + std::shared_ptr create_device_ptr(std::function deleter) { rs2_error* e = nullptr; std::shared_ptr dev( rs2_create_software_device(&e), - rs2_delete_device); + deleter); error::handle(e); return dev; } public: - software_device() - : device(create_device_ptr()) - {} + software_device(std::function deleter = &rs2_delete_device) + : device(create_device_ptr(deleter)) + { + this->set_destruction_callback([]{}); + } software_device(std::string name) - : device(create_device_ptr()) + : device(create_device_ptr(&rs2_delete_device)) { update_info(RS2_CAMERA_INFO_NAME, name); } diff --git a/scripts/setup_network_queues.sh b/scripts/setup_network_queues.sh new file mode 100755 index 0000000000..2a9bda40ee --- /dev/null +++ b/scripts/setup_network_queues.sh @@ -0,0 +1,6 @@ +#!/bin/bash -e + +echo 8388608 > /proc/sys/net/core/wmem_default +echo 8388608 > /proc/sys/net/core/rmem_default + +echo "Setting-up network queues successfully changed" diff --git a/src/compression/CMakeLists.txt b/src/compression/CMakeLists.txt new file mode 100644 index 0000000000..9f959c04dc --- /dev/null +++ b/src/compression/CMakeLists.txt @@ -0,0 +1,65 @@ +# License: Apache 2.0. See LICENSE file in root directory. +# Copyright(c) 2019 Intel Corporation. All Rights Reserved. +cmake_minimum_required(VERSION 3.1.0) + +project(realsense2-compression VERSION 1.0.0 LANGUAGES CXX C) + +# Save the command line compile commands in the build output +set(CMAKE_EXPORT_COMPILE_COMMANDS 1) + +set(DEPENDENCIES ${DEPENDENCIES} realsense2) + +file(GLOB COMPRESSION_SOURCES + "*.h" + "*.cpp" + "../ipDeviceCommon/*.h" +) + +set(COMPRESSION_SOURCES ${COMPRESSION_SOURCES} ${LZ4_DIR}/lz4.h ${LZ4_DIR}/lz4.c) + +add_library(${PROJECT_NAME} STATIC ${COMPRESSION_SOURCES}) + +include_directories(${PROJECT_NAME} + ../../common + ../ipDeviceCommon + ../../third-party/easyloggingpp/src +) + +target_compile_features(${PROJECT_NAME} PRIVATE cxx_std_11) +#set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) + +add_dependencies(${PROJECT_NAME} + libjpeg-turbo +) + +include_directories(${PROJECT_NAME} + ${CMAKE_BINARY_DIR}/libjpeg-turbo/include + ${LZ4_DIR} +) + +if(WIN32) + target_link_libraries(${PROJECT_NAME} + PRIVATE ${DEPENDENCIES} + PRIVATE ${CMAKE_BINARY_DIR}/libjpeg-turbo/lib/turbojpeg-static.lib + ) +else() + target_link_libraries(${PROJECT_NAME} + PRIVATE ${DEPENDENCIES} + PRIVATE ${CMAKE_BINARY_DIR}/libjpeg-turbo/lib/libturbojpeg.a + ) +endif() + +set_target_properties (${PROJECT_NAME} PROPERTIES FOLDER "Library") + +set(CMAKECONFIG_COMPRESS_INSTALL_DIR "${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME}") +install(TARGETS ${PROJECT_NAME} + EXPORT realsense2-compressTargets + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +install(EXPORT realsense2-compressTargets + FILE realsense2-compressTargets.cmake + NAMESPACE ${PROJECT_NAME}:: + DESTINATION ${CMAKECONFIG_COMPRESS_INSTALL_DIR} +) diff --git a/src/compression/CompressionFactory.cpp b/src/compression/CompressionFactory.cpp new file mode 100644 index 0000000000..980aea0fc0 --- /dev/null +++ b/src/compression/CompressionFactory.cpp @@ -0,0 +1,60 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#include "CompressionFactory.h" +#include "JpegCompression.h" +#include "Lz4Compression.h" +#include "RvlCompression.h" + +std::shared_ptr CompressionFactory::getObject(int t_width, int t_height, rs2_format t_format, rs2_stream t_streamType, int t_bpp) +{ + ZipMethod zipMeth; + if(t_streamType == RS2_STREAM_COLOR || t_streamType == RS2_STREAM_INFRARED) + { + zipMeth = ZipMethod::jpeg; + } + else if(t_streamType == RS2_STREAM_DEPTH) + { + zipMeth = ZipMethod::lz; + } + if(!isCompressionSupported(t_format, t_streamType)) + { + return nullptr; + } + + switch(zipMeth) + { + case ZipMethod::rvl: + return std::make_shared(t_width, t_height, t_format, t_bpp); + break; + case ZipMethod::jpeg: + return std::make_shared(t_width, t_height, t_format, t_bpp); + break; + case ZipMethod::lz: + return std::make_shared(t_width, t_height, t_format, t_bpp); + break; + default: + ERR << "unknown zip method"; + return nullptr; + } +} + +bool& CompressionFactory::getIsEnabled() +{ + static bool m_isEnabled = true; + return m_isEnabled; +} + +bool CompressionFactory::isCompressionSupported(rs2_format t_format, rs2_stream t_streamType) +{ + if(getIsEnabled() == 0) + { + return false; + } + + if((t_streamType == RS2_STREAM_COLOR || t_streamType == RS2_STREAM_INFRARED) && (t_format != RS2_FORMAT_BGR8 && t_format != RS2_FORMAT_RGB8 && t_format != RS2_FORMAT_Y8 && t_format != RS2_FORMAT_YUYV && t_format != RS2_FORMAT_UYVY)) + { + return false; + } + return true; +} diff --git a/src/compression/CompressionFactory.h b/src/compression/CompressionFactory.h new file mode 100644 index 0000000000..17f7cc0c2e --- /dev/null +++ b/src/compression/CompressionFactory.h @@ -0,0 +1,23 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "ICompression.h" +#define IS_COMPRESSION_ENABLED 1 + +typedef enum ZipMethod +{ + gzip, + rvl, + jpeg, + lz, +} ZipMethod; + +class CompressionFactory +{ +public: + static std::shared_ptr getObject(int t_width, int t_height, rs2_format t_format, rs2_stream t_streamType, int t_bpp); + static bool isCompressionSupported(rs2_format t_format, rs2_stream t_streamType); + static bool& getIsEnabled(); +}; diff --git a/src/compression/ICompression.h b/src/compression/ICompression.h new file mode 100644 index 0000000000..eabc5930ea --- /dev/null +++ b/src/compression/ICompression.h @@ -0,0 +1,22 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include + +#include + +class ICompression +{ +public: + ICompression(int t_width, int t_height, rs2_format t_format, int t_bpp): + m_width(t_width),m_height(t_height), m_format(t_format), m_bpp(t_bpp) {}; + virtual int compressBuffer(unsigned char* t_buffer, int t_size, unsigned char* t_compressedBuf) = 0; + virtual int decompressBuffer(unsigned char* t_buffer, int t_size, unsigned char* t_uncompressedBuf) = 0; + +protected: + int m_width, m_height, m_bpp; + rs2_format m_format; + int m_decompFrameCounter = 0, m_compFrameCounter = 0; +}; diff --git a/src/compression/JpegCompression.cpp b/src/compression/JpegCompression.cpp new file mode 100644 index 0000000000..d6420a18ad --- /dev/null +++ b/src/compression/JpegCompression.cpp @@ -0,0 +1,262 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#include "JpegCompression.h" +#include "jpeglib.h" +#include +#include +#include +#include +#include +#include + +JpegCompression::JpegCompression(int t_width, int t_height, rs2_format t_format, int t_bpp) + :ICompression(t_width, t_height, t_format, t_bpp) +{ + m_cinfo.err = jpeg_std_error(&m_jerr); + m_dinfo.err = jpeg_std_error(&m_jerr); + jpeg_create_compress(&m_cinfo); + jpeg_create_decompress(&m_dinfo); + m_cinfo.input_components = m_bpp; + if(m_format == RS2_FORMAT_YUYV || m_format == RS2_FORMAT_UYVY) + { + m_cinfo.in_color_space = JCS_YCbCr; + m_cinfo.input_components = 3; //yuyv and uyvy is 2 bpp, converted to yuv that is 3 bpp. + } + else if(m_format == RS2_FORMAT_Y8) + { + m_cinfo.in_color_space = JCS_GRAYSCALE; + m_cinfo.input_components = 1; + } + else if(m_format == RS2_FORMAT_RGB8 || m_format == RS2_FORMAT_BGR8) + { + m_cinfo.in_color_space = JCS_RGB; + } + else + { + ERR << "unsupported format " << t_format << " for JPEG compression"; + } + m_rowBuffer = new unsigned char[m_cinfo.input_components * t_width]; + m_destBuffer = (*m_cinfo.mem->alloc_sarray)((j_common_ptr)&m_cinfo, JPOOL_IMAGE, m_cinfo.input_components * t_width, 1); + jpeg_set_defaults(&m_cinfo); +} + +JpegCompression::~JpegCompression() +{ + delete[] m_rowBuffer; + jpeg_destroy_decompress(&m_dinfo); + jpeg_destroy_compress(&m_cinfo); +} + +void JpegCompression::convertYUYVtoYUV(unsigned char** t_buffer) +{ + for(int i = 0; i < m_cinfo.image_width; i += 2) + { + m_rowBuffer[i * 3] = (*t_buffer)[i * 2 + 0]; // Y + m_rowBuffer[i * 3 + 1] = (*t_buffer)[i * 2 + 1]; // U + m_rowBuffer[i * 3 + 2] = (*t_buffer)[i * 2 + 3]; // V + m_rowBuffer[i * 3 + 3] = (*t_buffer)[i * 2 + 2]; // Y + m_rowBuffer[i * 3 + 4] = (*t_buffer)[i * 2 + 1]; // U + m_rowBuffer[i * 3 + 5] = (*t_buffer)[i * 2 + 3]; // V + } + m_row_pointer[0] = m_rowBuffer; + (*t_buffer) += m_cinfo.image_width * m_bpp; +} + +void JpegCompression::convertUYVYtoYUV(unsigned char** t_buffer) +{ + for(int i = 0; i < m_cinfo.image_width; i += 2) + { + m_rowBuffer[i * 3] = (*t_buffer)[i * 2 + 1]; // Y + m_rowBuffer[i * 3 + 1] = (*t_buffer)[i * 2 + 0]; // U + m_rowBuffer[i * 3 + 2] = (*t_buffer)[i * 2 + 2]; // V + m_rowBuffer[i * 3 + 3] = (*t_buffer)[i * 2 + 3]; // Y + m_rowBuffer[i * 3 + 4] = (*t_buffer)[i * 2 + 0]; // U + m_rowBuffer[i * 3 + 5] = (*t_buffer)[i * 2 + 2]; // V + } + m_row_pointer[0] = m_rowBuffer; + (*t_buffer) += m_cinfo.image_width * m_bpp; +} + +void JpegCompression::convertYUVtoYUYV(unsigned char** t_uncompressBuff) +{ + for(int i = 0; i < m_dinfo.output_width; i += 2) + { + (*t_uncompressBuff)[i * 2] = m_destBuffer[0][i * 3]; // Y + (*t_uncompressBuff)[i * 2 + 1] = m_destBuffer[0][i * 3 + 1]; // U + (*t_uncompressBuff)[i * 2 + 2] = m_destBuffer[0][i * 3 + 3]; // Y + (*t_uncompressBuff)[i * 2 + 3] = m_destBuffer[0][i * 3 + 2]; // V + } + (*t_uncompressBuff) += m_dinfo.output_width * m_bpp; +} + +void JpegCompression::convertYUVtoUYVY(unsigned char** t_uncompressBuff) +{ + for(int i = 0; i < m_dinfo.output_width; i += 2) + { + (*t_uncompressBuff)[i * 2] = m_destBuffer[0][i * 3 + 1]; // U + (*t_uncompressBuff)[i * 2 + 1] = m_destBuffer[0][i * 3 + 0]; // Y + (*t_uncompressBuff)[i * 2 + 2] = m_destBuffer[0][i * 3 + 2]; // V + (*t_uncompressBuff)[i * 2 + 3] = m_destBuffer[0][i * 3 + 3]; // Y + } + (*t_uncompressBuff) += m_dinfo.output_width * m_bpp; +} + +void JpegCompression::convertBGRtoRGB(unsigned char** t_buffer) +{ + for(int i = 0; i < m_cinfo.image_width * m_bpp; i += 3) + { + m_rowBuffer[i] = (*t_buffer)[i + 2]; // R + m_rowBuffer[i + 1] = (*t_buffer)[i + 1]; // G + m_rowBuffer[i + 2] = (*t_buffer)[i]; // B + } + m_row_pointer[0] = m_rowBuffer; + (*t_buffer) += m_cinfo.image_width * m_bpp; +} + +void JpegCompression::convertRGBtoBGR(unsigned char** t_uncompressBuff) +{ + for(int i = 0; i < m_dinfo.output_width * m_bpp; i += 3) + { + (*t_uncompressBuff)[i] = m_destBuffer[0][i + 2]; // B + (*t_uncompressBuff)[i + 1] = m_destBuffer[0][i + 1]; // G + (*t_uncompressBuff)[i + 2] = m_destBuffer[0][i]; // R + } + (*t_uncompressBuff) += m_dinfo.output_width * m_bpp; +} + +int JpegCompression::compressBuffer(unsigned char* t_buffer, int t_size, unsigned char* t_compressedBuf) +{ + long unsigned int compressedSize = 0; + unsigned char* data = nullptr; + jpeg_mem_dest(&m_cinfo, &data, &compressedSize); + m_cinfo.image_width = m_width; + m_cinfo.image_height = m_height; + uint64_t row_stride = m_cinfo.image_width * m_cinfo.input_components; + jpeg_start_compress(&m_cinfo, TRUE); + while(m_cinfo.next_scanline < m_cinfo.image_height) + { + if(m_format == RS2_FORMAT_RGB8 || m_format == RS2_FORMAT_Y8) + { + m_row_pointer[0] = &t_buffer[m_cinfo.next_scanline * row_stride]; + } + else if(m_format == RS2_FORMAT_YUYV) + { + convertYUYVtoYUV(&t_buffer); + } + else if(m_format == RS2_FORMAT_UYVY) + { + convertUYVYtoYUV(&t_buffer); + } + else if(m_format == RS2_FORMAT_BGR8) + { + convertBGRtoRGB(&t_buffer); + } + else + { + ERR << "unsupported format " << m_format << " for JPEG compression"; + return -1; + } + jpeg_write_scanlines(&m_cinfo, m_row_pointer, 1); + } + jpeg_finish_compress(&m_cinfo); + int compressWithHeaderSize = compressedSize + sizeof(int); + if(compressWithHeaderSize > t_size) + { + ERR << "compression overflow, destination buffer is smaller than the compressed size"; + return -1; + } + memcpy(t_compressedBuf, &compressedSize, sizeof(int)); + memcpy(t_compressedBuf + sizeof(int), data, compressedSize); + if(m_compFrameCounter++ % 50 == 0) + { + INF << "frame " << m_compFrameCounter << "\tcolor\tcompression\tJPEG\t" << t_size << "\t/\t" << compressedSize; + } + free(data); + return compressWithHeaderSize; +} + +int JpegCompression::decompressBuffer(unsigned char* t_buffer, int t_compressedSize, unsigned char* t_uncompressedBuf) +{ + unsigned char* ptr = t_uncompressedBuf; + unsigned char* data = t_buffer; + unsigned int jpegHeader{}, res{}; + jpeg_mem_src(&m_dinfo, data, t_compressedSize); + if(t_buffer != nullptr) + { + memcpy(&jpegHeader, t_buffer, sizeof(unsigned int)); + } + if(jpegHeader != 0xE0FFD8FF) + { //check header integrity if = E0FF D8FF - the First 4 bytes jpeg standards. + ERR << "Not a JPEG frame, skipping"; + return -1; + } + res = jpeg_read_header(&m_dinfo, TRUE); + if(!res) + { + ERR << "Cannot read JPEG header"; + return -1; + } + if(m_format == RS2_FORMAT_RGB8 || m_format == RS2_FORMAT_BGR8) + { + m_dinfo.out_color_space = JCS_RGB; + } + else if(m_format == RS2_FORMAT_YUYV || m_format == RS2_FORMAT_UYVY) + { + m_dinfo.out_color_space = JCS_YCbCr; + } + else if(m_format == RS2_FORMAT_Y8) + { + m_dinfo.out_color_space = JCS_GRAYSCALE; + } + else + { + ERR << "Unsupported format " << m_format << " for the JPEG compression"; + return -1; + } + res = jpeg_start_decompress(&m_dinfo); + if(!res) + { + ERR << "jpeg_start_decompress failed"; + return -1; + } + uint64_t row_stride = m_dinfo.output_width * m_dinfo.output_components; + while(m_dinfo.output_scanline < m_dinfo.output_height) + { + int numLines = jpeg_read_scanlines(&m_dinfo, m_destBuffer, 1); + if(numLines <= 0) + { + ERR << "jpeg_read_scanlines failed at " << numLines; + return -1; + } + if(m_format == RS2_FORMAT_RGB8 || m_format == RS2_FORMAT_Y8) + { + memcpy(ptr, m_destBuffer[0], row_stride); + ptr += row_stride; + } + else if(m_format == RS2_FORMAT_YUYV) + { + convertYUVtoYUYV(&ptr); + } + else if(m_format == RS2_FORMAT_UYVY) + { + convertYUVtoUYVY(&ptr); + } + else if(m_format == RS2_FORMAT_BGR8) + { + convertRGBtoBGR(&ptr); + } + } + res = jpeg_finish_decompress(&m_dinfo); + if(!res) + { + ERR << "jpeg_finish_decompress failed"; + return -1; + } + int uncompressedSize = m_dinfo.output_width * m_dinfo.output_height * m_bpp; + if(m_decompFrameCounter++ % 50 == 0) + { + INF << "frame " << m_decompFrameCounter << "\tcolor\tdecompression\tJPEG\t" << t_compressedSize << "\t/\t" << uncompressedSize; + } + return uncompressedSize; +} diff --git a/src/compression/JpegCompression.h b/src/compression/JpegCompression.h new file mode 100644 index 0000000000..cc235a0b58 --- /dev/null +++ b/src/compression/JpegCompression.h @@ -0,0 +1,32 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "ICompression.h" +#include "jpeglib.h" +#include + +class JpegCompression : public ICompression +{ +public: + JpegCompression(int t_width, int t_height, rs2_format t_format, int t_bpp); + ~JpegCompression(); + int compressBuffer(unsigned char* t_buffer, int t_size, unsigned char* t_compressedBuf); + int decompressBuffer(unsigned char* t_buffer, int t_size, unsigned char* t_uncompressedBuf); + +private: + void convertYUYVtoYUV(unsigned char** t_buffer); + void convertYUVtoYUYV(unsigned char** t_uncompressBuff); + void convertUYVYtoYUV(unsigned char** t_buffer); + void convertYUVtoUYVY(unsigned char** t_uncompressBuff); + void convertBGRtoRGB(unsigned char** t_buffer); + void convertRGBtoBGR(unsigned char** t_uncompressBuff); + + struct jpeg_error_mgr m_jerr; + struct jpeg_compress_struct m_cinfo; + struct jpeg_decompress_struct m_dinfo; + JSAMPROW m_row_pointer[1]; + JSAMPARRAY m_destBuffer; + unsigned char* m_rowBuffer; +}; diff --git a/src/compression/Lz4Compression.cpp b/src/compression/Lz4Compression.cpp new file mode 100644 index 0000000000..71b73068ad --- /dev/null +++ b/src/compression/Lz4Compression.cpp @@ -0,0 +1,51 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#include "Lz4Compression.h" +#include +#include +#include + +Lz4Compression::Lz4Compression(int t_width, int t_height, rs2_format t_format, int t_bpp) + :ICompression(t_width, t_height, t_format, t_bpp) +{ +} + +int Lz4Compression::compressBuffer(unsigned char* t_buffer, int t_size, unsigned char* t_compressedBuf) +{ + const int maxDstSize = LZ4_compressBound(t_size); + const int compressedSize = LZ4_compress_default((const char*)t_buffer, (char*)t_compressedBuf + sizeof(int), t_size, maxDstSize); + if(compressedSize <= 0) + { + ERR << "Failure trying to compress the data."; + return -1; + } + int compressWithHeaderSize = compressedSize + sizeof(compressedSize); + if(compressWithHeaderSize > t_size) + { + ERR << "Compression overflow, destination buffer is smaller than the compressed size."; + return -1; + } + if(m_compFrameCounter++ % 50 == 0) + { + INF << "frame " << m_compFrameCounter << "\tdepth\tcompression\tlz4\t" << t_size << "\t/\t" << compressedSize; + } + memcpy(t_compressedBuf, &compressedSize, sizeof(compressedSize)); + return compressWithHeaderSize; +} + +int Lz4Compression::decompressBuffer(unsigned char* t_buffer, int t_compressedSize, unsigned char* t_uncompressedBuf) +{ + const int decompressed_size = LZ4_decompress_safe((const char*)t_buffer, (char*)t_uncompressedBuf, t_compressedSize, m_width * m_height * m_bpp); + if(decompressed_size < 0) + { + ERR << "Failure trying to decompress the frame."; + return -1; + } + int original_size = m_width * m_height * m_bpp; + if(m_decompFrameCounter++ % 50 == 0) + { + INF << "frame " << m_decompFrameCounter << "\tdepth\tdecompression\tlz4\t" << t_compressedSize << "\t/\t" << decompressed_size; + } + return decompressed_size; +} diff --git a/src/compression/Lz4Compression.h b/src/compression/Lz4Compression.h new file mode 100644 index 0000000000..ae2239fba1 --- /dev/null +++ b/src/compression/Lz4Compression.h @@ -0,0 +1,15 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "ICompression.h" +#include + +class Lz4Compression : public ICompression +{ +public: + Lz4Compression(int t_width, int t_height, rs2_format t_format, int t_bpp); + int compressBuffer(unsigned char* t_buffer, int t_size, unsigned char* t_compressedBuf); + int decompressBuffer(unsigned char* t_buffer, int t_size, unsigned char* t_uncompressedBuf); +}; diff --git a/src/compression/RvlCompression.cpp b/src/compression/RvlCompression.cpp new file mode 100644 index 0000000000..3a0624dc1f --- /dev/null +++ b/src/compression/RvlCompression.cpp @@ -0,0 +1,129 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#include "RvlCompression.h" +#include +#include +#include +#include + +RvlCompression::RvlCompression(int t_width, int t_height, rs2_format t_format, int t_bpp) + :ICompression(t_width, t_height, t_format, t_bpp) +{ +} + +int RvlCompression::encodeVLE(int t_value) +{ + do + { + int nibble = t_value & 0x7; // lower 3 bits + if(t_value >>= 3) + nibble |= 0x8; // more to come + m_word <<= 4; + m_word |= nibble; + if(++m_nibblesWritten == 8) // output word + { + *m_pBuffer++ = m_word; + m_nibblesWritten = 0; + m_word = 0; + } + } while(t_value); + + return 0; +} + +int RvlCompression::decodeVLE() +{ + unsigned int nibble; + int value = 0, bits = 29; + do + { + if(!m_nibblesWritten) + { + m_word = *m_pBuffer++; // load word + m_nibblesWritten = 8; + } + nibble = m_word & 0xf0000000; + value |= (nibble << 1) >> bits; + m_word <<= 4; + m_nibblesWritten--; + bits -= 3; + } while(nibble & 0x80000000); + return value; +} + +int RvlCompression::compressBuffer(unsigned char* t_buffer, int t_size, unsigned char* t_compressedBuf) +{ + short* buffer2 = (short*)t_buffer; + int* pHead = m_pBuffer = (int*)t_compressedBuf + 1; + m_nibblesWritten = 0; + short* end = buffer2 + t_size / m_bpp; + short previous = 0; + while(buffer2 != end) + { + int zeros = 0, nonzeros = 0; + for(; (buffer2 != end) && !*buffer2; buffer2++, zeros++) + ; + encodeVLE(zeros); + for(short* p = buffer2; (p != end) && *p++; nonzeros++) + ; + encodeVLE(nonzeros); + for(int i = 0; i < nonzeros; i++) + { + short current = *buffer2++; + int delta = current - previous; + int positive = (delta << 1) ^ (delta >> 31); + encodeVLE(positive); + previous = current; + } + } + if(m_nibblesWritten) // last few values + *m_pBuffer++ = m_word << 4 * (8 - m_nibblesWritten); + int compressedSize = int((char*)m_pBuffer - (char*)pHead); + int compressWithHeaderSize = compressedSize + sizeof(compressedSize); + if(compressWithHeaderSize > t_size) + { + ERR << "Compression overflow, destination buffer is smaller than the compressed size"; + return -1; + } + memcpy(t_compressedBuf, &compressedSize, sizeof(compressedSize)); + if(m_compFrameCounter++ % 50 == 0) + { + INF << "frame " << m_compFrameCounter << "\tdepth\tcompression\tlz4\t" << t_size << "\t/\t" << compressedSize; + } + memcpy(t_compressedBuf, &compressedSize, sizeof(compressedSize)); + return compressWithHeaderSize; +} + +int RvlCompression::decompressBuffer(unsigned char* t_buffer, int t_size, unsigned char* t_uncompressedBuf) +{ + short* currentPtr = (short*)t_uncompressedBuf; + m_pBuffer = (int*)t_buffer + 1; + m_nibblesWritten = 0; + short current, previous = 0; + unsigned int compressedSize; + int numPixelsToDecode = t_size / 2; + while(numPixelsToDecode) + { + int zeros = decodeVLE(); + numPixelsToDecode -= zeros; + for(; zeros; zeros--) + *currentPtr++ = 0; + int nonzeros = decodeVLE(); + numPixelsToDecode -= nonzeros; + for(; nonzeros; nonzeros--) + { + int positive = decodeVLE(); + int delta = (positive >> 1) ^ -(positive & 1); + current = previous + delta; + *currentPtr++ = current; + previous = current; + } + } + int uncompressedSize = int((char*)currentPtr - (char*)t_uncompressedBuf); + if(m_decompFrameCounter++ % 50 == 0) + { + INF << "frame " << m_decompFrameCounter << "\tdepth\tcompression\tlz4\t" << compressedSize << "\t/\t" << uncompressedSize; + } + return uncompressedSize; +} diff --git a/src/compression/RvlCompression.h b/src/compression/RvlCompression.h new file mode 100644 index 0000000000..11e22776b5 --- /dev/null +++ b/src/compression/RvlCompression.h @@ -0,0 +1,19 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "ICompression.h" + +class RvlCompression : public ICompression +{ +public: + RvlCompression(int t_width, int t_height, rs2_format t_format, int t_bpp); + int compressBuffer(unsigned char* t_buffer, int t_size, unsigned char* t_compressedBuf); + int decompressBuffer(unsigned char* t_buffer, int t_size, unsigned char* t_uncompressedBuf); + +private: + int encodeVLE(int value); + int decodeVLE(); + int *m_pBuffer, m_word, m_nibblesWritten; +}; diff --git a/src/ds5/ds5-device.cpp b/src/ds5/ds5-device.cpp index f6390f7f23..15d6eefd35 100644 --- a/src/ds5/ds5-device.cpp +++ b/src/ds5/ds5-device.cpp @@ -901,7 +901,12 @@ namespace librealsense //{ // throw not_implemented_exception("device time not supported for backend."); //} - + +#ifdef RASPBERRY_PI + // TODO: This is temporary work-around since global timestamp seems to compromise RPi stability + using namespace std::chrono; + return duration_cast(system_clock::now().time_since_epoch()).count(); +#else if (!_hw_monitor) throw wrong_api_call_sequence_exception("_hw_monitor is not initialized yet"); @@ -916,6 +921,7 @@ namespace librealsense uint32_t dt = *(uint32_t*)res.data(); double ts = dt * TIMESTAMP_USEC_TO_MSEC; return ts; +#endif } std::shared_ptr ds5u_device::create_ds5u_depth_device(std::shared_ptr ctx, diff --git a/src/ethernet/CMakeLists.txt b/src/ethernet/CMakeLists.txt new file mode 100644 index 0000000000..10f8b29b42 --- /dev/null +++ b/src/ethernet/CMakeLists.txt @@ -0,0 +1,128 @@ +# 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(realsense2-net VERSION 1.0.0 LANGUAGES CXX C) + +# Save the command line compile commands in the build output +set(CMAKE_EXPORT_COMPILE_COMMANDS 1) + +add_definitions(-DNEWLOCALE_NOT_USED=1 -DBSD=1 -DSOCKLEN_T=socklen_t -D_FILE_OFFSET_BITS=64 -D_LARGEFILE_SOURCE=1 -DALLOW_RTSP_SERVER_PORT_REUSE=1 -DNO_OPENSSL=1) + +set(LIVE ${CMAKE_BINARY_DIR}/third-party/live) + +file(GLOB REALSENSE_NET_CPP + "*.h*" + "*.c*" + "realsense-net.def" + "../ipDeviceCommon/*.h*" + "../ipDeviceCommon/*.c*" + "${LIVE}/groupsock/*.c*" + "${LIVE}/BasicUsageEnvironment/*.c*" + "${LIVE}/liveMedia/*.c*" + "${LIVE}/UsageEnvironment/*.c*" +) + +if (${BUILD_SHARED_LIBS} AND ${BUILD_EASYLOGGINGPP}) + list(APPEND REALSENSE_NET_CPP ../../third-party/easyloggingpp/src/easylogging++.cc) +endif() +add_definitions(-DELPP_NO_DEFAULT_LOG_FILE) + +add_library(${PROJECT_NAME} ${REALSENSE_NET_CPP}) + +set(REALSENSE_NET_PUBLIC_HEADERS + ../../include/librealsense2-net/rs_net.h + ../../include/librealsense2-net/rs_net.hpp +) + +set(CMAKECONFIG_NET_INSTALL_DIR "${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME}") + +include_directories(${PROJECT_NAME} + ../../common + ../ipDeviceCommon + ../../third-party/easyloggingpp/src + ${LIVE}/groupsock/include + ${LIVE}/liveMedia/include + ${LIVE}/UsageEnvironment/include + ${LIVE}/BasicUsageEnvironment/include +) + +if(WIN32) + # + # Windows doesn't support POSIX ssize_t type + # by default so I use the following defines: + # + # #ifdef _WIN64 + # # define ssize_t __int64 + # #else + # # define ssize_t long + # #endif + # + if("${CMAKE_SIZEOF_VOID_P}" EQUAL "8") + # 64-bit environment... + message(STATUS "Target is 64 bits") + add_definitions(-Dssize_t=__int64) + else("${CMAKE_SIZEOF_VOID_P}" EQUAL "8") + # 32-bit environment... + message(STATUS "Target is 32 bits") + add_definitions(-Dssize_t=long) + endif("${CMAKE_SIZEOF_VOID_P}" EQUAL "8") + + add_definitions(-DELPP_WINSOCK2) + + set(WINLIB Ws2_32.lib) +endif() + +set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) + +target_link_libraries(${PROJECT_NAME} + PRIVATE ${WINLIB} realsense2 realsense2-compression +) + +set_target_properties(${PROJECT_NAME} PROPERTIES FOLDER Library) +set_target_properties(${PROJECT_NAME} PROPERTIES PUBLIC_HEADER "${REALSENSE_NET_PUBLIC_HEADERS}") +set_target_properties(${PROJECT_NAME} PROPERTIES VERSION ${REALSENSE_VERSION_STRING} SOVERSION "${REALSENSE_VERSION_MAJOR}.${REALSENSE_VERSION_MINOR}") +# set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE C) + +WRITE_BASIC_CONFIG_VERSION_FILE("${CMAKE_CURRENT_BINARY_DIR}/realsense2-netConfigVersion.cmake" + VERSION ${REALSENSE_VERSION_STRING} + COMPATIBILITY AnyNewerVersion +) + +set(CMAKECONFIG_NET_INSTALL_DIR "${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME}") +configure_package_config_file(../../CMake/realsense2-netConfig.cmake.in realsense2-netConfig.cmake + INSTALL_DESTINATION ${CMAKECONFIG_NET_INSTALL_DIR} + INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX}/bin + PATH_VARS CMAKE_INSTALL_INCLUDEDIR +) +configure_file(../../config/librealsense-net.pc.in ../../config/realsense2-net.pc @ONLY) + +install(TARGETS ${PROJECT_NAME} + EXPORT realsense2-netTargets + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_PREFIX}/include/librealsense2-net" +) + +install(EXPORT realsense2-netTargets + FILE realsense2-netTargets.cmake + NAMESPACE ${PROJECT_NAME}:: + DESTINATION ${CMAKECONFIG_NET_INSTALL_DIR} +) + +install(FILES "${CMAKE_BINARY_DIR}/src/ethernet/realsense2-netConfig.cmake" + DESTINATION ${CMAKECONFIG_NET_INSTALL_DIR} +) + +install(FILES "${CMAKE_BINARY_DIR}/src/ethernet/realsense2-netConfigVersion.cmake" + DESTINATION ${CMAKECONFIG_NET_INSTALL_DIR} +) + +# Set library pkgconfig file for facilitating 3rd party integration +install(FILES "${CMAKE_BINARY_DIR}/config/realsense2-net.pc" + DESTINATION "${CMAKE_INSTALL_LIBDIR}/pkgconfig" +) + +install(CODE "execute_process(COMMAND ldconfig)") diff --git a/src/ethernet/IRsRtsp.h b/src/ethernet/IRsRtsp.h new file mode 100644 index 0000000000..af9169d10f --- /dev/null +++ b/src/ethernet/IRsRtsp.h @@ -0,0 +1,33 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "RsSink.h" +#include "rtp_callback.hh" +#include + +#include + +#include + +struct DeviceData +{ + std::string serialNum; + std::string name; + std::string usbType; +}; + +class IRsRtsp +{ +public: + virtual std::vector getStreams() = 0; + virtual int addStream(rs2_video_stream t_stream, rtp_callback* t_frameCallBack) = 0; + virtual int start() = 0; + virtual int stop() = 0; + virtual int close() = 0; + virtual int getOption(const std::string& t_sensorName, rs2_option t_option, float& t_value) = 0; + virtual int setOption(const std::string& t_sensorName, rs2_option t_option, float t_value) = 0; + virtual DeviceData getDeviceData() = 0; + virtual std::vector getControls() = 0; +}; diff --git a/src/ethernet/RsMediaSession.cpp b/src/ethernet/RsMediaSession.cpp new file mode 100644 index 0000000000..b43adc3355 --- /dev/null +++ b/src/ethernet/RsMediaSession.cpp @@ -0,0 +1,94 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#include "GroupsockHelper.hh" +#include "Locale.hh" +#include "liveMedia.hh" + +#include "RsCommon.h" +#include "RsMediaSession.hh" + +#include + +// RsMediaSession + +RsMediaSession* RsMediaSession::createNew(UsageEnvironment& env, char const* sdpDescription) +{ + RsMediaSession* newSession = new RsMediaSession(env); + if(newSession != NULL) + { + if(!newSession->initializeWithSDP(sdpDescription)) + { + delete newSession; + return NULL; + } + } + return newSession; +} + +RsMediaSession::RsMediaSession(UsageEnvironment& env) + : MediaSession(env) +{} + +RsMediaSession::~RsMediaSession() {} + +MediaSubsession* RsMediaSession::createNewMediaSubsession() +{ + // default implementation: + return new RsMediaSubsession(*this); +} + +// RsMediaSubsessionIterator + +RsMediaSubsessionIterator::RsMediaSubsessionIterator(RsMediaSession const& session) + : fOurSession(session) +{ + reset(); +} + +RsMediaSubsessionIterator::~RsMediaSubsessionIterator() {} + +RsMediaSubsession* RsMediaSubsessionIterator::next() +{ + RsMediaSubsession* result = fNextPtr; + + if(fNextPtr != NULL) + fNextPtr = (RsMediaSubsession*)(fNextPtr->fNext); + + return result; +} + +void RsMediaSubsessionIterator::reset() +{ + fNextPtr = (RsMediaSubsession*)(fOurSession.fSubsessionsHead); +} + +// MediaSubsession + +RsMediaSubsession::RsMediaSubsession(RsMediaSession& parent) + : MediaSubsession(parent) +{} + +RsMediaSubsession::~RsMediaSubsession() +{ + if(sink != NULL) + { + Medium::close(sink); + } +} + +Boolean RsMediaSubsession::createSourceObjects(int useSpecialRTPoffset) +{ + if(strcmp(fCodecName, RS_PAYLOAD_FORMAT.c_str()) == 0) + { + // This subsession uses our custom RTP payload format: + std::string mimeTypeString = RS_MEDIA_TYPE + "/" + RS_PAYLOAD_FORMAT; + fReadSource = fRTPSource = SimpleRTPSource::createNew(env(), fRTPSocket, fRTPPayloadFormat, fRTPTimestampFrequency, mimeTypeString.c_str()); + return True; + } + else + { + // This subsession uses some other RTP payload format - perhaps one that we already implement: + return MediaSubsession::createSourceObjects(useSpecialRTPoffset); + } +} diff --git a/src/ethernet/RsMediaSession.hh b/src/ethernet/RsMediaSession.hh new file mode 100644 index 0000000000..b2fe0d5242 --- /dev/null +++ b/src/ethernet/RsMediaSession.hh @@ -0,0 +1,48 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2019 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "MediaSession.hh" + +class RsMediaSubsession; // forward + +class RsMediaSession : public MediaSession +{ +public: + static RsMediaSession* createNew(UsageEnvironment& env, char const* sdpDescription); + +protected: + RsMediaSession(UsageEnvironment& env); + // called only by createNew(); + virtual ~RsMediaSession(); + + virtual MediaSubsession* createNewMediaSubsession(); + + friend class RsMediaSubsessionIterator; +}; + +class RsMediaSubsessionIterator +{ +public: + RsMediaSubsessionIterator(RsMediaSession const& session); + virtual ~RsMediaSubsessionIterator(); + + RsMediaSubsession* next(); // NULL if none + void reset(); + +private: + RsMediaSession const& fOurSession; + RsMediaSubsession* fNextPtr; +}; + +class RsMediaSubsession : public MediaSubsession +{ +protected: + friend class RsMediaSession; + friend class RsMediaSubsessionIterator; + RsMediaSubsession(RsMediaSession& parent); + virtual ~RsMediaSubsession(); + virtual Boolean createSourceObjects(int useSpecialRTPoffset); + // create "fRTPSource" and "fReadSource" member objects, after we've been initialized via SDP +}; diff --git a/src/ethernet/RsRtspClient.cpp b/src/ethernet/RsRtspClient.cpp new file mode 100644 index 0000000000..dcbcf741da --- /dev/null +++ b/src/ethernet/RsRtspClient.cpp @@ -0,0 +1,741 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#include "liveMedia.hh" + +#include "RsRtspClient.h" +#include +#include + +#include +#include +#include +#include +#include +#include + +#define RTSP_CLIENT_VERBOSITY_LEVEL 0 // by default, print verbose output from each "RTSPClient" +#define REQUEST_STREAMING_OVER_TCP 0 + +// map for stream pysical sensor +// key is generated by rs2_stream+index: depth=1,color=2,irl=3,irr=4 +std::map, rs2_extrinsics> minimal_extrinsics_map; + +std::string format_error_msg(std::string function, RsRtspReturnValue retVal) +{ + return std::string("[" + function + "] error: " + retVal.msg + " - " + std::to_string(retVal.exit_code)); +} + +long long int RsRTSPClient::getStreamProfileUniqueKey(rs2_video_stream t_profile) +{ + long long int key; + key = t_profile.type * pow(10, 12) + t_profile.fmt * pow(10, 10) + t_profile.fps * pow(10, 8) + t_profile.index + t_profile.width * pow(10, 4) + t_profile.height; + return key; +} + +int RsRTSPClient::getPhysicalSensorUniqueKey(rs2_stream stream_type, int sensors_index) +{ + return stream_type * 10 + sensors_index; +} + +IRsRtsp *RsRTSPClient::getRtspClient(char const *t_rtspURL, char const *t_applicationName, portNumBits t_tunnelOverHTTPPortNum) +{ + TaskScheduler *scheduler = BasicTaskScheduler::createNew(); + UsageEnvironment *env = RSUsageEnvironment::createNew(*scheduler); + + RTSPClient::responseBufferSize = 100000; + return (IRsRtsp *)new RsRTSPClient(scheduler, env, t_rtspURL, RTSP_CLIENT_VERBOSITY_LEVEL, t_applicationName, t_tunnelOverHTTPPortNum); +} + +RsRTSPClient::RsRTSPClient(TaskScheduler *t_scheduler, UsageEnvironment *t_env, char const *t_rtspURL, int t_verbosityLevel, char const *t_applicationName, portNumBits t_tunnelOverHTTPPortNum) + : RTSPClient(*t_env, t_rtspURL, t_verbosityLevel, t_applicationName, t_tunnelOverHTTPPortNum, -1) +{ + m_lastReturnValue.exit_code = RsRtspReturnCode::OK; + m_env = t_env; + m_scheduler = t_scheduler; +} + +RsRTSPClient::~RsRTSPClient() {} + +std::vector RsRTSPClient::getStreams() +{ + this->sendDescribeCommand(this->continueAfterDESCRIBE); + + // wait for continueAfterDESCRIBE to finish + std::unique_lock lck(m_commandMtx); + m_cv.wait_for(lck, std::chrono::seconds(RTSP_CLIENT_COMMANDS_TIMEOUT_SEC), [this] { return m_commandDone; }); + // for the next command - if not done - throw timeout + if (!m_commandDone) + { + RsRtspReturnValue err = {RsRtspReturnCode::ERROR_TIME_OUT, "client time out"}; + throw std::runtime_error(format_error_msg(__FUNCTION__, err)); + } + m_commandDone = false; + + if (m_lastReturnValue.exit_code != RsRtspReturnCode::OK) + { + throw std::runtime_error(format_error_msg(__FUNCTION__, m_lastReturnValue)); + } + + if (this->m_supportedProfiles.size() == 0) + { + RsRtspReturnValue err = {RsRtspReturnCode::ERROR_GENERAL, std::string("failed to get streams from network device at url: " + std::string(this->url()))}; + throw std::runtime_error(format_error_msg(__FUNCTION__, err)); + } + + return this->m_supportedProfiles; +} + +int RsRTSPClient::addStream(rs2_video_stream t_stream, rtp_callback *t_callbackObj) +{ + long long int uniqueKey = getStreamProfileUniqueKey(t_stream); + RsMediaSubsession *subsession = this->m_subsessionMap.find(uniqueKey)->second; + if (subsession == nullptr) + { + RsRtspReturnValue err = {RsRtspReturnCode::ERROR_WRONG_FLOW, "requested stream was not found"}; + throw std::runtime_error(format_error_msg(__FUNCTION__, err)); + } + + if (!subsession->initiate()) + { + this->envir() << "Failed to initiate the subsession \n"; + RsRtspReturnValue err = {RsRtspReturnCode::ERROR_WRONG_FLOW, "Failed to initiate the subsession"}; + throw std::runtime_error(format_error_msg(__FUNCTION__, err)); + } + + // Continue setting up this subsession, by sending a RTSP "SETUP" command: + unsigned res = this->sendSetupCommand(*subsession, this->continueAfterSETUP, False, REQUEST_STREAMING_OVER_TCP); + // wait for continueAfterSETUP to finish + std::unique_lock lck(m_commandMtx); + m_cv.wait_for(lck, std::chrono::seconds(RTSP_CLIENT_COMMANDS_TIMEOUT_SEC), [this] { return m_commandDone; }); + // for the next command + if (!m_commandDone) + { + RsRtspReturnValue err = {RsRtspReturnCode::ERROR_TIME_OUT, "client time out"}; + throw std::runtime_error(format_error_msg(__FUNCTION__, err)); + } + m_commandDone = false; + + if (m_lastReturnValue.exit_code != RsRtspReturnCode::OK) + { + throw std::runtime_error(format_error_msg(__FUNCTION__, m_lastReturnValue)); + } + + subsession->sink = RsSink::createNew(this->envir(), *subsession, t_stream, m_memPool, this->url()); + // perhaps use your own custom "MediaSink" subclass instead + if (subsession->sink == NULL) + { + this->envir() << "Failed to create a data sink for the subsession: " << this->envir().getResultMsg() << "\n"; + RsRtspReturnValue err = {(RsRtspReturnCode)envir().getErrno(), std::string("Failed to create a data sink for the subsession: " + std::string(envir().getResultMsg()))}; + throw std::runtime_error(format_error_msg(__FUNCTION__, err)); + } + + subsession->miscPtr = this; // a hack to let subsession handler functions get the "RTSPClient" from the subsession + ((RsSink *)(subsession->sink))->setCallback(t_callbackObj); + subsession->sink->startPlaying(*(subsession->readSource()), subsessionAfterPlaying, subsession); + // Also set a handler to be called if a RTCP "BYE" arrives for this subsession: + if (subsession->rtcpInstance() != NULL) + { + subsession->rtcpInstance()->setByeWithReasonHandler(subsessionByeHandler, subsession); + } + + return this->m_lastReturnValue.exit_code; +} + +int RsRTSPClient::start() +{ + unsigned res = this->sendPlayCommand(*this->m_scs.m_session, this->continueAfterPLAY); + // wait for continueAfterPLAY to finish + std::unique_lock lck(m_commandMtx); + m_cv.wait_for(lck, std::chrono::seconds(RTSP_CLIENT_COMMANDS_TIMEOUT_SEC), [this] { return m_commandDone; }); + // for the next command + if (!m_commandDone) + { + RsRtspReturnValue err = {RsRtspReturnCode::ERROR_TIME_OUT, "client time out"}; + throw std::runtime_error(format_error_msg(__FUNCTION__, err)); + } + m_commandDone = false; + + if (m_lastReturnValue.exit_code != RsRtspReturnCode::OK) + { + throw std::runtime_error(format_error_msg(__FUNCTION__, m_lastReturnValue)); + } + return m_lastReturnValue.exit_code; +} + +int RsRTSPClient::stop() +{ + unsigned res = this->sendPauseCommand(*this->m_scs.m_session, this->continueAfterPAUSE); + // wait for continueAfterPAUSE to finish + std::unique_lock lck(m_commandMtx); + m_cv.wait_for(lck, std::chrono::seconds(RTSP_CLIENT_COMMANDS_TIMEOUT_SEC), [this] { return m_commandDone; }); + // for the next command + if (!m_commandDone) + { + RsRtspReturnValue err = {RsRtspReturnCode::ERROR_TIME_OUT, "client time out"}; + throw std::runtime_error(format_error_msg(__FUNCTION__, err)); + } + m_commandDone = false; + if (m_lastReturnValue.exit_code != RsRtspReturnCode::OK) + { + throw std::runtime_error(format_error_msg(__FUNCTION__, m_lastReturnValue)); + } + return m_lastReturnValue.exit_code; +} + +int RsRTSPClient::close() +{ + unsigned res = this->sendTeardownCommand(*this->m_scs.m_session, this->continueAfterTEARDOWN); + // wait for continueAfterTEARDOWN to finish + std::unique_lock lck(m_commandMtx); + m_cv.wait_for(lck, std::chrono::seconds(RTSP_CLIENT_COMMANDS_TIMEOUT_SEC), [this] { return m_commandDone; }); + // for the next command + if (!m_commandDone) + { + RsRtspReturnValue err = {RsRtspReturnCode::ERROR_TIME_OUT, "client time out"}; + throw std::runtime_error(format_error_msg(__FUNCTION__, err)); + } + m_commandDone = false; + + if (m_lastReturnValue.exit_code != RsRtspReturnCode::OK) + { + throw std::runtime_error(format_error_msg(__FUNCTION__, m_lastReturnValue)); + } + m_eventLoopWatchVariable = ~0; + std::unique_lock lk(m_taskSchedulerMutex); + this->envir() << "Closing the stream.\n"; + Medium::close(this); + m_env->reclaim(); + m_env = NULL; + delete m_scheduler; + m_scheduler = NULL; + return m_lastReturnValue.exit_code; +} + +int RsRTSPClient::setOption(const std::string &t_sensorName, rs2_option t_option, float t_value) +{ + std::string option = t_sensorName + "_" + std::to_string(t_option); + std::string value = std::to_string(t_value); + if (isActiveSession) + { + RTSPClient::sendSetParameterCommand(*this->m_scs.m_session, this->continueAfterSETCOMMAND, option.c_str(), value.c_str()); + } + else + { + sendSetParameterCommand(this->continueAfterSETCOMMAND, option.c_str(), value.c_str()); + } + + std::unique_lock lck(m_commandMtx); + m_cv.wait_for(lck, std::chrono::seconds(RTSP_CLIENT_COMMANDS_TIMEOUT_SEC), [this] { return m_commandDone; }); + // for the next command + if (!m_commandDone) + { + RsRtspReturnValue err = {RsRtspReturnCode::ERROR_TIME_OUT, "client time out"}; + throw std::runtime_error(format_error_msg(__FUNCTION__, err)); + } + m_commandDone = false; + + if (m_lastReturnValue.exit_code != RsRtspReturnCode::OK) + { + throw std::runtime_error(format_error_msg(__FUNCTION__, m_lastReturnValue)); + } + + return m_lastReturnValue.exit_code; +} + +void RsRTSPClient::setGetParamResponse(float t_res) +{ + m_getParamRes = t_res; +} + +int RsRTSPClient::getOption(const std::string &t_sensorName, rs2_option t_option, float &t_value) +{ + unsigned res; + t_value = m_getParamRes = -1; + std::string option = t_sensorName + "_" + std::to_string(t_option); + if (isActiveSession) + { + res = RTSPClient::sendGetParameterCommand(*this->m_scs.m_session, this->continueAfterGETCOMMAND, option.c_str()); + } + else + { + res = sendGetParameterCommand(this->continueAfterGETCOMMAND, option.c_str()); + } + // wait for continueAfterGETCOMMAND to finish + std::unique_lock lck(m_commandMtx); + m_cv.wait_for(lck, std::chrono::seconds(RTSP_CLIENT_COMMANDS_TIMEOUT_SEC), [this] { return m_commandDone; }); + if (!m_commandDone) + { + RsRtspReturnValue err = {RsRtspReturnCode::ERROR_TIME_OUT, "client time out"}; + + throw std::runtime_error(format_error_msg(__FUNCTION__, err)); + } + m_commandDone = false; + if (m_lastReturnValue.exit_code != RsRtspReturnCode::OK) + { + throw std::runtime_error(format_error_msg(__FUNCTION__, m_lastReturnValue)); + } + + t_value = m_getParamRes; + + return m_lastReturnValue.exit_code; +} + +void schedulerThread(RsRTSPClient *t_rtspClientInstance) +{ + std::unique_lock lk(t_rtspClientInstance->getTaskSchedulerMutex()); + t_rtspClientInstance->envir().taskScheduler().doEventLoop(&t_rtspClientInstance->getEventLoopWatchVariable()); + lk.unlock(); +} + +void RsRTSPClient::initFunc(MemoryPool *t_pool) +{ + std::thread thread_scheduler(schedulerThread, this); + thread_scheduler.detach(); + m_memPool = t_pool; +} + +void RsRTSPClient::setDeviceData(DeviceData t_data) +{ + m_deviceData = t_data; +} +std::vector controls; + +std::vector RsRTSPClient::getControls() +{ + this->sendOptionsCommand(this->continueAfterOPTIONS); + + // wait for continueAfterOPTIONS to finish + std::unique_lock lck(m_commandMtx); + m_cv.wait_for(lck, std::chrono::seconds(RTSP_CLIENT_COMMANDS_TIMEOUT_SEC), [this] { return m_commandDone; }); + // for the next command + if (!m_commandDone) + { + RsRtspReturnValue err = {RsRtspReturnCode::ERROR_TIME_OUT, "client time out"}; + throw std::runtime_error(format_error_msg(__FUNCTION__, err)); + } + m_commandDone = false; + + if (m_lastReturnValue.exit_code != RsRtspReturnCode::OK) + { + throw std::runtime_error(format_error_msg(__FUNCTION__, m_lastReturnValue)); + } + return controls; +} + +void updateExtrinsicsMap(rs2_video_stream videoStream, std::string extrinsics_str) +{ + std::istringstream extrinsics_stream(extrinsics_str); + std::string s; + while (std::getline(extrinsics_stream, s, '&')) + { + rs2_extrinsics extrinsics; + int target_sensor; + int params_count = sscanf(s.c_str(), + "rotation:%f,%f,%f,%f,%f,%f,%f,%f,%ftranslation:%f,%f,%f", + &target_sensor, + &extrinsics.rotation[0], + &extrinsics.rotation[1], + &extrinsics.rotation[2], + &extrinsics.rotation[3], + &extrinsics.rotation[4], + &extrinsics.rotation[5], + &extrinsics.rotation[6], + &extrinsics.rotation[7], + &extrinsics.rotation[8], + &extrinsics.translation[0], + &extrinsics.translation[1], + &extrinsics.translation[2]); + + // hanle NaN values + if (params_count != SDP_EXTRINSICS_ARGS) + { + extrinsics = {{NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN}, {NAN, NAN, NAN}}; + } + + minimal_extrinsics_map[std::make_pair(RsRTSPClient::getPhysicalSensorUniqueKey(videoStream.type, videoStream.index), target_sensor)] = extrinsics; + } +} + +/********************************* + * CALLBACKS * + *********************************/ + +void RsRTSPClient::continueAfterDESCRIBE(RTSPClient *rtspClient, int resultCode, char *resultString) +{ + std::string resultStr; + if(nullptr != resultString) + { + resultStr = resultString; + delete [] resultString; + } + UsageEnvironment &env = rtspClient->envir(); // alias + RsRTSPClient *rsRtspClient = dynamic_cast(rtspClient); // alias + StreamClientState &scs = rsRtspClient->m_scs; // alias + + if(!resultStr.empty()) + rsRtspClient->m_lastReturnValue.msg = resultStr; + rsRtspClient->m_lastReturnValue.exit_code = (RsRtspReturnCode)resultCode; + + do + { + if (resultCode != 0) + { + env << "Failed to get a SDP description: " << resultStr.c_str() << "\n"; + break; + } + + // Create a media session object from this SDP description(resultString): + scs.m_session = RsMediaSession::createNew(env, resultStr.c_str()); + if (scs.m_session == NULL) + { + env << "Failed to create a RsMediaSession object from the SDP description: " << env.getResultMsg() << "\n"; + break; + } + else if (!scs.m_session->hasSubsessions()) + { + env << "This session has no media subsessions (i.e., no \"m=\" lines)\n"; + break; + } + + RsMediaSubsessionIterator iter(*scs.m_session); + RsMediaSubsession *subsession = iter.next(); + while (subsession != NULL) + { + // Get more data from the SDP string + const char *strWidthVal = subsession->attrVal_str("width"); + const char *strHeightVal = subsession->attrVal_str("height"); + const char *strFormatVal = subsession->attrVal_str("format"); + const char *strUidVal = subsession->attrVal_str("uid"); + const char *strFpsVal = subsession->attrVal_str("fps"); + const char *strIndexVal = subsession->attrVal_str("stream_index"); + const char *strStreamTypeVal = subsession->attrVal_str("stream_type"); + const char *strBppVal = subsession->attrVal_str("bpp"); + + const char *strSerialNumVal = subsession->attrVal_str("cam_serial_num"); + const char *strCamNameVal = subsession->attrVal_str("cam_name"); + const char *strUsbTypeVal = subsession->attrVal_str("usb_type"); + + int width = strWidthVal != "" ? std::stoi(strWidthVal) : 0; + int height = strHeightVal != "" ? std::stoi(strHeightVal) : 0; + int format = strFormatVal != "" ? std::stoi(strFormatVal) : 0; + int uid = strUidVal != "" ? std::stoi(strUidVal) : 0; + int fps = strFpsVal != "" ? std::stoi(strFpsVal) : 0; + int index = strIndexVal != "" ? std::stoi(strIndexVal) : 0; + int stream_type = strStreamTypeVal != "" ? std::stoi(strStreamTypeVal) : 0; + int bpp = strBppVal != "" ? std::stoi(strBppVal) : 0; + rs2_video_stream videoStream; + videoStream.width = width; + videoStream.height = height; + videoStream.uid = uid; + videoStream.fmt = static_cast(format); + videoStream.fps = fps; + videoStream.index = index; + videoStream.type = static_cast(stream_type); + videoStream.bpp = bpp; + + // intrinsics desirialization should happend at once (usgin json?) + videoStream.intrinsics.width = subsession->attrVal_int("width"); + videoStream.intrinsics.height = subsession->attrVal_int("height"); + videoStream.intrinsics.ppx = subsession->attrVal_int("ppx"); + videoStream.intrinsics.ppy = subsession->attrVal_int("ppy"); + videoStream.intrinsics.fx = subsession->attrVal_int("fx"); + videoStream.intrinsics.fy = subsession->attrVal_int("fy"); + CompressionFactory::getIsEnabled() = subsession->attrVal_bool("compression"); + videoStream.intrinsics.model = (rs2_distortion)subsession->attrVal_int("model"); + + for (size_t i = 0; i < 5; i++) + { + videoStream.intrinsics.coeffs[i] = subsession->attrVal_int("coeff_" + i); + } + + // extrinsics + std::string extrinsics = subsession->attrVal_str("extrinsics"); + updateExtrinsicsMap(videoStream, extrinsics); + + DeviceData deviceData; + deviceData.serialNum = strSerialNumVal; + deviceData.name = strCamNameVal; + // Return spaces back to string after getting it from the SDP + std::replace(deviceData.name.begin(), deviceData.name.end(), '^', ' '); + deviceData.usbType = strUsbTypeVal; + rsRtspClient->setDeviceData(deviceData); + + long long int uniqueKey = getStreamProfileUniqueKey(videoStream); + rsRtspClient->m_subsessionMap.insert(std::pair(uniqueKey, subsession)); + rsRtspClient->m_supportedProfiles.push_back(videoStream); + subsession = iter.next(); + // TODO: when to delete p? + } + } while (0); + + { + std::lock_guard lck(rsRtspClient->m_commandMtx); + rsRtspClient->m_commandDone = true; + } + rsRtspClient->m_cv.notify_one(); +} + +void RsRTSPClient::continueAfterSETUP(RTSPClient *rtspClient, int resultCode, char *resultString) +{ + std::string resultStr; + if(nullptr != resultString) + { + resultStr = resultString; + delete [] resultString; + } + UsageEnvironment &env = rtspClient->envir(); // alias + RsRTSPClient *rsRtspClient = dynamic_cast(rtspClient); // alias + StreamClientState &scs = rsRtspClient->m_scs; // alias + + env << "continueAfterSETUP " << resultCode << " " << resultStr.c_str() << "\n"; + + if(!resultStr.empty()) + rsRtspClient->m_lastReturnValue.msg = resultStr; + + rsRtspClient->m_lastReturnValue.exit_code = (RsRtspReturnCode)resultCode; + { + std::lock_guard lck(rsRtspClient->m_commandMtx); + rsRtspClient->m_commandDone = true; + } + rsRtspClient->m_cv.notify_one(); +} + +void RsRTSPClient::continueAfterPLAY(RTSPClient *rtspClient, int resultCode, char *resultString) +{ + std::string resultStr; + if(nullptr != resultString) + { + resultStr = resultString; + delete [] resultString; + } + UsageEnvironment &env = rtspClient->envir(); // alias + RsRTSPClient *rsRtspClient = dynamic_cast(rtspClient); // alias + env << "continueAfterPLAY " << resultCode << " " << resultStr.c_str() << "\n"; + + if(!resultStr.empty()) + rsRtspClient->m_lastReturnValue.msg = resultStr; + rsRtspClient->m_lastReturnValue.exit_code = (RsRtspReturnCode)resultCode; + + { + std::lock_guard lck(rsRtspClient->m_commandMtx); + rsRtspClient->m_commandDone = true; + } + rsRtspClient->m_cv.notify_one(); +} + +void RsRTSPClient::continueAfterTEARDOWN(RTSPClient *rtspClient, int resultCode, char *resultString) +{ + std::string resultStr; + if(nullptr != resultString) + { + resultStr = resultString; + delete [] resultString; + } + UsageEnvironment &env = rtspClient->envir(); // alias + RsRTSPClient *rsRtspClient = dynamic_cast(rtspClient); // alias + env << "continueAfterTEARDOWN " << resultCode << " " << resultStr.c_str() << "\n"; + + if(!resultStr.empty()) + rsRtspClient->m_lastReturnValue.msg = resultStr; + rsRtspClient->m_lastReturnValue.exit_code = (RsRtspReturnCode)resultCode; + + { + std::lock_guard lck(rsRtspClient->m_commandMtx); + rsRtspClient->m_commandDone = true; + } + rsRtspClient->m_cv.notify_one(); +} + +void RsRTSPClient::continueAfterPAUSE(RTSPClient *rtspClient, int resultCode, char *resultString) +{ + std::string resultStr; + if(nullptr != resultString) + { + resultStr = resultString; + delete [] resultString; + } + UsageEnvironment &env = rtspClient->envir(); // alias + RsRTSPClient *rsRtspClient = dynamic_cast(rtspClient); // alias + env << "continueAfterPAUSE " << resultCode << " " << resultStr.c_str() << "\n"; + + if(!resultStr.empty()) + rsRtspClient->m_lastReturnValue.msg = resultStr; + rsRtspClient->m_lastReturnValue.exit_code = (RsRtspReturnCode)resultCode; + + { + std::lock_guard lck(rsRtspClient->m_commandMtx); + rsRtspClient->m_commandDone = true; + } + rsRtspClient->m_cv.notify_one(); +} + +void RsRTSPClient::continueAfterOPTIONS(RTSPClient *rtspClient, int resultCode, char *resultString) +{ + std::string resultStr; + if(nullptr != resultString) + { + resultStr = resultString; + delete [] resultString; + } + UsageEnvironment &env = rtspClient->envir(); // alias + RsRTSPClient *rsRtspClient = dynamic_cast(rtspClient); // alias + env << "continueAfterOPTIONS " << resultCode << " " << resultStr.c_str() << "\n"; + + if(!resultStr.empty()) + rsRtspClient->m_lastReturnValue.msg = resultStr; + rsRtspClient->m_lastReturnValue.exit_code = (RsRtspReturnCode)resultCode; + + { + std::lock_guard lck(rsRtspClient->m_commandMtx); + std::size_t foundBegin = resultStr.find_first_of("["); + IpDeviceControlData controlData; + int counter = 0; + while (foundBegin != std::string::npos) + { + + std::size_t foundEnd = resultStr.find_first_of("]", foundBegin + 1); + std::string controlsPerSensor = resultStr.substr(foundBegin + 1, foundEnd - foundBegin); + std::size_t pos = 0; + while ((pos = controlsPerSensor.find(';')) != std::string::npos) + { + std::string controlStr = controlsPerSensor.substr(0, pos); + + controlData.sensorId = counter == 0 ? 1 : 0; + int option_code; + int params_count = sscanf(controlStr.c_str(), "%d{%f,%f,%f,%f}", &option_code, &controlData.range.min, &controlData.range.max, &controlData.range.def, &controlData.range.step); + + //to avoid sscanf warning + controlData.option = (rs2_option)option_code; + controls.push_back(controlData); + controlsPerSensor.erase(0, pos + 1); + } + counter++; + foundBegin = resultStr.find_first_of("[", foundBegin + 1); + } + rsRtspClient->m_commandDone = true; + } + rsRtspClient->m_cv.notify_one(); +} + +void RsRTSPClient::continueAfterSETCOMMAND(RTSPClient *rtspClient, int resultCode, char *resultString) +{ + std::string resultStr; + if(nullptr != resultString) + { + resultStr = resultString; + delete [] resultString; + } + UsageEnvironment &env = rtspClient->envir(); // alias + RsRTSPClient *rsRtspClient = dynamic_cast(rtspClient); // alias + env << "continueAfterSETCOMMAND " << resultCode << "\n"; + + if(!resultStr.empty()) + { + rsRtspClient->m_lastReturnValue.msg = resultStr; + } + rsRtspClient->m_lastReturnValue.exit_code = (RsRtspReturnCode)resultCode; + + { + std::lock_guard lck(rsRtspClient->m_commandMtx); + rsRtspClient->m_commandDone = true; + } + rsRtspClient->m_cv.notify_one(); +} + +void RsRTSPClient::continueAfterGETCOMMAND(RTSPClient *rtspClient, int resultCode, char *resultString) +{ + std::string resultStr; + if(nullptr != resultString) + { + resultStr = resultString; + delete [] resultString; + } + UsageEnvironment &env = rtspClient->envir(); // alias + RsRTSPClient *rsRtspClient = dynamic_cast(rtspClient); // alias + DBG << "continueAfterGETCOMMAND: resultCode " << resultCode << ", resultString '" << resultStr.c_str(); + + if(!resultStr.empty()) + { + rsRtspClient->m_lastReturnValue.msg = resultStr; + } + rsRtspClient->m_lastReturnValue.exit_code = (RsRtspReturnCode)resultCode; + + if (resultCode == 0) + { + rsRtspClient->setGetParamResponse(std::stof(resultStr)); + } + + { + std::lock_guard lck(rsRtspClient->m_commandMtx); + rsRtspClient->m_commandDone = true; + } + rsRtspClient->m_cv.notify_one(); +} + +// TODO: implementation +void RsRTSPClient::subsessionAfterPlaying(void *clientData) +{ + MediaSubsession *subsession = (MediaSubsession *)clientData; + RTSPClient *rtspClient = (RTSPClient *)(subsession->miscPtr); + rtspClient->envir() << "subsessionAfterPlaying\n"; +} + +void RsRTSPClient::subsessionByeHandler(void *clientData, char const *reason) {} + +unsigned RsRTSPClient::sendSetParameterCommand(responseHandler *responseHandler, char const *parameterName, char const *parameterValue, Authenticator *authenticator) +{ + if (fCurrentAuthenticator < authenticator) + fCurrentAuthenticator = *authenticator; + char *paramString = new char[strlen(parameterName) + strlen(parameterValue) + 10]; + sprintf(paramString, "%s: %s\r\n", parameterName, parameterValue); + unsigned result = sendRequest(new RequestRecord(++fCSeq, "SET_PARAMETER", responseHandler, NULL, NULL, False, 0.0f, -1.0f, 1.0f, paramString)); + delete[] paramString; + return result; +} + +unsigned RsRTSPClient::sendGetParameterCommand(responseHandler *responseHandler, char const *parameterName, Authenticator *authenticator) +{ + if (fCurrentAuthenticator < authenticator) + fCurrentAuthenticator = *authenticator; + + // We assume that: + // parameterName is NULL means: Send no body in the request. + // parameterName is "" means: Send only \r\n in the request body. + // parameterName is non-empty means: Send "\r\n" as the request body. + unsigned parameterNameLen = parameterName == NULL ? 0 : strlen(parameterName); + char *paramString = new char[parameterNameLen + 3]; // the 3 is for \r\n + the '\0' byte + if (parameterName == NULL) + { + paramString[0] = '\0'; + } + else + { + sprintf(paramString, "%s\r\n", parameterName); + } + unsigned result = sendRequest(new RequestRecord(++fCSeq, "GET_PARAMETER", responseHandler, NULL, NULL, False, 0.0f, -1.0f, 1.0f, paramString)); + delete[] paramString; + return result; +} + +Boolean RsRTSPClient::setRequestFields(RequestRecord *request, char *&cmdURL, Boolean &cmdURLWasAllocated, char const *&protocolStr, char *&extraHeaders, Boolean &extraHeadersWereAllocated) +{ + // Set various fields that will appear in our outgoing request, depending upon the particular command that we are sending. + if (request == nullptr) + { + return False; + } + if ((strcmp(request->commandName(), "SET_PARAMETER") == 0 || strcmp(request->commandName(), "GET_PARAMETER") == 0) && (request->session() == NULL)) + { + cmdURL = new char[4]; + + cmdURLWasAllocated = True; //use BaseUrl + sprintf(cmdURL, "%s", "*"); + } + else + { + return RTSPClient::setRequestFields(request, cmdURL, cmdURLWasAllocated, protocolStr, extraHeaders, extraHeadersWereAllocated); + } + + return True; +} diff --git a/src/ethernet/RsRtspClient.h b/src/ethernet/RsRtspClient.h new file mode 100644 index 0000000000..446082299a --- /dev/null +++ b/src/ethernet/RsRtspClient.h @@ -0,0 +1,99 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "BasicUsageEnvironment.hh" +#include "liveMedia.hh" + +#include "IRsRtsp.h" +#include "StreamClientState.h" +#include "common/RsRtspCommon.h" +#include +#include + +#include + +#include +#include +#include + +//TODO: check if this timeout is reasonable for all commands +#define RTSP_CLIENT_COMMANDS_TIMEOUT_SEC 10 + +#define SDP_EXTRINSICS_ARGS 13 + +class RsRTSPClient : public RTSPClient, IRsRtsp +{ +public: + static IRsRtsp* getRtspClient(char const* t_rtspURL, char const* t_applicationName = NULL, portNumBits t_tunnelOverHTTPPortNum = 0); + void describe(); + void setup(rs2_video_stream t_stream); + void initFunc(MemoryPool* t_pool); + + static long long int getStreamProfileUniqueKey(rs2_video_stream t_profile); + static int getPhysicalSensorUniqueKey(rs2_stream stream_type, int sensors_index); + void setDeviceData(DeviceData t_data); + + // IcamOERtsp functions + virtual std::vector getStreams(); + virtual int addStream(rs2_video_stream t_stream, rtp_callback* t_frameCallBack); + virtual int start(); + virtual int stop(); + virtual int close(); + virtual int getOption(const std::string& t_sensorName, rs2_option t_option, float& t_value); + virtual int setOption(const std::string& t_sensorName, rs2_option t_option, float t_value); + void setGetParamResponse(float t_res); + virtual DeviceData getDeviceData() + { + return m_deviceData; + } + virtual std::vector getControls(); + + static void continueAfterDESCRIBE(RTSPClient* rtspClient, int resultCode, char* resultString); + static void continueAfterSETUP(RTSPClient* rtspClient, int resultCode, char* resultString); + static void continueAfterPLAY(RTSPClient* rtspClient, int resultCode, char* resultString); + static void continueAfterTEARDOWN(RTSPClient* rtspClient, int resultCode, char* resultString); + static void continueAfterPAUSE(RTSPClient* rtspClient, int resultCode, char* resultString); + static void continueAfterOPTIONS(RTSPClient* rtspClient, int resultCode, char* resultString); + static void continueAfterSETCOMMAND(RTSPClient* rtspClient, int resultCode, char* resultString); + static void continueAfterGETCOMMAND(RTSPClient* rtspClient, int resultCode, char* resultString); + static void subsessionAfterPlaying(void* clientData); // called when a stream's subsession (e.g., audio or video substream) ends + static void subsessionByeHandler(void* clientData, char const* reason); + char& getEventLoopWatchVariable() + { + return m_eventLoopWatchVariable; + }; + std::mutex& getTaskSchedulerMutex() + { + return m_taskSchedulerMutex; + }; + + unsigned sendSetParameterCommand(responseHandler* responseHandler, char const* parameterName, char const* parameterValue, Authenticator* authenticator = NULL); + unsigned sendGetParameterCommand(responseHandler* responseHandler, char const* parameterName, Authenticator* authenticator = NULL); + Boolean setRequestFields(RequestRecord* request, char*& cmdURL, Boolean& cmdURLWasAllocated, char const*& protocolStr, char*& extraHeaders, Boolean& extraHeadersWereAllocated); + +private: + RsRTSPClient(TaskScheduler* t_scheduler, UsageEnvironment* t_env, char const* t_rtspURL, int t_verbosityLevel, char const* t_applicationName, portNumBits t_tunnelOverHTTPPortNum); + + // called only by createNew(); + virtual ~RsRTSPClient(); + + StreamClientState m_scs; + bool isActiveSession = false; //this flag should affect the get/set param commands to run in context of specific session, currently value is always false + std::vector m_supportedProfiles; + std::map m_subsessionMap; + RsRtspReturnValue m_lastReturnValue; + static int m_streamCounter; + // TODO: should we have seperate mutex for each command? + std::condition_variable m_cv; + std::mutex m_commandMtx; + bool m_commandDone = false; + DeviceData m_deviceData; + MemoryPool* m_memPool; + float m_getParamRes; + TaskScheduler* m_scheduler; + UsageEnvironment* m_env; + char m_eventLoopWatchVariable = 0; + std::mutex m_taskSchedulerMutex; +}; diff --git a/src/ethernet/RsSink.cpp b/src/ethernet/RsSink.cpp new file mode 100644 index 0000000000..56b84e7f9c --- /dev/null +++ b/src/ethernet/RsSink.cpp @@ -0,0 +1,174 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#include "RsSink.h" +#include + +#include "stdio.h" +#include + +#include + +#define WRITE_FRAMES_TO_FILE 0 + +RsSink* RsSink::createNew(UsageEnvironment& t_env, MediaSubsession& t_subsession, rs2_video_stream t_stream, MemoryPool* t_memPool, char const* t_streamId) +{ + return new RsSink(t_env, t_subsession, t_stream, t_memPool, t_streamId); +} + +RsSink::RsSink(UsageEnvironment& t_env, MediaSubsession& t_subsession, rs2_video_stream t_stream, MemoryPool* t_memPool, char const* t_streamId) + : MediaSink(t_env) + , m_memPool(t_memPool) + , m_subsession(t_subsession) +{ + m_stream = t_stream; + m_streamId = strDup(t_streamId); + m_bufferSize = t_stream.width * t_stream.height * t_stream.bpp + sizeof(RsFrameHeader); + m_receiveBuffer = nullptr; + m_to = nullptr; + std::string urlStr = m_streamId; + m_afterGettingFunctions.push_back(afterGettingFrameUid0); + m_afterGettingFunctions.push_back(afterGettingFrameUid1); + m_afterGettingFunctions.push_back(afterGettingFrameUid2); + m_afterGettingFunctions.push_back(afterGettingFrameUid3); + + // Remove last "/" + /* + urlStr = urlStr.substr(0, urlStr.size()-1); + std::size_t streamNameIndex = urlStr.find_last_of("/") + 1; + std::string streamName = urlStr.substr(streamNameIndex, urlStr.size()); + + if (streamName.compare("depth") == 0) + { + fp = fopen("file_depth.bin", "ab"); + } + else if((streamName.compare("color") == 0)) + { + fp = fopen("file_rgb.bin", "ab"); + } + */ + if(CompressionFactory::isCompressionSupported(m_stream.fmt, m_stream.type)) + { + m_iCompress = CompressionFactory::getObject(m_stream.width, m_stream.height, m_stream.fmt, m_stream.type, m_stream.bpp); + } + else + { + INF << "compression is disabled or configured unsupported format to zip, run without compression"; + } +} + +RsSink::~RsSink() +{ + if(m_receiveBuffer != nullptr) + { + m_memPool->returnMem(m_receiveBuffer); + } + delete[] m_streamId; + //fclose(fp); +} + +void RsSink::afterGettingFrameUid0(void* t_clientData, unsigned t_frameSize, unsigned t_numTruncatedBytes, struct timeval t_presentationTime, unsigned t_durationInMicroseconds) +{ + + RsSink* sink = (RsSink*)t_clientData; + sink->afterGettingFrame(t_frameSize, t_numTruncatedBytes, t_presentationTime, t_durationInMicroseconds); +} + +void RsSink::afterGettingFrameUid1(void* t_clientData, unsigned t_frameSize, unsigned t_numTruncatedBytes, struct timeval t_presentationTime, unsigned t_durationInMicroseconds) +{ + + RsSink* sink = (RsSink*)t_clientData; + sink->afterGettingFrame(t_frameSize, t_numTruncatedBytes, t_presentationTime, t_durationInMicroseconds); +} + +void RsSink::afterGettingFrameUid2(void* t_clientData, unsigned t_frameSize, unsigned t_numTruncatedBytes, struct timeval t_presentationTime, unsigned t_durationInMicroseconds) +{ + + RsSink* sink = (RsSink*)t_clientData; + sink->afterGettingFrame(t_frameSize, t_numTruncatedBytes, t_presentationTime, t_durationInMicroseconds); +} + +void RsSink::afterGettingFrameUid3(void* t_clientData, unsigned t_frameSize, unsigned t_numTruncatedBytes, struct timeval t_presentationTime, unsigned t_durationInMicroseconds) +{ + + RsSink* sink = (RsSink*)t_clientData; + sink->afterGettingFrame(t_frameSize, t_numTruncatedBytes, t_presentationTime, t_durationInMicroseconds); +} + +// If you don't want to see debugging output for each received frame, then comment out the following line: +#define DEBUG_PRINT_EACH_RECEIVED_FRAME 0 + +void RsSink::afterGettingFrame(unsigned t_frameSize, unsigned t_numTruncatedBytes, struct timeval t_presentationTime, unsigned /*t_durationInMicroseconds*/) +{ + RsNetworkHeader* header = (RsNetworkHeader*)m_receiveBuffer; + if(header->data.frameSize == t_frameSize - sizeof(RsNetworkHeader)) + { + if(this->m_rtpCallback != NULL) + { + if(CompressionFactory::isCompressionSupported(m_stream.fmt, m_stream.type) && m_iCompress != nullptr) + { + m_to = m_memPool->getNextMem(); + if(m_to == nullptr) + { + return; + } + int decompressedSize = m_iCompress->decompressBuffer(m_receiveBuffer + sizeof(RsFrameHeader), header->data.frameSize - sizeof(RsMetadataHeader), m_to + sizeof(RsFrameHeader)); + if(decompressedSize != -1) + { + // copy metadata + memcpy(m_to + sizeof(RsNetworkHeader), m_receiveBuffer + sizeof(RsNetworkHeader), sizeof(RsMetadataHeader)); + this->m_rtpCallback->on_frame((u_int8_t*)m_to + sizeof(RsNetworkHeader), decompressedSize + sizeof(RsMetadataHeader), t_presentationTime); + } + m_memPool->returnMem(m_receiveBuffer); + } + else + { + this->m_rtpCallback->on_frame(m_receiveBuffer + sizeof(RsNetworkHeader), header->data.frameSize, t_presentationTime); + } + } + else + { + // TODO: error, no call back + m_memPool->returnMem(m_receiveBuffer); + envir() << "Frame call back is NULL\n"; + } + } + else + { + m_memPool->returnMem(m_receiveBuffer); + envir() << m_streamId << ":corrupted frame!!!: data size is " << header->data.frameSize << " frame size is " << t_frameSize << "\n"; + } + m_receiveBuffer = nullptr; + + // Then continue, to request the next frame of data + continuePlaying(); +} + +Boolean RsSink::continuePlaying() +{ + if(fSource == NULL) + return False; // sanity check (should not happen) + + // Request the next frame of data from our input source. "afterGettingFrame()" will get called later, when it arrives: + m_receiveBuffer = m_memPool->getNextMem(); + if(m_receiveBuffer == nullptr) + { + return false; + } + + if(m_stream.uid >= 0 && m_stream.uid < m_afterGettingFunctions.size()) + { + fSource->getNextFrame(m_receiveBuffer, m_bufferSize, m_afterGettingFunctions.at(m_stream.uid), this, onSourceClosure, this); + } + else + { + return false; + } + + return True; +} + +void RsSink::setCallback(rtp_callback* t_callback) +{ + this->m_rtpCallback = t_callback; +} diff --git a/src/ethernet/RsSink.h b/src/ethernet/RsSink.h new file mode 100644 index 0000000000..ddfa7bc7be --- /dev/null +++ b/src/ethernet/RsSink.h @@ -0,0 +1,57 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#ifndef RS_SINK_H +#define RS_SINK_H + +#include "BasicUsageEnvironment.hh" +#include "liveMedia.hh" + +#include "rtp_callback.hh" +#include +#include + +#include + +class RsSink : public MediaSink +{ +public: + static RsSink* createNew(UsageEnvironment& t_env, + MediaSubsession& t_subsession, + rs2_video_stream t_stream, // identifies the kind of data that's being received + MemoryPool* t_mempool, + char const* t_streamId = NULL); // identifies the stream itself (optional) + + void setCallback(rtp_callback* t_callback); + +private: + RsSink(UsageEnvironment& t_env, MediaSubsession& t_subsession, rs2_video_stream t_stream, MemoryPool* t_mempool, char const* t_streamId); + // called only by "createNew()" + virtual ~RsSink(); + + static void afterGettingFrameUid0(void* t_clientData, unsigned t_frameSize, unsigned t_numTruncatedBytes, struct timeval t_presentationTime, unsigned t_durationInMicroseconds); + static void afterGettingFrameUid1(void* t_clientData, unsigned t_frameSize, unsigned t_numTruncatedBytes, struct timeval t_presentationTime, unsigned t_durationInMicroseconds); + static void afterGettingFrameUid2(void* t_clientData, unsigned t_frameSize, unsigned t_numTruncatedBytes, struct timeval t_presentationTime, unsigned t_durationInMicroseconds); + static void afterGettingFrameUid3(void* t_clientData, unsigned t_frameSize, unsigned t_numTruncatedBytes, struct timeval t_presentationTime, unsigned t_durationInMicroseconds); + void afterGettingFrame(unsigned t_frameSize, unsigned t_numTruncatedBytes, struct timeval t_presentationTime, unsigned t_durationInMicroseconds); + +private: + // redefined virtual functions: + virtual Boolean continuePlaying(); + +private: + unsigned char* m_receiveBuffer; + unsigned char* m_to; + int m_bufferSize; + MediaSubsession& m_subsession; + char* m_streamId; + FILE* m_fp; + + rtp_callback* m_rtpCallback; + rs2_video_stream m_stream; + std::shared_ptr m_iCompress; + MemoryPool* m_memPool; + std::vector m_afterGettingFunctions; +}; + +#endif // RS_SINK_H diff --git a/src/ethernet/StreamClientState.h b/src/ethernet/StreamClientState.h new file mode 100644 index 0000000000..1f6699f094 --- /dev/null +++ b/src/ethernet/StreamClientState.h @@ -0,0 +1,26 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#ifndef _STREAM_CLIENT_STATE_H +#define _STREAM_CLIENT_STATE_H + +#include "RsMediaSession.hh" + +// Define a class to hold per-stream state that we maintain throughout each stream's lifetime: + +class StreamClientState +{ +public: + StreamClientState() + : m_session(NULL) + {} + virtual ~StreamClientState() + { + Medium::close(m_session); + } + +public: + RsMediaSession* m_session; +}; + +#endif // _STREAM_CLIENT_STATE_H diff --git a/src/ethernet/common/RsRtspCommon.h b/src/ethernet/common/RsRtspCommon.h new file mode 100644 index 0000000000..ce76814de8 --- /dev/null +++ b/src/ethernet/common/RsRtspCommon.h @@ -0,0 +1,19 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +enum RsRtspReturnCode +{ + OK, + ERROR_GENERAL, + ERROR_NETWROK, + ERROR_TIME_OUT, + ERROR_WRONG_FLOW +}; + +struct RsRtspReturnValue +{ + RsRtspReturnCode exit_code; + std::string msg; +}; diff --git a/src/ethernet/ip_device.cpp b/src/ethernet/ip_device.cpp new file mode 100644 index 0000000000..32057d335f --- /dev/null +++ b/src/ethernet/ip_device.cpp @@ -0,0 +1,411 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#include "ip_device.hh" +#include + +#include "api.h" +#include + +#include +#include +#include + +#include + +extern std::map, rs2_extrinsics> minimal_extrinsics_map; + +std::string sensors_str[] = {STEREO_SENSOR_NAME, RGB_SENSOR_NAME}; + +//WA for stop +void ip_device::recover_rtsp_client(int sensor_index) +{ + remote_sensors[sensor_index]->rtsp_client = RsRTSPClient::getRtspClient(std::string("rtsp://" + ip_address + ":8554/" + sensors_str[sensor_index]).c_str(), "ethernet_device"); + + ((RsRTSPClient*)remote_sensors[sensor_index]->rtsp_client)->initFunc(&rs_rtp_stream::get_memory_pool()); + ((RsRTSPClient*)remote_sensors[sensor_index]->rtsp_client)->getStreams(); +} + +ip_device::~ip_device() +{ + DBG << "Destroying ip_device"; + + try + { + is_device_alive = false; + + if (sw_device_status_check.joinable()) + { + sw_device_status_check.join(); + } + + for (int remote_sensor_index = 0; remote_sensor_index < NUM_OF_SENSORS; remote_sensor_index++) + { + update_sensor_state(remote_sensor_index, {}, false); + delete (remote_sensors[remote_sensor_index]); + } + } + catch (const std::exception &e) + { + ERR << e.what(); + } + DBG << "Destroying ip_device completed"; +} + +void ip_device::stop_sensor_streams(int sensor_index) +{ + for(long long int key : remote_sensors[sensor_index]->active_streams_keys) + { + DBG << "Stopping stream [uid:key] " << streams_collection[key].get()->m_rs_stream.uid << ":" << key << "]"; + streams_collection[key].get()->is_enabled = false; + if(inject_frames_thread[key].joinable()) + inject_frames_thread[key].join(); + } + remote_sensors[sensor_index]->active_streams_keys.clear(); +} + +ip_device::ip_device(std::string ip_address, rs2::software_device sw_device) +{ + this->ip_address = ip_address; + this->is_device_alive = true; + + //init device data + init_device_data(sw_device); +} + +std::vector ip_device::query_streams(int sensor_id) +{ + DBG << "query_streams"; + std::vector streams; + + if(remote_sensors[sensor_id]->rtsp_client == NULL) + return streams; + + //workaround + if(remote_sensors[sensor_id]->rtsp_client == nullptr) + recover_rtsp_client(sensor_id); + + streams = remote_sensors[sensor_id]->rtsp_client->getStreams(); + return streams; +} +std::vector ip_device::get_controls(int sensor_id) +{ + DBG << "get_controls"; + std::vector controls; + controls = remote_sensors[sensor_id]->rtsp_client->getControls(); + + return controls; +} + +bool ip_device::init_device_data(rs2::software_device sw_device) +{ + std::vector device_streams; + std::string url, sensor_name = ""; + for(int sensor_id = 0; sensor_id < NUM_OF_SENSORS; sensor_id++) + { + + url = std::string("rtsp://" + ip_address + ":8554/" + sensors_str[sensor_id]); + sensor_name = sensors_str[sensor_id]; + + remote_sensors[sensor_id] = new ip_sensor(); + + remote_sensors[sensor_id]->rtsp_client = RsRTSPClient::getRtspClient(url.c_str(), "ip_device_device"); + ((RsRTSPClient*)remote_sensors[sensor_id]->rtsp_client)->initFunc(&rs_rtp_stream::get_memory_pool()); + + rs2::software_sensor tmp_sensor = sw_device.add_sensor(sensor_name); + + remote_sensors[sensor_id]->sw_sensor = std::make_shared(tmp_sensor); + + if(sensor_id == 1) //todo: remove hard coded + { + std::vector controls = get_controls(sensor_id); + for(auto& control : controls) + { + + float val = NAN; + + INF << "Init sensor " << control.sensorId << ", option '" << control.option << "', value " << control.range.def; + + if(control.range.min == control.range.max) + { + remote_sensors[control.sensorId]->sw_sensor->add_read_only_option(control.option, control.range.def); + } + else + { + remote_sensors[control.sensorId]->sw_sensor->add_option(control.option, {control.range.min, control.range.max, control.range.def, control.range.step}); + } + remote_sensors[control.sensorId]->sensors_option[control.option] = control.range.def; + try + { + get_option_value(control.sensorId, control.option, val); + if(val != control.range.def && val >= control.range.min && val <= control.range.max) + { + remote_sensors[control.sensorId]->sw_sensor->set_option(control.option, val); + } + } + catch(const std::exception& e) + { + ERR << e.what(); + } + } + } + + auto streams = query_streams(sensor_id); + + DBG << "Init got " << streams.size() << " streams per sensor " << sensor_id; + + for(int stream_index = 0; stream_index < streams.size(); stream_index++) + { + bool is_default=false; + // just for readable code + rs2_video_stream st = streams[stream_index]; + long long int stream_key = RsRTSPClient::getStreamProfileUniqueKey(st); + + //check if default value per this stream type were picked + if(default_streams[std::make_pair(st.type, st.index)] == -1) + { + if (st.width==DEFAULT_PROFILE_WIDTH && st.height==DEFAULT_PROFILE_HIGHT && st.fps==DEFAULT_PROFILE_FPS + && (st.type != rs2_stream::RS2_STREAM_COLOR || st.fmt == DEFAULT_PROFILE_COLOR_FORMAT)) + { + default_streams[std::make_pair(st.type, st.index)] = stream_index; + is_default=true; + } + } + + auto stream_profile = remote_sensors[sensor_id]->sw_sensor->add_video_stream(st, is_default); + device_streams.push_back(stream_profile); + streams_collection[stream_key] = std::make_shared(st, stream_profile); + memory_pool = &rs_rtp_stream::get_memory_pool(); + } + DBG << "Init done adding streams for sensor ID: " << sensor_id; + } + + for(auto stream_profile_from : device_streams) + { + for(auto stream_profile_to : device_streams) + { + int from_key = RsRTSPClient::getPhysicalSensorUniqueKey(stream_profile_from.stream_type(), stream_profile_from.stream_index()); + int to_key = RsRTSPClient::getPhysicalSensorUniqueKey(stream_profile_from.stream_type(), stream_profile_from.stream_index()); + + if(minimal_extrinsics_map.find(std::make_pair(from_key, to_key)) == minimal_extrinsics_map.end()) + { + ERR << "Extrinsics data is missing."; + } + rs2_extrinsics extrinisics = minimal_extrinsics_map[std::make_pair(from_key, to_key)]; + + stream_profile_from.register_extrinsics_to(stream_profile_to, extrinisics); + } + } + + //poll sw device streaming state + this->sw_device_status_check = std::thread(&ip_device::polling_state_loop, this); + return true; +} + +void ip_device::polling_state_loop() +{ + while(this->is_device_alive) + { + try + { + bool enabled; + for(int i = 0; i < NUM_OF_SENSORS; i++) + { + //poll start/stop events + auto sw_sensor = remote_sensors[i]->sw_sensor.get(); + + enabled = (sw_sensor->get_active_streams().size() > 0); + + if (remote_sensors[i]->is_enabled != enabled) + { + try + { + //the ftate flag is togled before the actual updatee to avoid endless re-tries on case of failure. + remote_sensors[i]->is_enabled = enabled; + update_sensor_state(i, sw_sensor->get_active_streams(), true); + } + catch(const std::exception& e) + { + ERR << e.what(); + update_sensor_state(i, {}, true); + rs2_software_notification notification; + notification.description = e.what(); + notification.severity = RS2_LOG_SEVERITY_ERROR; + notification.type = RS2_EXCEPTION_TYPE_UNKNOWN; + remote_sensors[i]->sw_sensor.get()->on_notification(notification); + continue; + } + } + auto sensor_supported_option = sw_sensor->get_supported_options(); + for(rs2_option opt : sensor_supported_option) + { + if(remote_sensors[i]->sensors_option[opt] != (float)sw_sensor->get_option(opt)) + { + //TODO: get from map once to reduce logarithmic complexity + remote_sensors[i]->sensors_option[opt] = (float)sw_sensor->get_option(opt); + INF << "Option '" << opt << "' has changed to: " << remote_sensors[i]->sensors_option[opt]; + update_option_value(i, opt, remote_sensors[i]->sensors_option[opt]); + } + } + } + } + catch(const std::exception& e) + { + ERR << e.what(); + } + std::this_thread::sleep_for(std::chrono::milliseconds(POLLING_SW_DEVICE_STATE_INTERVAL)); + } +} + +void ip_device::update_option_value(int sensor_index, rs2_option opt, float val) +{ + float updated_value = 0; + set_option_value(sensor_index, opt, val); + get_option_value(sensor_index, opt, updated_value); + if(val != updated_value) + { + //TODO:: to uncomment after adding exception handling + //throw std::runtime_error("[update_option_value] error"); + ERR << "Cannot update option value."; + } +} + +void ip_device::set_option_value(int sensor_index, rs2_option opt, float val) +{ + if(sensor_index < (sizeof(remote_sensors) / sizeof(remote_sensors[0])) && remote_sensors[sensor_index] != nullptr) + { + remote_sensors[sensor_index]->rtsp_client->setOption(std::string(sensors_str[sensor_index]), opt, val); + } +} + +void ip_device::get_option_value(int sensor_index, rs2_option opt, float& val) +{ + if(sensor_index < sizeof(remote_sensors) && remote_sensors[sensor_index] != nullptr) + { + remote_sensors[sensor_index]->rtsp_client->getOption(std::string(sensors_str[sensor_index]), opt, val); + } +} + +rs2_video_stream convert_stream_object(rs2::video_stream_profile sp) +{ + rs2_video_stream retVal; + retVal.fmt = sp.format(); + retVal.type = sp.stream_type(); + retVal.fps = sp.fps(); + retVal.width = sp.width(); + retVal.height = sp.height(); + retVal.index = sp.stream_index(); + + return retVal; +} + +void ip_device::update_sensor_state(int sensor_index, std::vector updated_streams, bool recover) +{ + //check if need to close all + if(updated_streams.size() == 0) + { + remote_sensors[sensor_index]->rtsp_client->stop(); + remote_sensors[sensor_index]->rtsp_client->close(); + remote_sensors[sensor_index]->rtsp_client = nullptr; + stop_sensor_streams(sensor_index); + if(recover) + { + recover_rtsp_client(sensor_index); + } + return; + } + for(size_t i = 0; i < updated_streams.size(); i++) + { + rs2::video_stream_profile vst(updated_streams[i]); + + long long int requested_stream_key = RsRTSPClient::getStreamProfileUniqueKey(convert_stream_object(vst)); + + if(streams_collection.find(requested_stream_key) == streams_collection.end()) + { + throw std::runtime_error("[update_sensor_state] stream key: " + std::to_string(requested_stream_key) + " is not found. closing device."); + } + + rtp_callbacks[requested_stream_key] = new rs_rtp_callback(streams_collection[requested_stream_key]); + remote_sensors[sensor_index]->rtsp_client->addStream(streams_collection[requested_stream_key].get()->m_rs_stream, rtp_callbacks[requested_stream_key]); + inject_frames_thread[requested_stream_key] = std::thread(&ip_device::inject_frames_loop, this, streams_collection[requested_stream_key]); + remote_sensors[sensor_index]->active_streams_keys.push_front(requested_stream_key); + } + + remote_sensors[sensor_index]->rtsp_client->start(); + INF << "Stream started for sensor " << sensor_index; +} + +int stream_type_to_sensor_id(rs2_stream type) +{ + if(type == RS2_STREAM_INFRARED || type == RS2_STREAM_DEPTH) + return 0; + return 1; +} + +void ip_device::inject_frames_loop(std::shared_ptr rtp_stream) +{ + try + { + rtp_stream.get()->is_enabled = true; + + rtp_stream.get()->frame_data_buff.frame_number = 0; + int uid = rtp_stream.get()->m_rs_stream.uid; + rs2_stream type = rtp_stream.get()->m_rs_stream.type; + int sensor_id = stream_type_to_sensor_id(type); + + while(rtp_stream.get()->is_enabled == true) + { + if(rtp_stream.get()->queue_size() != 0) + { + Raw_Frame* frame = rtp_stream.get()->extract_frame(); + rtp_stream.get()->frame_data_buff.pixels = frame->m_buffer; + + rtp_stream.get()->frame_data_buff.timestamp = frame->m_metadata->data.timestamp; + + rtp_stream.get()->frame_data_buff.frame_number++; + rtp_stream.get()->frame_data_buff.domain = frame->m_metadata->data.timestampDomain; + + remote_sensors[sensor_id]->sw_sensor->set_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP, rtp_stream.get()->frame_data_buff.timestamp); + remote_sensors[sensor_id]->sw_sensor->set_metadata(RS2_FRAME_METADATA_ACTUAL_FPS, frame->m_metadata->data.actualFps); + remote_sensors[sensor_id]->sw_sensor->set_metadata(RS2_FRAME_METADATA_FRAME_COUNTER, rtp_stream.get()->frame_data_buff.frame_number); + remote_sensors[sensor_id]->sw_sensor->set_metadata(RS2_FRAME_METADATA_FRAME_EMITTER_MODE, 1); + + remote_sensors[sensor_id]->sw_sensor->set_metadata(RS2_FRAME_METADATA_TIME_OF_ARRIVAL, std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count()); + remote_sensors[sensor_id]->sw_sensor->on_video_frame(rtp_stream.get()->frame_data_buff); + } + } + + rtp_stream.get()->reset_queue(); + DBG << "Polling data at stream " << rtp_stream.get()->m_rs_stream.uid << " completed"; + } + catch(const std::exception& ex) + { + ERR << ex.what(); + } +} + +rs2_device* rs2_create_net_device(int api_version, const char* address, rs2_error** error) BEGIN_API_CALL +{ + verify_version_compatibility(api_version); + VALIDATE_NOT_NULL(address); + + std::string addr(address); + + // create sw device + rs2::software_device sw_dev = rs2::software_device([](rs2_device*) {}); + // create IP instance + ip_device* ip_dev = new ip_device(addr, sw_dev); + // set client destruction functioun + sw_dev.set_destruction_callback([ip_dev] { delete ip_dev; }); + // register device info to sw device + DeviceData data = ip_dev->remote_sensors[0]->rtsp_client->getDeviceData(); + sw_dev.update_info(RS2_CAMERA_INFO_NAME, data.name + " IP Device"); + sw_dev.register_info(rs2_camera_info::RS2_CAMERA_INFO_IP_ADDRESS, addr); + sw_dev.register_info(rs2_camera_info::RS2_CAMERA_INFO_SERIAL_NUMBER, data.serialNum); + sw_dev.register_info(rs2_camera_info::RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR, data.usbType); + // return sw device + //return sw_dev; + + return sw_dev.get().get(); +} +HANDLE_EXCEPTIONS_AND_RETURN(nullptr, api_version, address) diff --git a/src/ethernet/ip_device.hh b/src/ethernet/ip_device.hh new file mode 100644 index 0000000000..1dd8815143 --- /dev/null +++ b/src/ethernet/ip_device.hh @@ -0,0 +1,84 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "RsRtspClient.h" +#include "ip_sensor.hh" +#include "rs_rtp_callback.hh" + +#include "option.h" +#include "software-device.h" +#include +#include + +#include + +#define MAX_ACTIVE_STREAMS 4 + +#define NUM_OF_SENSORS 2 + +#define POLLING_SW_DEVICE_STATE_INTERVAL 100 + +#define DEFAULT_PROFILE_FPS 15 + +#define DEFAULT_PROFILE_WIDTH 424 + +#define DEFAULT_PROFILE_HIGHT 240 + +#define DEFAULT_PROFILE_COLOR_FORMAT RS2_FORMAT_RGB8 + +class ip_device +{ + +public: + ip_device(std::string ip_address, rs2::software_device sw_device); + ~ip_device(); + + ip_sensor* remote_sensors[NUM_OF_SENSORS]; + +private: + bool is_device_alive; + + //TODO: get smart ptr + MemoryPool* memory_pool; + + std::string ip_address; + + //todo: consider wrapp all maps to single container + std::map> streams_collection; + + std::map inject_frames_thread; + + std::map rtp_callbacks; + + std::thread sw_device_status_check; + + bool init_device_data(rs2::software_device sw_device); + + void polling_state_loop(); + + void inject_frames_loop(std::shared_ptr rtp_stream); + + void stop_sensor_streams(int sensor_id); + + void update_sensor_state(int sensor_index, std::vector updated_streams, bool recover); + void update_option_value(int sensor_index, rs2_option opt, float val); + + void set_option_value(int sensor_index, rs2_option opt, float val); + void get_option_value(int sensor_index, rs2_option opt, float& val); + + std::vector query_streams(int sensor_id); + + std::vector get_controls(int sensor_id); + + void recover_rtsp_client(int sensor_index); + + // default device stream index per type + sensor_index + // streams will be loaded at runtime so here the place holder + std::map,int> default_streams = + { + { std::make_pair(rs2_stream::RS2_STREAM_COLOR,0),-1}, + { std::make_pair(rs2_stream::RS2_STREAM_DEPTH,0),-1} + }; +}; diff --git a/src/ethernet/ip_sensor.hh b/src/ethernet/ip_sensor.hh new file mode 100644 index 0000000000..24a0dd7050 --- /dev/null +++ b/src/ethernet/ip_sensor.hh @@ -0,0 +1,40 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "software-device.h" +#include + +#include + +// Holds ip sensor data. +// 1. sw sensor +// 2. relative streams +// 3. state -> on/off + +class ip_sensor +{ +public: + //todo: remove smart ptr here + std::shared_ptr sw_sensor; + + std::list active_streams_keys; + + std::map sensors_option; + + bool is_enabled; + + //TODO: get smart ptr from rtsp client creator + IRsRtsp* rtsp_client; + + ip_sensor(/* args */); + ~ip_sensor(); +}; + +ip_sensor::ip_sensor(/* args */) +{ + is_enabled = false; +} + +ip_sensor::~ip_sensor() {} diff --git a/src/ethernet/realsense-net.def b/src/ethernet/realsense-net.def new file mode 100644 index 0000000000..8108068edf --- /dev/null +++ b/src/ethernet/realsense-net.def @@ -0,0 +1,4 @@ +LIBRARY + +EXPORTS + rs2_create_net_device diff --git a/src/ethernet/rs_rtp_callback.cpp b/src/ethernet/rs_rtp_callback.cpp new file mode 100644 index 0000000000..a0671cf694 --- /dev/null +++ b/src/ethernet/rs_rtp_callback.cpp @@ -0,0 +1,13 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2019 Intel Corporation. All Rights Reserved. + +#include "rs_rtp_callback.hh" + +#include + +void rs_rtp_callback::on_frame(unsigned char* buffer, ssize_t size, struct timeval presentationTime) +{ + m_rtp_stream.get()->insert_frame(new Raw_Frame((char*)buffer, size, presentationTime)); +} + +rs_rtp_callback::~rs_rtp_callback() {} diff --git a/src/ethernet/rs_rtp_callback.hh b/src/ethernet/rs_rtp_callback.hh new file mode 100644 index 0000000000..84ef917522 --- /dev/null +++ b/src/ethernet/rs_rtp_callback.hh @@ -0,0 +1,37 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "rs_rtp_stream.hh" + +#include +#include +#include +#include + +class rs_rtp_callback : public rtp_callback +{ +private: + std::shared_ptr m_rtp_stream; + + int m_stream_uid; + + int arrive_frames_counter; + +public: + rs_rtp_callback(std::shared_ptr rtp_stream) + { + m_rtp_stream = rtp_stream; + m_stream_uid = m_rtp_stream.get()->m_rs_stream.uid; + } + + ~rs_rtp_callback(); + + void on_frame(unsigned char* buffer, ssize_t size, struct timeval presentationTime); + + int arrived_frames() + { + return arrive_frames_counter; + } +}; diff --git a/src/ethernet/rs_rtp_stream.hh b/src/ethernet/rs_rtp_stream.hh new file mode 100644 index 0000000000..5916fd0ab4 --- /dev/null +++ b/src/ethernet/rs_rtp_stream.hh @@ -0,0 +1,128 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2019 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "RsRtspClient.h" +#include "RsSink.h" + +#include "software-device.h" +#include + +#include + +const int RTP_QUEUE_MAX_SIZE = 30; + +struct Raw_Frame +{ + Raw_Frame(char* buffer, int size, struct timeval timestamp) + : m_metadata((RsMetadataHeader*)buffer) + , m_buffer(buffer + sizeof(RsMetadataHeader)) + , m_size(size) + , m_timestamp(timestamp){}; + Raw_Frame(const Raw_Frame&); + Raw_Frame& operator=(const Raw_Frame&); + ~Raw_Frame() + { + delete[] m_buffer; + }; + + RsMetadataHeader* m_metadata; + char* m_buffer; + unsigned int m_size; + struct timeval m_timestamp; +}; + +class rs_rtp_stream +{ +public: + rs_rtp_stream(rs2_video_stream rs_stream, rs2::stream_profile rs_profile) + { + frame_data_buff.bpp = rs_stream.bpp; + + frame_data_buff.profile = rs_profile; + m_stream_profile = rs_profile; + frame_data_buff.stride = rs_stream.bpp * rs_stream.width; + pixels_buff.resize(frame_data_buff.stride * rs_stream.height, 0); + frame_data_buff.pixels = pixels_buff.data(); + frame_data_buff.deleter = this->frame_deleter; + + m_rs_stream = rs_stream; + } + + const rs2::stream_profile get_stream_profile() + { + return m_stream_profile; + } + + rs2_stream stream_type() + { + return m_rs_stream.type; + } + + void insert_frame(Raw_Frame* new_raw_frame) + { + if(queue_size() > RTP_QUEUE_MAX_SIZE) + { + ERR << "Queue is full. Dropping frame for: " << this->m_rs_stream.uid; + } + else + { + std::lock_guard lock(this->stream_lock); + frames_queue.push(new_raw_frame); + } + } + + // extrinsics between this stream to all other streams + // the key is generated by RsRTSPClient::getStreamProfileUniqueKey function + std::map extrinsics_map; + + Raw_Frame* extract_frame() + { + std::lock_guard lock(this->stream_lock); + Raw_Frame* frame = frames_queue.front(); + frames_queue.pop(); + return frame; + } + + void reset_queue() + { + while(!frames_queue.empty()) + { + frames_queue.pop(); + } + INF << "Frames queue cleaned for " << m_rs_stream.uid; + } + + int queue_size() + { + std::lock_guard lock(this->stream_lock); + return frames_queue.size(); + } + + static MemoryPool& get_memory_pool() + { + static MemoryPool memory_pool_instance = MemoryPool(); + return memory_pool_instance; + } + + bool is_enabled; + + rs2_video_stream m_rs_stream; + + rs2_software_video_frame frame_data_buff; + +private: + static void frame_deleter(void* p) + { + get_memory_pool().returnMem((unsigned char*)p - sizeof(RsFrameHeader)); + } + + rs2::stream_profile m_stream_profile; + + std::mutex stream_lock; + + std::queue frames_queue; + + std::vector pixels_buff; +}; diff --git a/src/ethernet/rtp_callback.hh b/src/ethernet/rtp_callback.hh new file mode 100644 index 0000000000..0f7287ce41 --- /dev/null +++ b/src/ethernet/rtp_callback.hh @@ -0,0 +1,16 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2019 Intel Corporation. All Rights Reserved. + +#pragma once + +#include +#include +#include +#include + +class rtp_callback +{ +public: + void virtual on_frame(unsigned char* buffer, ssize_t size, struct timeval presentationTime) = 0; + +}; diff --git a/src/ipDeviceCommon/MemoryPool.h b/src/ipDeviceCommon/MemoryPool.h new file mode 100644 index 0000000000..8f8fba794d --- /dev/null +++ b/src/ipDeviceCommon/MemoryPool.h @@ -0,0 +1,96 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include + +#include +#include +#include + +#include "NetdevLog.h" + +#define POOL_SIZE 100 + +class MemoryPool +{ + +public: + MemoryPool() + { + //alloc memory + std::unique_lock lk(m_mutex); + for(int i = 0; i < POOL_SIZE; i++) + { + unsigned char *mem = new unsigned char[sizeof(RsFrameHeader) + MAX_FRAME_SIZE]; //TODO:to use OutPacketBuffer::maxSize; + m_pool.push(mem); + } + lk.unlock(); + } + + MemoryPool(const MemoryPool& obj) + { + m_pool = obj.m_pool; + } + + MemoryPool& operator=(const MemoryPool&& obj) + { + m_pool = obj.m_pool; + return *this; + } + + unsigned char* getNextMem() + { + unsigned char* mem = nullptr; + std::unique_lock lk(m_mutex); + if(!m_pool.empty()) + { + mem = m_pool.front(); + m_pool.pop(); + } + else + { + ERR << "getNextMem: pool is empty"; + } + lk.unlock(); + return mem; + } + + void returnMem(unsigned char* t_mem) + { + if(t_mem != nullptr) + { + std::unique_lock lk(m_mutex); + m_pool.push(t_mem); + lk.unlock(); + } + else + { + ERR << "returnMem: invalid address"; + } + } + + ~MemoryPool() + { + std::unique_lock lk(m_mutex); + while(!m_pool.empty()) + { + unsigned char* mem = m_pool.front(); + m_pool.pop(); + if(mem != nullptr) + { + delete mem; + } + else + { + ERR << "~memory_pool: invalid address"; + } + } + lk.unlock(); + } + +private: + std::queue m_pool; + std::mutex m_mutex; +}; diff --git a/src/ipDeviceCommon/NetdevLog.h b/src/ipDeviceCommon/NetdevLog.h new file mode 100644 index 0000000000..7f8167057f --- /dev/null +++ b/src/ipDeviceCommon/NetdevLog.h @@ -0,0 +1,11 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include + +#define DBG CLOG(DEBUG, "librealsense") +#define ERR CLOG(ERROR, "librealsense") +#define WRN CLOG(WARNING, "librealsense") +#define INF CLOG(INFO, "librealsense") \ No newline at end of file diff --git a/src/ipDeviceCommon/RsCommon.h b/src/ipDeviceCommon/RsCommon.h new file mode 100644 index 0000000000..1de262ed4d --- /dev/null +++ b/src/ipDeviceCommon/RsCommon.h @@ -0,0 +1,55 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "string.h" +#include + +#pragma pack(push, 1) + +union RsNetworkHeader { //IMPORTANT:: RsNetworkHeader should be alligned to 16 bytes, this enables frame data to start on 16 bit alligned address + char maxHeaderSize[128]; + struct + { + uint32_t frameSize; + } data; +}; + +union RsMetadataHeader { //IMPORTANT:: RsNetworkHeader should be alligned to 16 bytes, this enables frame data to start on 16 bit alligned address + char maxHeaderSize[128]; + struct + { + double timestamp; + long long frameCounter; + int actualFps; + rs2_timestamp_domain timestampDomain; + } data; +}; + +struct RsFrameHeader +{ + RsNetworkHeader networkHeader; + RsMetadataHeader metadataHeader; +}; + +struct IpDeviceControlData +{ + int sensorId; + rs2_option option; + rs2::option_range range; +}; + +const std::string STEREO_SENSOR_NAME("Stereo Module"); +const std::string RGB_SENSOR_NAME("RGB Camera"); +const std::string RS_MEDIA_TYPE("RS_VIDEO"); +const std::string RS_PAYLOAD_FORMAT("RS_FORMAT"); +const int MAX_WIDTH = 1280; +const int MAX_HEIGHT = 720; +const int MAX_BPP = 3; +const int MAX_FRAME_SIZE = MAX_WIDTH * MAX_HEIGHT * MAX_BPP; +const int MAX_MESSAGE_SIZE = MAX_FRAME_SIZE + sizeof(RsFrameHeader); +const unsigned int SDP_MAX_LINE_LENGHT = 4000; +const unsigned int RTP_TIMESTAMP_FREQ = 90000; + +#pragma pack(pop) diff --git a/src/ipDeviceCommon/RsUsageEnvironment.cpp b/src/ipDeviceCommon/RsUsageEnvironment.cpp new file mode 100644 index 0000000000..844cc08ae9 --- /dev/null +++ b/src/ipDeviceCommon/RsUsageEnvironment.cpp @@ -0,0 +1,110 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#include "RsUsageEnvironment.h" + +#ifdef BUILD_EASYLOGGINGCPP +#ifdef BUILD_SHARED_LIBS +INITIALIZE_EASYLOGGINGPP +#endif +#endif + +RSUsageEnvironment::RSUsageEnvironment(TaskScheduler& taskScheduler) + : BasicUsageEnvironment(taskScheduler) +{} + +RSUsageEnvironment::~RSUsageEnvironment() +{ + CLOG(INFO, "netdev") << "RealSense network logging closed"; + + el::Loggers::unregisterLogger("librealsense"); + el::Loggers::unregisterLogger("netdev"); +} + +RSUsageEnvironment* RSUsageEnvironment::createNew(TaskScheduler& taskScheduler) +{ + RSUsageEnvironment* env = new RSUsageEnvironment(taskScheduler); + + if(env) + { + env->ptr = env->buffer; + env->netdev_log = el::Loggers::getLogger("netdev"); + env->lrs_log = el::Loggers::getLogger("librealsense"); + + el::Loggers::reconfigureAllLoggers(el::Level::Global, el::ConfigurationType::Format, "%datetime{%y%M%d%H%m%s.%g} [%logger]\t%levshort: %msg"); + el::Loggers::reconfigureAllLoggers(el::Level::Debug, el::ConfigurationType::Enabled, "false"); + el::Loggers::reconfigureAllLoggers(el::Level::Global, el::ConfigurationType::ToStandardOutput, "false"); + el::Loggers::reconfigureAllLoggers(el::Level::Global, el::ConfigurationType::ToFile, "true"); + + CLOG(INFO, "netdev") << "RealSense network logging initialized"; + } + + return env; +} + +void RSUsageEnvironment::flush() +{ + *ptr = '\0'; + CLOG(INFO, "netdev") << buffer; + ptr = buffer; +} + +void RSUsageEnvironment::check() +{ + if((ptr - buffer) > (RS_MAX_LOG_MSG_SIZE - RS_MAX_LOG_MSG_THLD)) + { + flush(); + } +} + +UsageEnvironment& RSUsageEnvironment::operator<<(char const* str) +{ + int num = 0; + + if(str == NULL) + str = "(NULL)"; // sanity check + + while(str[num] != '\0') + { + if(str[num] == '\n') + { + flush(); + } + else + { + *ptr++ = str[num]; + check(); + } + num++; + } + + return *this; +} + +UsageEnvironment& RSUsageEnvironment::operator<<(int i) +{ + ptr += sprintf(ptr, "%d", i); + check(); + return *this; +} + +UsageEnvironment& RSUsageEnvironment::operator<<(unsigned u) +{ + ptr += sprintf(ptr, "%u", u); + check(); + return *this; +} + +UsageEnvironment& RSUsageEnvironment::operator<<(double d) +{ + ptr += sprintf(ptr, "%f", d); + check(); + return *this; +} + +UsageEnvironment& RSUsageEnvironment::operator<<(void* p) +{ + ptr += sprintf(ptr, "%p", p); + check(); + return *this; +} diff --git a/src/ipDeviceCommon/RsUsageEnvironment.h b/src/ipDeviceCommon/RsUsageEnvironment.h new file mode 100644 index 0000000000..1ebbf13718 --- /dev/null +++ b/src/ipDeviceCommon/RsUsageEnvironment.h @@ -0,0 +1,37 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include +#include + +#define RS_MAX_LOG_MSG_SIZE 1024 +#define RS_MAX_LOG_MSG_THLD 128 + +class RSUsageEnvironment : public BasicUsageEnvironment +{ +public: + static RSUsageEnvironment* createNew(TaskScheduler& taskScheduler); + + virtual UsageEnvironment& operator<<(char const* str); + virtual UsageEnvironment& operator<<(int i); + virtual UsageEnvironment& operator<<(unsigned u); + virtual UsageEnvironment& operator<<(double d); + virtual UsageEnvironment& operator<<(void* p); + +protected: + RSUsageEnvironment(TaskScheduler& taskScheduler); + // called only by "createNew()" (or subclass constructors) + virtual ~RSUsageEnvironment(); + +private: + void flush(); + void check(); + + char buffer[RS_MAX_LOG_MSG_SIZE]; + char* ptr; + + el::Logger* netdev_log; + el::Logger* lrs_log; +}; diff --git a/src/ipDeviceCommon/Statistic.h b/src/ipDeviceCommon/Statistic.h new file mode 100644 index 0000000000..d9ec62fa24 --- /dev/null +++ b/src/ipDeviceCommon/Statistic.h @@ -0,0 +1,30 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "time.h" +#include +#include +#include + +class StreamStatistic +{ +public: + std::queue m_clockBeginVec; + std::chrono::system_clock::time_point m_prevClockBegin, m_compressionBegin, m_decompressionBegin; + std::chrono::duration m_processingTime, m_getFrameDiffTime, m_compressionTime, m_decompressionTime; + int m_frameCounter = 0, m_compressionFrameCounter = 0, m_decompressionFrameCounter = 0; + double m_avgProcessingTime = 0, m_avgGettingTime = 0, m_avgCompressionTime = 0, m_avgDecompressionTime = 0; + long long m_decompressedSizeSum = 0, m_compressedSizeSum = 0; +}; + +class Statistic +{ +public: + static std::map& getStatisticStreams() //todo:change to uid instead of type + { + static std::map m_streamStatistic; + return m_streamStatistic; + }; +}; diff --git a/src/software-device.cpp b/src/software-device.cpp index 641c41705e..2441719ec6 100644 --- a/src/software-device.cpp +++ b/src/software-device.cpp @@ -95,8 +95,8 @@ namespace librealsense 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!"); + //LOG_WARNING("Video stream unique ID already exist!"); + //throw rs2::error("Stream unique ID already exist!"); } auto profile = std::make_shared( diff --git a/src/types.cpp b/src/types.cpp index bb1efba043..ad38cb8f3f 100644 --- a/src/types.cpp +++ b/src/types.cpp @@ -406,6 +406,7 @@ namespace librealsense CASE(USB_TYPE_DESCRIPTOR) CASE(ASIC_SERIAL_NUMBER) CASE(FIRMWARE_UPDATE_ID) + CASE(IP_ADDRESS) default: assert(!is_valid(value)); return UNKNOWN_VALUE; } #undef CASE diff --git a/third-party/CMakeLists.txt b/third-party/CMakeLists.txt index 877d8787ad..b46d0e5dba 100644 --- a/third-party/CMakeLists.txt +++ b/third-party/CMakeLists.txt @@ -10,3 +10,23 @@ if(BUILD_EASYLOGGINGPP) endif() add_subdirectory(${_rel_path}/realsense-file) + +if(BUILD_NETWORK_DEVICE) + + add_subdirectory(${_rel_path}/live555) + + include(ExternalProject) + + ### libjpeg-turbo ################################################### + ExternalProject_Add (libjpeg-turbo + PREFIX libjpeg-turbo + GIT_REPOSITORY "https://github.com/libjpeg-turbo/libjpeg-turbo.git" + GIT_TAG "master" + SOURCE_DIR "${CMAKE_BINARY_DIR}/third-party/libjpeg-turbo" + CMAKE_ARGS "-DCMAKE_INSTALL_PREFIX=${CMAKE_BINARY_DIR}/libjpeg-turbo" + "-DCMAKE_GENERATOR=${CMAKE_GENERATOR}" + "-DCMAKE_POSITION_INDEPENDENT_CODE=ON" + "-DCMAKE_TOOLCHAIN_FILE=${CMAKE_TOOLCHAIN_FILE}" + ) + +endif() diff --git a/third-party/live555/CMakeLists.txt b/third-party/live555/CMakeLists.txt new file mode 100644 index 0000000000..afdf2cd9ce --- /dev/null +++ b/third-party/live555/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 2.8.2) +project(live-download NONE) + +set(live_file "${CMAKE_BINARY_DIR}/third-party/live555/live555-latest.tar.gz") + +function(remote_source url) + if (EXISTS ${live_file}) + message(STATUS "LIVE555: File exists, delete it to download again (${live_file})") + else() + message(STATUS "LIVE555: Downloading ${url}, this could take up to 3 min ") + file(DOWNLOAD ${url} ${live_file} LOG log STATUS status TIMEOUT 180 SHOW_PROGRESS) + list(GET status 0 error_code) + if (NOT ${error_code} EQUAL 0) + message(FATAL_ERROR "Download LIVE555 (${status}) - ${url}") + else() + message(STATUS "LIVE555: Downloaded (${status}) from ${url} to ${live_file}") + endif() + endif() + + message(STATUS "LIVE555: Unpacking source") + execute_process( + COMMAND + ${CMAKE_COMMAND} -E + tar xzf "${live_file}" + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/third-party + RESULT_VARIABLE tar_res + OUTPUT_VARIABLE tar_out + ) + message(STATUS "LIVE555: Source unpacked") +endfunction() + +if(BUILD_NETWORK_DEVICE) + remote_source( "http://www.live555.com/liveMedia/public/live555-latest.tar.gz") +endif() + + diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index 446ecfcaad..1d19364724 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -18,6 +18,12 @@ add_subdirectory(terminal) add_subdirectory(recorder) add_subdirectory(fw-update) +if(NOT WIN32) + if(BUILD_NETWORK_DEVICE) + add_subdirectory(rs-server) + endif() +endif() + if(BUILD_GRAPHICAL_EXAMPLES) include(${CMAKE_SOURCE_DIR}/CMake/opengl_config.cmake) if (NOT BUILD_GLSL_EXTENSIONS) diff --git a/tools/realsense-viewer/CMakeLists.txt b/tools/realsense-viewer/CMakeLists.txt index ec58f46063..bfd0575a01 100644 --- a/tools/realsense-viewer/CMakeLists.txt +++ b/tools/realsense-viewer/CMakeLists.txt @@ -10,7 +10,6 @@ find_package(Threads REQUIRED) include_directories(${CMAKE_BINARY_DIR}) - set( ELPP_DIR ../../third-party/easyloggingpp/src ) include_directories( ${ELPP_DIR} ) set( ELPP_FILES @@ -18,7 +17,6 @@ set( ELPP_FILES ${ELPP_DIR}/easylogging++.h ) - include(../../common/CMakeLists.txt) if(BUILD_GRAPHICAL_EXAMPLES) @@ -156,16 +154,20 @@ include_directories( set_property(TARGET realsense-viewer PROPERTY CXX_STANDARD 11) -target_link_libraries(realsense-viewer ${DEPENDENCIES} - ${GTK3_LIBRARIES} - Threads::Threads - realsense2-gl - ) +set(RS_VIEWER_LIBS ${GTK3_LIBRARIES} Threads::Threads realsense2-gl) + if (IMPORT_DEPTH_CAM_FW) add_definitions(-DINTERNAL_FW) target_link_libraries(realsense-viewer fw) endif() +if(BUILD_NETWORK_DEVICE) + add_definitions(-DNETWORK_DEVICE) + set(RS_VIEWER_LIBS ${RS_VIEWER_LIBS} realsense2-net) +endif() + +target_link_libraries(realsense-viewer ${DEPENDENCIES} ${RS_VIEWER_LIBS}) + set_target_properties (realsense-viewer PROPERTIES FOLDER Tools ) diff --git a/tools/realsense-viewer/realsense-viewer.cpp b/tools/realsense-viewer/realsense-viewer.cpp index f7d9a0437e..b8f600b7b8 100644 --- a/tools/realsense-viewer/realsense-viewer.cpp +++ b/tools/realsense-viewer/realsense-viewer.cpp @@ -1,7 +1,11 @@ // License: Apache 2.0. See LICENSE file in root directory. // Copyright(c) 2017 Intel Corporation. All Rights Reserved. + #include +#ifdef NETWORK_DEVICE +#include +#endif #include "viewer.h" #include "os.h" #include "ux-window.h" @@ -17,6 +21,7 @@ #include #include #include +#include #include @@ -39,6 +44,19 @@ INITIALIZE_EASYLOGGINGPP using namespace rs2; using namespace rs400; +#define MIN_IP_SIZE 7 //TODO: Ester - update size when host name is supported + +bool add_remote_device(context& ctx, std::string address) +{ +#ifdef NETWORK_DEVICE + rs2::net_device dev(address); + dev.add_to(ctx); + return true; // NEtwork device exists +#else + return false; +#endif +} + void add_playback_device(context& ctx, device_models_list& device_models, std::string& error_message, viewer_model& viewer_model, const std::string& file) { @@ -185,7 +203,7 @@ bool refresh_devices(std::mutex& m, bool added = false; if (device_models.size() == 0 && - dev.supports(RS2_CAMERA_INFO_NAME) && std::string(dev.get_info(RS2_CAMERA_INFO_NAME)) != "Platform Camera") + dev.supports(RS2_CAMERA_INFO_NAME) && std::string(dev.get_info(RS2_CAMERA_INFO_NAME)) != "Platform Camera" && std::string(dev.get_info(RS2_CAMERA_INFO_NAME)).find("IP Device") == std::string::npos) { device_models.emplace_back(new device_model(dev, error_message, viewer_model)); viewer_model.not_model.add_log(to_string() << (*device_models.rbegin())->dev.get_info(RS2_CAMERA_INFO_NAME) << " was selected as a default device"); @@ -195,7 +213,10 @@ bool refresh_devices(std::mutex& m, if (!initial_refresh) { if (added || dev.is()) - viewer_model.not_model.add_notification({ dev_descriptor.first + " Connected\n", + viewer_model.not_model.add_notification({ dev_descriptor.first + " Connected\n", + RS2_LOG_SEVERITY_INFO, RS2_NOTIFICATION_CATEGORY_UNKNOWN_ERROR }); + else if (added || dev.supports(RS2_CAMERA_INFO_IP_ADDRESS)) + viewer_model.not_model.add_notification({ dev_descriptor.first + " Connected\n", RS2_LOG_SEVERITY_INFO, RS2_NOTIFICATION_CATEGORY_UNKNOWN_ERROR }); else viewer_model.not_model.add_notification({ dev_descriptor.first + " Connected\n", @@ -267,12 +288,26 @@ int main(int argc, const char** argv) try std::shared_ptr device_models = std::make_shared(); device_model* device_to_remove = nullptr; + bool is_ip_device_connected = false; + std::string ip_address; viewer_model viewer_model(ctx); std::vector connected_devs; std::mutex m; + if (argc == 2) + { + try + { + is_ip_device_connected = add_remote_device(ctx, argv[1]);; + } + catch (std::runtime_error e) + { + error_message = e.what(); + } + } + #if 1 // Configure the logger el::Configurations conf; @@ -305,7 +340,7 @@ int main(int argc, const char** argv) try window.on_file_drop = [&](std::string filename) { - std::string error_message{}; + add_playback_device(ctx, *device_models, error_message, viewer_model, filename); if (!error_message.empty()) { @@ -391,7 +426,29 @@ int main(int argc, const char** argv) try ImGui::PushFont(window.get_font()); - ImGui::SetNextWindowSize({ viewer_model.panel_width, 20.f * new_devices_count + 8 }); + + int multiline_devices_names = 0; + for (size_t i = 0; i < device_names.size(); i++) + { + if(device_names[i].first.find("\n") != std::string::npos) + { + bool show_device_in_list = true; + for (auto&& dev_model : *device_models) + { + if (get_device_name(dev_model->dev) == device_names[i]) + { + show_device_in_list = false; + break; + } + } + if(show_device_in_list) + { + multiline_devices_names++; + } + } + } + + ImGui::SetNextWindowSize({ viewer_model.panel_width, 20.f * (new_devices_count + multiline_devices_names) + 8 + (is_ip_device_connected? 0 : 20)}); if (ImGui::BeginPopup("select")) { ImGui::PushStyleColor(ImGuiCol_Text, dark_grey); @@ -450,6 +507,122 @@ int main(int argc, const char** argv) try ImGui::Text("%s", ""); ImGui::NextColumn(); + bool close_ip_popup = false; + + if (!is_ip_device_connected) + { + //ImGui::Separator(); + if (ImGui::Selectable("Add Network Device", false, ImGuiSelectableFlags_SpanAllColumns | ImGuiSelectableFlags_DontClosePopups)) + { +#ifdef NETWORK_DEVICE + ip_address = config_file::instance().get_or_default(configurations::viewer::last_ip, std::string{}); + ImGui::OpenPopup("Network Device"); +#else + error_message = "To enable RealSense device over network, please build the SDK with CMake flag -DBUILD_NETWORK_DEVICE=ON.\nThis binary distribution was built with network features disabled."; +#endif + } + + float width = 300; + float height = 125; + float posx = window.width() * 0.5f - width * 0.5f; + float posy = window.height() * 0.5f - height * 0.5f; + ImGui::SetNextWindowPos({ posx, posy }); + ImGui::SetNextWindowSize({ width, height }); + ImGui::PushStyleColor(ImGuiCol_PopupBg, sensor_bg); + ImGui::PushStyleColor(ImGuiCol_TextSelectedBg, white); + ImGui::PushStyleColor(ImGuiCol_Text, light_grey); + ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 0); + + if (ImGui::BeginPopupModal("Network Device", nullptr, ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove)) + { + ImGui::SetCursorPosY(ImGui::GetCursorPosY() + 3); + ImGui::SetCursorPosX(10); + ImGui::Text("Connect to a Linux system running rs-server"); + + ImGui::SetCursorPosY(ImGui::GetCursorPosY() + 5); + + bool connect = false; + static char ip_input[17]; + std::copy(ip_address.begin(), ip_address.end(), ip_input); + ip_input[ip_address.size()] = '\0'; + ImGui::SetCursorPosY(ImGui::GetCursorPosY() + 5); + ImGui::SetCursorPosX(10); + ImGui::Text("Device IP: "); + ImGui::SameLine(); + //ImGui::SetCursorPosY(ImGui::GetCursorPosY() - 1); + ImGui::PushItemWidth(width - ImGui::GetCursorPosX() - 10); + if (ImGui::GetWindowIsFocused() && + !ImGui::IsAnyItemActive() && ip_address == "") + { + ImGui::SetKeyboardFocusHere(); + } + ImGui::PushStyleColor(ImGuiCol_TextSelectedBg, light_blue); + + + ImGui::SetCursorPosY(ImGui::GetCursorPosY() - 3); + if (ImGui::InputText("##ip", ip_input, 16, + ImGuiInputTextFlags_CharsDecimal || + ImGuiInputTextFlags_EnterReturnsTrue + )) + { + ip_address = ip_input; + } + ImGui::PopStyleColor(); + + ImGui::SetCursorPosY(ImGui::GetCursorPosY() + 6); + + ImGui::PopItemWidth(); + ImGui::SetCursorPosX(width / 2 - 105); + + std::regex rgx("\\d{1,3}(\\.\\d{1,3}){3}"); + if (!std::regex_match(ip_address,rgx)) + { + ImGui::ButtonEx("OK",{100.f, 25.f}, ImGuiButtonFlags_Disabled); + } + else + { + if (ImGui::ButtonEx("OK",{100.f, 25.f}) || ImGui::IsKeyDown(GLFW_KEY_ENTER) || ImGui::IsKeyDown(GLFW_KEY_KP_ENTER)) + { + try + { + is_ip_device_connected = add_remote_device(ctx, ip_address);; + refresh_devices(m, ctx, devices_connection_changes, connected_devs, device_names, *device_models, viewer_model, error_message); + auto dev = connected_devs[connected_devs.size()-1]; + device_models->emplace_back(new device_model(dev, error_message, viewer_model)); + config_file::instance().set(configurations::viewer::last_ip, ip_address); + } + catch (std::runtime_error e) + { + error_message = e.what(); + } + ip_address = ""; + close_ip_popup = true; + ImGui::CloseCurrentPopup(); + } + } + ImGui::SameLine(); + ImGui::SetCursorPosX(width / 2 + 5); + if(ImGui::Button("Cancel",{100.f, 25.f}) || ImGui::IsKeyDown(GLFW_KEY_ESCAPE)) + { + ip_address = ""; + close_ip_popup = true; + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } + ImGui::PopStyleColor(3); + ImGui::PopStyleVar(1); + + ImGui::NextColumn(); + ImGui::Text("%s", ""); + ImGui::NextColumn(); + } + + if (close_ip_popup) + { + ImGui::CloseCurrentPopup(); + close_ip_popup = false; + } ImGui::PopStyleColor(); ImGui::EndPopup(); } diff --git a/tools/rs-server/CMakeLists.txt b/tools/rs-server/CMakeLists.txt new file mode 100644 index 0000000000..e39879453c --- /dev/null +++ b/tools/rs-server/CMakeLists.txt @@ -0,0 +1,62 @@ +# License: Apache 2.0. See LICENSE file in root directory. +# Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +# minimum required cmake version: 3.1.0 +cmake_minimum_required(VERSION 3.1.0) + +project(rs-server VERSION 1.0.0 LANGUAGES CXX C) + +set(THREADS_PREFER_PTHREAD_FLAG ON) +find_package(Threads REQUIRED) + +# Save the command line compile commands in the build output +set(CMAKE_EXPORT_COMPILE_COMMANDS 1) + +set(DEPENDENCIES ${DEPENDENCIES} realsense2 Threads::Threads) + +set(CMAKE_CXX_FLAGS "-DNEWLOCALE_NOT_USED=1 -DBSD=1 -DSOCKLEN_T=socklen_t -D_FILE_OFFSET_BITS=64 -D_LARGEFILE_SOURCE=1 -DALLOW_RTSP_SERVER_PORT_REUSE=1 -DNO_OPENSSL=1 -latomic") + +SET(CMAKE_EXE_LINKER_FLAGS ${CMAKE_EXE_LINKER_FLAGS} "-pthread") + +set(LIVE ${CMAKE_BINARY_DIR}/third-party/live) + +if(WIN32) + message(SEND_ERROR "rs-server supports Linux only") +else() + file(GLOB RS_SERVER_SOURCES LIST_DIRECTORIES false CONFIGURE_DEPENDS + "*.c*" + "../../src/ipDeviceCommon/*.c*" + "${LIVE}/*.c*" + "${LIVE}/groupsock/*.c*" + "${LIVE}/BasicUsageEnvironment/*.c*" + "${LIVE}/liveMedia/*.c*" + "${LIVE}/UsageEnvironment/*.c*" + ) + add_executable(${PROJECT_NAME} ${RS_SERVER_SOURCES}) + add_definitions(-DELPP_NO_DEFAULT_LOG_FILE) + + include_directories(${PROJECT_NAME} + ../../common + ../../src/ipDeviceCommon + ../../include/librealsense2 + ../../third-party/easyloggingpp/src + ${LIVE}/groupsock/include + ${LIVE}/liveMedia/include + ${LIVE}/UsageEnvironment/include + ${LIVE}/BasicUsageEnvironment/include + ) + + set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) + + set(DEPENDENCIES ${DEPENDENCIES} realsense2 Threads::Threads realsense2-compression ${ZLIB_LIBRARIES} ${JPEG_LIBRARIES}) + + target_link_libraries(${PROJECT_NAME} ${DEPENDENCIES}) + + set_target_properties(${PROJECT_NAME} PROPERTIES FOLDER "Tools") + + target_compile_definitions(${PROJECT_NAME} PUBLIC RESPONSE_BUFFER_SIZE=100000) + + install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) +endif() + + diff --git a/tools/rs-server/RsDevice.cpp b/tools/rs-server/RsDevice.cpp new file mode 100644 index 0000000000..45791ba2bc --- /dev/null +++ b/tools/rs-server/RsDevice.cpp @@ -0,0 +1,62 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#include "RsDevice.hh" +#include "RsUsageEnvironment.h" +#include +#include + +int RsDevice::getPhysicalSensorUniqueKey(rs2_stream stream_type, int sensors_index) +{ + return stream_type * 10 + sensors_index; +} + +RsDevice::RsDevice(UsageEnvironment* t_env) + : env(t_env) +{ + //get LRS device + // The context represents the current platform with respect to connected devices + + bool found = false; + bool first = true; + do + { + rs2::context ctx; + rs2::device_list devices = ctx.query_devices(); + rs2::device dev; + if(devices.size()) + { + try + { + m_device = devices[0]; // Only one device is supported + *env << "RealSense Device Connected\n"; + + found = true; + } + catch(const std::exception& e) + { + std::cerr << e.what() << '\n'; + } + } + if(!found) + { + if(first) + { + std::cerr << "Waiting for Device..." << std::endl; + first = false; + } + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + } while(!found); + + //get RS sensors + for(auto& sensor : m_device.query_sensors()) + { + m_sensors.push_back(RsSensor(env, sensor, m_device)); + } +} + +RsDevice::~RsDevice() +{ + std::cerr << "RsDevice destructor" << std::endl; +} diff --git a/tools/rs-server/RsDevice.hh b/tools/rs-server/RsDevice.hh new file mode 100644 index 0000000000..387fd7c962 --- /dev/null +++ b/tools/rs-server/RsDevice.hh @@ -0,0 +1,39 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include +#include "RsUsageEnvironment.h" +#include "RsSensor.hh" +#include + +class RsDevice +{ +public: + RsDevice(UsageEnvironment* t_env); + ~RsDevice(); + std::vector& getSensors() + { + return m_sensors; + } + + static int getPhysicalSensorUniqueKey(rs2_stream stream_type, int sensors_index); + + // sensor index + // map for stream pysical sensor + // key is generated by rs2_stream+index: depth=1,color=2,irl=3,irr=4 + // todo: make smart_ptr + std::map, rs2_extrinsics> minimal_extrinsics_map; + + rs2::device getDevice() + { + return m_device; + } + +private: + rs2::device m_device; + std::vector m_sensors; + + UsageEnvironment* env; +}; diff --git a/tools/rs-server/RsRTSPServer.cpp b/tools/rs-server/RsRTSPServer.cpp new file mode 100644 index 0000000000..68f1ff85ff --- /dev/null +++ b/tools/rs-server/RsRTSPServer.cpp @@ -0,0 +1,315 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#include "RTSPServer.hh" +#include "RTSPCommon.hh" +#include "RTSPRegisterSender.hh" +#include "Base64.hh" +#include +#include "RsRTSPServer.hh" +#include "RsServerMediaSession.h" +#include "RsServerMediaSubsession.h" +#include "librealsense2/hpp/rs_options.hpp" +#include +#include "RsUsageEnvironment.h" + +// RTSPServer implementation + +RsRTSPServer* RsRTSPServer::createNew(UsageEnvironment& t_env, std::shared_ptr t_device, Port t_ourPort, UserAuthenticationDatabase* t_authDatabase, unsigned t_reclamationSeconds) +{ + int ourSocket = setUpOurSocket(t_env, t_ourPort); + if(ourSocket == -1) + return NULL; + + return new RsRTSPServer(t_env, t_device, ourSocket, t_ourPort, t_authDatabase, t_reclamationSeconds); +} + +RsRTSPServer::RsRTSPServer(UsageEnvironment& t_env, std::shared_ptr t_device, int t_ourSocket, Port t_ourPort, UserAuthenticationDatabase* t_authDatabase, unsigned t_reclamationSeconds) + : RTSPServer(t_env, t_ourSocket, t_ourPort, t_authDatabase, t_reclamationSeconds) + , m_device(t_device) +{} + +RsRTSPServer::~RsRTSPServer() {} + +std::string getOptionString(rs2_option t_opt, float t_min, float t_max, float t_def, float t_step) +{ + std::ostringstream oss; + oss << (int)t_opt << "{" << t_min << "," << t_max << "," << t_def << "," << t_step << "}" + << ";"; + return oss.str(); +} + +char const* RsRTSPServer::allowedCommandNames() +{ + m_supportedOptionsStr.clear(); + for(const auto& optionsPair : m_supportedOptions) + { + m_supportedOptionsStr.append(optionsPair.first); + m_supportedOptionsStr.append("["); + for(auto option : optionsPair.second) + { + m_supportedOptionsStr.append(getOptionString(option.m_opt, option.m_range.min, option.m_range.max, option.m_range.def, option.m_range.step)); + } + m_supportedOptionsStr.append("]"); + } + return m_supportedOptionsStr.c_str(); +} + +void RsRTSPServer::setSupportedOptions(std::string t_key, std::vector t_supportedOptions) +{ + m_supportedOptions.insert(std::pair>(t_key, t_supportedOptions)); +} + +// RTSPServer::RTSPClientConnection implementation + +RsRTSPServer::RsRTSPClientConnection ::RsRTSPClientConnection(RsRTSPServer& t_ourServer, int t_clientSocket, struct sockaddr_in t_clientAddr) + : RTSPClientConnection(t_ourServer, t_clientSocket, t_clientAddr) + , m_fOurRsRTSPServer(t_ourServer) +{} + +RsRTSPServer::RsRTSPClientConnection::~RsRTSPClientConnection() {} + +void RsRTSPServer::RsRTSPClientConnection::handleCmd_GET_PARAMETER(char const* t_fullRequestStr) +{ + std::ostringstream oss; + std::vector sensors; + std::string str(t_fullRequestStr); + std::string ContentLength("Content-Length:"); + std::string afterSplit; + + afterSplit = str.substr(str.find(ContentLength) + ContentLength.size()); + char* contLength = strtok((char*)afterSplit.c_str(), "\r\n: "); + char* sensorName = strtok(NULL, "_\r\n:"); + char* option = strtok(NULL, "\r\n:"); + sensors = m_fOurRsRTSPServer.m_device.get()->getSensors(); + for(auto sensor : sensors) + { + if(sensor.getSensorName().compare(sensorName) == 0) + { + try + { + float value = sensor.getRsSensor().get_option((rs2_option)stoi(std::string(option))); + char* paramString = new char[strlen(sensorName) + 1 + strlen(option) + strlen(std::to_string(value).c_str()) + 10]; + sprintf(paramString, "%s_%s: %s\r\n", sensorName, option, std::to_string(value).c_str()); + envir() << "GET_PARAMETER: sensor '" << sensorName << "', option '" << option << "', value " << value << "\n"; + setRTSPResponse("200 OK", paramString); + return; + } + catch(const std::exception& e) + { + std::string error("500 " + std::string(e.what())); + setRTSPResponse(error.c_str()); + return; + } + } + } + setRTSPResponse("500 Invalid Option"); +} + +void RsRTSPServer::RsRTSPClientConnection::handleCmd_SET_PARAMETER(char const* t_fullRequestStr) +{ + std::ostringstream oss; + std::vector sensors; + std::string str(t_fullRequestStr); + std::string ContentLength("Content-Length:"); + std::string afterSplit; //, opt, val; + + afterSplit = str.substr(str.find(ContentLength) + ContentLength.size()); + char* contLength = strtok((char*)afterSplit.c_str(), "\r\n: "); + char* sensorName = strtok(NULL, "_\r\n:"); + char* option = strtok(NULL, "\r\n:"); + char* value = strtok(NULL, "\r\n:"); + + envir() << "SET_PARAMETER: sensor '" << sensorName << "', option '" << option << "', value " << value << "\n"; + sensors = m_fOurRsRTSPServer.m_device.get()->getSensors(); + for(auto sensor : sensors) + { + if(sensor.getSensorName().compare(sensorName) == 0) + { + try + { + sensor.getRsSensor().set_option((rs2_option)stoi(std::string(option)), stof(std::string(value))); + setRTSPResponse("200 OK"); + return; + } + catch(const std::exception& e) + { + std::string error("500 " + std::string(e.what())); + setRTSPResponse(error.c_str()); + return; + } + } + } + setRTSPResponse("500 Invalid Option"); +} + +// RsRTSPServer::RsRTSPClientSession implementation + +RsRTSPServer::RsRTSPClientSession ::RsRTSPClientSession(RTSPServer& t_ourServer, u_int32_t t_sessionId) + : RTSPClientSession(t_ourServer, t_sessionId) +{} + +RsRTSPServer::RsRTSPClientSession::~RsRTSPClientSession() {} + +void RsRTSPServer::RsRTSPClientSession::handleCmd_TEARDOWN(RTSPClientConnection* t_ourClientConnection, ServerMediaSubsession* t_subsession) +{ + envir() << "TEARDOWN \n"; + try + { + closeRsCamera(); + } + catch(const std::exception& e) + { + std::string error("500 " + std::string(e.what())); + setRTSPResponse(t_ourClientConnection, error.c_str()); + return; + } + + RTSPServer::RTSPClientSession::handleCmd_TEARDOWN(t_ourClientConnection, t_subsession); +} + +void RsRTSPServer::RsRTSPClientSession::handleCmd_PLAY(RTSPClientConnection* t_ourClientConnection, ServerMediaSubsession* t_subsession, char const* t_fullRequestStr) +{ + envir() << "PLAY \n"; + try + { + openRsCamera(); + } + catch(const std::exception& e) + { + std::string error("500 " + std::string(e.what())); + setRTSPResponse(t_ourClientConnection, error.c_str()); + return; + } + + RTSPServer::RTSPClientSession::handleCmd_PLAY(t_ourClientConnection, t_subsession, t_fullRequestStr); +} + +void RsRTSPServer::RsRTSPClientSession::handleCmd_PAUSE(RTSPClientConnection* t_ourClientConnection, ServerMediaSubsession* t_subsession) +{ + envir() << "PAUSE \n"; + RTSPServer::RTSPClientSession::handleCmd_PAUSE(t_ourClientConnection, t_subsession); + try + { + closeRsCamera(); + } + catch(const std::exception& e) + { + std::string error("500 " + std::string(e.what())); + setRTSPResponse(t_ourClientConnection, error.c_str()); + return; + } +} +void RsRTSPServer::RsRTSPClientSession::handleCmd_GET_PARAMETER(RTSPClientConnection* t_ourClientConnection, ServerMediaSubsession* t_subsession, char const* t_fullRequestStr) +{ + std::ostringstream oss; + std::vector sensors; + std::string str(t_fullRequestStr); + std::string ContentLength("Content-Length:"); + std::string afterSplit; //, opt, val; + + envir() << "GET_PARAMETER\n"; + afterSplit = str.substr(str.find(ContentLength) + ContentLength.size()); + char* contLength = strtok((char*)afterSplit.c_str(), "\r\n: "); + char* sensorName = strtok(NULL, "_\r\n:"); + char* option = strtok(NULL, "\r\n:"); + try + { + float value = static_cast(fOurServerMediaSession)->getRsSensor().getRsSensor().get_option((rs2_option)stoi(std::string(option))); + char* paramString = new char[strlen(sensorName) + 1 + strlen(option) + strlen(std::to_string(value).c_str()) + 10]; + sprintf(paramString, "%s_%s: %s\r\n", sensorName, option, std::to_string(value).c_str()); + setRTSPResponse(t_ourClientConnection, "200 OK", paramString); + } + catch(const std::exception& e) + { + std::string error("500 " + std::string(e.what())); + setRTSPResponse(t_ourClientConnection, error.c_str()); + return; + } +} + +void RsRTSPServer::RsRTSPClientSession::handleCmd_SET_PARAMETER(RTSPClientConnection* t_ourClientConnection, ServerMediaSubsession* t_subsession, char const* t_fullRequestStr) +{ + std::ostringstream oss; + std::vector sensors; + std::string str(t_fullRequestStr); + std::string ContentLength("Content-Length:"); + std::string afterSplit; //, opt, val; + + envir() << "SET_PARAMETER \n"; + afterSplit = str.substr(str.find(ContentLength) + ContentLength.size()); + char* contLength = strtok((char*)afterSplit.c_str(), "\r\n: "); + char* sensorName = strtok(NULL, "_\r\n:"); + char* option = strtok(NULL, "\r\n:"); + char* value = strtok(NULL, "\r\n:"); + + try + { + static_cast(fOurServerMediaSession)->getRsSensor().getRsSensor().set_option((rs2_option)stoi(std::string(option)), stof(std::string(value))); + setRTSPResponse(t_ourClientConnection, "200 OK"); + } + catch(const std::exception& e) + { + std::string error("500 " + std::string(e.what())); + setRTSPResponse(t_ourClientConnection, error.c_str()); + return; + } +} +void RsRTSPServer::RsRTSPClientSession::handleCmd_SETUP(RTSPServer::RTSPClientConnection* t_ourClientConnection, char const* t_urlPreSuffix, char const* t_urlSuffix, char const* t_fullRequestStr) +{ + RTSPServer::RTSPClientSession::handleCmd_SETUP(t_ourClientConnection, t_urlPreSuffix, t_urlSuffix, t_fullRequestStr); + ServerMediaSubsession* subsession; + if(t_urlSuffix[0] != '\0' && strcmp(fOurServerMediaSession->streamName(), t_urlPreSuffix) == 0) + { + // Non-aggregated operation. + // Look up the media subsession whose track id is "urlSuffix": + ServerMediaSubsessionIterator iter(*fOurServerMediaSession); + while((subsession = iter.next()) != NULL) + { + if(strcmp(subsession->trackId(), t_urlSuffix) == 0) + { + long long int profileKey = static_cast(fOurServerMediaSession)->getRsSensor().getStreamProfileKey(((RsServerMediaSubsession*)(subsession))->getStreamProfile()); + m_streamProfiles[profileKey] = ((RsServerMediaSubsession*)(subsession))->getFrameQueue(); + break; // success + } + } + } +} + +void RsRTSPServer::RsRTSPClientSession::openRsCamera() +{ + static_cast(fOurServerMediaSession)->openRsCamera(m_streamProfiles); +} + +void RsRTSPServer::RsRTSPClientSession::closeRsCamera() +{ + ((RsServerMediaSession*)fOurServerMediaSession)->closeRsCamera(); + for(int i = 0; i < fNumStreamStates; ++i) + { + if(fStreamStates[i].subsession != NULL) + { + long long int profile_key = static_cast(fOurServerMediaSession)->getRsSensor().getStreamProfileKey(((RsServerMediaSubsession*)(fStreamStates[i].subsession))->getStreamProfile()); + emptyStreamProfileQueue(profile_key); + } + } +} + +void RsRTSPServer::RsRTSPClientSession::emptyStreamProfileQueue(long long int profile_key) +{ + rs2::frame f; + if(m_streamProfiles.find(profile_key) != m_streamProfiles.end()) + { + while(m_streamProfiles[profile_key].poll_for_frame(&f)) + ; + } +} + +GenericMediaServer::ClientConnection* RsRTSPServer::createNewClientConnection(int clientSocket, struct sockaddr_in clientAddr) +{ + return new RsRTSPClientConnection(*this, clientSocket, clientAddr); +} + +GenericMediaServer::ClientSession* RsRTSPServer::createNewClientSession(u_int32_t t_sessionId) +{ + return new RsRTSPClientSession(*this, t_sessionId); +} diff --git a/tools/rs-server/RsRTSPServer.hh b/tools/rs-server/RsRTSPServer.hh new file mode 100644 index 0000000000..e1d0667c5a --- /dev/null +++ b/tools/rs-server/RsRTSPServer.hh @@ -0,0 +1,80 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "RsDevice.hh" +#include + +#include + +class RsRTSPServer : public RTSPServer +{ +public: + static RsRTSPServer* createNew(UsageEnvironment& t_env, std::shared_ptr t_device, Port t_ourPort = 554, UserAuthenticationDatabase* t_authDatabase = NULL, unsigned t_reclamationSeconds = 0); + void setSupportedOptions(std::string t_key, std::vector t_supportedOptions); + +protected: + RsRTSPServer(UsageEnvironment& t_env, std::shared_ptr t_device, int t_ourSocket, Port t_ourPort, UserAuthenticationDatabase* t_authDatabase, unsigned t_reclamationSeconds); + virtual ~RsRTSPServer(); + char const* allowedCommandNames(); + + std::map> m_supportedOptions; + std::string m_supportedOptionsStr; + std::shared_ptr m_device; + +public: + class RsRTSPClientSession; // forward + class RsRTSPClientConnection : public RTSPClientConnection + { + + protected: + RsRTSPClientConnection(RsRTSPServer& t_ourServer, int t_clientSocket, struct sockaddr_in t_clientAddr); + virtual ~RsRTSPClientConnection(); + virtual void handleCmd_GET_PARAMETER(char const* fullRequestStr); + virtual void handleCmd_SET_PARAMETER(char const* fullRequestStr); + + RsRTSPServer& m_fOurRsRTSPServer; + + friend class RsRTSPServer; + friend class RsRTSPClientSession; + }; + // The state of an individual client session (using one or more sequential TCP connections) handled by a RTSP server: + class RsRTSPClientSession : public RTSPClientSession + { + protected: + RsRTSPClientSession(RTSPServer& t_ourServer, u_int32_t t_sessionId); + virtual ~RsRTSPClientSession(); + + friend class RsRTSPServer; + friend class RsRTSPClientConnection; + + virtual void handleCmd_TEARDOWN(RTSPClientConnection* t_ourClientConnection, ServerMediaSubsession* t_subsession); + virtual void handleCmd_PLAY(RTSPClientConnection* t_ourClientConnection, ServerMediaSubsession* t_subsession, char const* t_fullRequestStr); + virtual void handleCmd_PAUSE(RTSPClientConnection* t_ourClientConnection, ServerMediaSubsession* t_subsession); + virtual void handleCmd_GET_PARAMETER(RTSPClientConnection* t_ourClientConnection, ServerMediaSubsession* t_subsession, char const* t_fullRequestStr); + virtual void handleCmd_SET_PARAMETER(RTSPClientConnection* t_ourClientConnection, ServerMediaSubsession* t_subsession, char const* t_fullRequestStr); + + virtual void handleCmd_SETUP(RTSPServer::RTSPClientConnection* t_ourClientConnection, char const* t_urlPreSuffix, char const* t_urlSuffix, char const* t_fullRequestStr); + + void openRsCamera(); + void closeRsCamera(); + void emptyStreamProfileQueue(long long int t_profile_key); + + private: + std::unordered_map m_streamProfiles; + }; + +protected: + virtual ClientConnection* createNewClientConnection(int t_clientSocket, struct sockaddr_in t_clientAddr); + +protected: + virtual ClientSession* createNewClientSession(u_int32_t t_sessionId); + +private: + int openRsCamera(RsSensor t_sensor, std::unordered_map& t_streamProfiles); + +private: + friend class RsRTSPClientConnection; + friend class RsRTSPClientSession; +}; \ No newline at end of file diff --git a/tools/rs-server/RsSensor.cpp b/tools/rs-server/RsSensor.cpp new file mode 100644 index 0000000000..8c08b72310 --- /dev/null +++ b/tools/rs-server/RsSensor.cpp @@ -0,0 +1,187 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#include "RsDevice.hh" +#include "RsUsageEnvironment.h" +#include "compression/CompressionFactory.h" +#include "string.h" +#include +#include +#include +#include + +RsSensor::RsSensor(UsageEnvironment* t_env, rs2::sensor t_sensor, rs2::device t_device) + : env(t_env) + , m_sensor(t_sensor) + , m_device(t_device) +{ + for(rs2::stream_profile streamProfile : m_sensor.get_stream_profiles()) + { + if(streamProfile.is()) + { + //make a map with all the sensor's stream profiles + m_streamProfiles.emplace(getStreamProfileKey(streamProfile), streamProfile.as()); + m_prevSample.emplace(getStreamProfileKey(streamProfile), std::chrono::high_resolution_clock::now()); + } + } + m_memPool = new MemoryPool(); +} + +int RsSensor::open(std::unordered_map& t_streamProfilesQueues) +{ + std::vector requestedStreamProfiles; + for(auto streamProfile : t_streamProfilesQueues) + { + //make a vector of all requested stream profiles + long long int streamProfileKey = streamProfile.first; + requestedStreamProfiles.push_back(m_streamProfiles.at(streamProfileKey)); + if(CompressionFactory::isCompressionSupported(m_streamProfiles.at(streamProfileKey).format(), m_streamProfiles.at(streamProfileKey).stream_type())) + { + rs2::video_stream_profile vsp = m_streamProfiles.at(streamProfileKey); + std::shared_ptr compressPtr = CompressionFactory::getObject(vsp.width(), vsp.height(), vsp.format(), vsp.stream_type(), RsSensor::getStreamProfileBpp(vsp.format())); + if(compressPtr != nullptr) + { + m_iCompress.insert(std::pair>(streamProfileKey, compressPtr)); + } + } + else + { + *env << "unsupported compression format or compression is disabled, continue without compression\n"; + } + } + m_sensor.open(requestedStreamProfiles); + return EXIT_SUCCESS; +} + +int RsSensor::close() +{ + m_sensor.close(); + return EXIT_SUCCESS; +} + +int RsSensor::stop() +{ + m_sensor.stop(); + return EXIT_SUCCESS; +} + +int RsSensor::start(std::unordered_map& t_streamProfilesQueues) +{ + auto callback = [&](const rs2::frame& frame) { + long long int profileKey = getStreamProfileKey(frame.get_profile()); + //check if profile exists in map: + if(t_streamProfilesQueues.find(profileKey) != t_streamProfilesQueues.end()) + { + std::chrono::high_resolution_clock::time_point curSample = std::chrono::high_resolution_clock::now(); + std::chrono::duration timeSpan = std::chrono::duration_cast>(curSample - m_prevSample[profileKey]); + if(CompressionFactory::isCompressionSupported(frame.get_profile().format(), frame.get_profile().stream_type())) + { + unsigned char* buff = m_memPool->getNextMem(); + int frameSize = m_iCompress.at(profileKey)->compressBuffer((unsigned char*)frame.get_data(), frame.get_data_size(), buff); + if(frameSize == -1) + { + m_memPool->returnMem(buff); + return; + } + memcpy((unsigned char*)frame.get_data(), buff, frameSize); + m_memPool->returnMem(buff); + } + //push frame to its queue + t_streamProfilesQueues[profileKey].enqueue(frame); + m_prevSample[profileKey] = curSample; + } + }; + m_sensor.start(callback); + return EXIT_SUCCESS; +} + +long long int RsSensor::getStreamProfileKey(rs2::stream_profile t_profile) +{ + long long int key; + key = t_profile.stream_type() * pow(10, 12) + t_profile.format() * pow(10, 10) + t_profile.fps() * pow(10, 8) + t_profile.stream_index(); + if(t_profile.is()) + { + rs2::video_stream_profile videoStreamProfile = t_profile.as(); + key += videoStreamProfile.width() * pow(10, 4) + videoStreamProfile.height(); + } + return key; +} + +std::string RsSensor::getSensorName() +{ + if(m_sensor.supports(RS2_CAMERA_INFO_NAME)) + { + return std::string(m_sensor.get_info(RS2_CAMERA_INFO_NAME)); + } + else + { + return "Unknown Sensor"; + } +} + +int RsSensor::getStreamProfileBpp(rs2_format t_format) +{ + int bpp = 0; + switch(t_format) + { + case RS2_FORMAT_RGB8: + { + bpp = 3; + break; + } + case RS2_FORMAT_BGR8: + { + bpp = 3; + break; + } + case RS2_FORMAT_RGBA8: + { + bpp = 3; //TODO: need to be 4 bpp, change it after add support for 4 bpp formats + break; + } + case RS2_FORMAT_BGRA8: + { + bpp = 3; //TODO: need to be 4 bpp, change it after add support for 4 bpp formats + break; + } + case RS2_FORMAT_Z16: + case RS2_FORMAT_Y16: + case RS2_FORMAT_Y8: + case RS2_FORMAT_RAW16: + case RS2_FORMAT_YUYV: + case RS2_FORMAT_UYVY: + { + bpp = 2; + break; + } + default: + bpp = 0; + break; + } + return bpp; +} + +std::vector RsSensor::getSupportedOptions() +{ + std::vector returnedVector; + try + { + + std::vector options = m_sensor.get_supported_options(); + for(auto opt : options) + { + if(!m_sensor.supports(opt)) + continue; + + RsOption option; + option.m_opt = opt; + option.m_range = m_sensor.get_option_range(opt); + returnedVector.push_back(option); + } + } + catch(const std::exception& e) + { + *env << e.what() << "\n"; + } + return returnedVector; +} diff --git a/tools/rs-server/RsSensor.hh b/tools/rs-server/RsSensor.hh new file mode 100644 index 0000000000..064453e106 --- /dev/null +++ b/tools/rs-server/RsSensor.hh @@ -0,0 +1,52 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "compression/ICompression.h" +#include +#include +#include +#include +#include + +typedef struct RsOption +{ + rs2_option m_opt; + rs2::option_range m_range; +} RsOption; + +class RsSensor +{ +public: + RsSensor(UsageEnvironment* t_env, rs2::sensor t_sensor, rs2::device t_device); + int open(std::unordered_map& t_streamProfilesQueues); + int start(std::unordered_map& t_streamProfilesQueues); + int close(); + int stop(); + rs2::sensor& getRsSensor() + { + return m_sensor; + } + std::unordered_map getStreamProfiles() + { + return m_streamProfiles; + } + static long long int getStreamProfileKey(rs2::stream_profile t_profile); + std::string getSensorName(); + static int getStreamProfileBpp(rs2_format t_format); + rs2::device getDevice() + { + return m_device; + } + std::vector getSupportedOptions(); + +private: + UsageEnvironment* env; + rs2::sensor m_sensor; + std::unordered_map m_streamProfiles; + std::unordered_map> m_iCompress; + rs2::device m_device; + MemoryPool* m_memPool; + std::unordered_map m_prevSample; +}; diff --git a/tools/rs-server/RsServer.cpp b/tools/rs-server/RsServer.cpp new file mode 100644 index 0000000000..591ce15a68 --- /dev/null +++ b/tools/rs-server/RsServer.cpp @@ -0,0 +1,159 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#include + +#include +#include +#include +#include "RsUsageEnvironment.h" +#include "RsSource.hh" +#include "RsServerMediaSubsession.h" +#include "RsDevice.hh" +#include "RsRTSPServer.hh" +#include "RsServerMediaSession.h" +#include "RsCommon.h" + +struct server +{ + rs2::device selected_device; + RsRTSPServer* rtspServer; + UsageEnvironment* env; + std::shared_ptr rsDevice; + std::vector supported_stream_profiles; // streams for extrinsics map creation + std::vector sensors; + TaskScheduler* scheduler; + + void main(int argc, char** argv) + { + std::cout << "Rs-server is running\n"; + + START_EASYLOGGINGPP(argc, argv); + + OutPacketBuffer::increaseMaxSizeTo(MAX_MESSAGE_SIZE); + + // Begin by setting up our usage environment: + scheduler = BasicTaskScheduler::createNew(); + env = RSUsageEnvironment::createNew(*scheduler); + + rsDevice = std::make_shared(env); + rtspServer = RsRTSPServer::createNew(*env, rsDevice, 8554); + + if(rtspServer == NULL) + { + *env << "Failed to create RTSP server: " << env->getResultMsg() << "\n"; + exit(1); + } + + sensors = rsDevice.get()->getSensors(); + for(auto sensor : sensors) + { + RsServerMediaSession* sms; + if(sensor.getSensorName().compare(STEREO_SENSOR_NAME) == 0 || sensor.getSensorName().compare(RGB_SENSOR_NAME) == 0) + { + sms = RsServerMediaSession::createNew(*env, sensor, sensor.getSensorName().data(), "", "Session streamed by \"realsense streamer\"", True); + } + else + { + break; + } + + for(auto stream_profile : sensor.getStreamProfiles()) + { + rs2::video_stream_profile stream = stream_profile.second; + if(stream.format() == RS2_FORMAT_BGR8 || stream.format() == RS2_FORMAT_RGB8 || stream.format() == RS2_FORMAT_Z16 || stream.format() == RS2_FORMAT_Y8 || stream.format() == RS2_FORMAT_YUYV || stream.format() == RS2_FORMAT_UYVY) + { + if(stream.fps() == 6) + { + if((stream.width() == 1280 && stream.height() == 720) || (stream.width() == 640 && stream.height() == 480) || (stream.width() == 480 && stream.height() == 270) || (stream.width() == 424 && stream.height() == 240)) + { + sms->addSubsession(RsServerMediaSubsession::createNew(*env, stream, rsDevice)); + + supported_stream_profiles.push_back(stream); + continue; + } + } + else if(stream.fps() == 15 || stream.fps() == 30) + { + if((stream.width() == 640 && stream.height() == 480) || (stream.width() == 480 && stream.height() == 270) || (stream.width() == 424 && stream.height() == 240)) + { + sms->addSubsession(RsServerMediaSubsession::createNew(*env, stream, rsDevice)); + supported_stream_profiles.push_back(stream); + continue; + } + } + else if(stream.fps() == 60) + { + if((stream.width() == 480 && stream.height() == 270) || (stream.width() == 424 && stream.height() == 240)) + { + sms->addSubsession(RsServerMediaSubsession::createNew(*env, stream, rsDevice)); + supported_stream_profiles.push_back(stream); + continue; + } + } + } + *env << "Ignoring stream: format: " << stream.format() << " width: " << stream.width() << " height: " << stream.height() << " fps: " << stream.fps() << "\n"; + } + + calculate_extrinsics(); + + rtspServer->addServerMediaSession(sms); + char* url = rtspServer->rtspURL(sms); + *env << "Play this stream using the URL \"" << url << "\"\n"; + + // query camera options + rtspServer->setSupportedOptions(sensor.getSensorName(), sensor.getSupportedOptions()); + + delete[] url; + } + + env->taskScheduler().doEventLoop(); // does not return + } + + void calculate_extrinsics() + { + for(auto stream_profile_from : supported_stream_profiles) + { + for(auto stream_profile_to : supported_stream_profiles) + { + int from_sensor_key = RsDevice::getPhysicalSensorUniqueKey(stream_profile_from.stream_type(), stream_profile_from.stream_index()); + int to_sensor_key = RsDevice::getPhysicalSensorUniqueKey(stream_profile_to.stream_type(), stream_profile_to.stream_index()); + + rsDevice.get()->minimal_extrinsics_map[std::make_pair(from_sensor_key, to_sensor_key)] = stream_profile_from.get_extrinsics_to(stream_profile_to); + } + } + } + + void sigint_handler(int sig) + { + Medium::close(rtspServer); + env->reclaim(); + env = NULL; + delete scheduler; + scheduler = NULL; + std::cout << "Rs-server downloading\n"; + } + + // Make server a proper singleton + // Otherwise, initialization of RsDevice is crashing + // in static build because of order of initialization + static server& instance() + { + static server inst; + return inst; + } +}; + +void sigint_handler(int sig); + +int main(int argc, char** argv) +{ + server::instance().main(argc, argv); + return 0; +} + +void sigint_handler(int sig) +{ + server::instance().sigint_handler(sig); + exit(sig); +} diff --git a/tools/rs-server/RsServerMediaSession.cpp b/tools/rs-server/RsServerMediaSession.cpp new file mode 100644 index 0000000000..bbc94087f5 --- /dev/null +++ b/tools/rs-server/RsServerMediaSession.cpp @@ -0,0 +1,46 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#include "RsServerMediaSession.h" + +// ServerMediaSession + +RsServerMediaSession* RsServerMediaSession ::createNew(UsageEnvironment& t_env, RsSensor& t_sensor, char const* t_streamName, char const* t_info, char const* t_description, Boolean t_isSSM, char const* t_miscSDPLines) +{ + return new RsServerMediaSession(t_env, t_sensor, t_streamName, t_info, t_description, t_isSSM, t_miscSDPLines); +} + +RsServerMediaSession::RsServerMediaSession(UsageEnvironment& t_env, RsSensor& t_sensor, char const* t_streamName, char const* t_info, char const* t_description, Boolean t_isSSM, char const* t_miscSDPLines) + : ServerMediaSession(t_env, t_streamName, t_info, t_description, t_isSSM, t_miscSDPLines) + , m_rsSensor(t_sensor) + , m_isActive(false) +{} + +RsServerMediaSession::~RsServerMediaSession() {} + +void RsServerMediaSession::openRsCamera(std::unordered_map& t_streamProfiles) +{ + if(m_isActive) + { + envir() << "sensor is already open, closing sensor and than open again...\n"; + closeRsCamera(); + } + m_rsSensor.open(t_streamProfiles); + m_rsSensor.start(t_streamProfiles); + m_isActive = true; +} + +void RsServerMediaSession::closeRsCamera() +{ + if(m_isActive) + { + m_rsSensor.getRsSensor().stop(); + m_rsSensor.getRsSensor().close(); + m_isActive = false; + } +} + +RsSensor& RsServerMediaSession::getRsSensor() +{ + return m_rsSensor; +} diff --git a/tools/rs-server/RsServerMediaSession.h b/tools/rs-server/RsServerMediaSession.h new file mode 100644 index 0000000000..b91ff8bdc6 --- /dev/null +++ b/tools/rs-server/RsServerMediaSession.h @@ -0,0 +1,24 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "RsDevice.hh" +#include "ServerMediaSession.hh" + +class RsServerMediaSession : public ServerMediaSession +{ +public: + static RsServerMediaSession* createNew(UsageEnvironment& t_env, RsSensor& t_sensor, char const* t_streamName = NULL, char const* t_info = NULL, char const* t_description = NULL, Boolean t_isSSM = False, char const* t_miscSDPLines = NULL); + RsSensor& getRsSensor(); + void openRsCamera(std::unordered_map& t_streamProfiles); + void closeRsCamera(); + +protected: + RsServerMediaSession(UsageEnvironment& t_env, RsSensor& t_sensor, char const* t_streamName, char const* t_info, char const* t_description, Boolean t_isSSM, char const* t_miscSDPLines); + virtual ~RsServerMediaSession(); + +private: + RsSensor m_rsSensor; + bool m_isActive; +}; diff --git a/tools/rs-server/RsServerMediaSubsession.cpp b/tools/rs-server/RsServerMediaSubsession.cpp new file mode 100644 index 0000000000..bb7bdf57b7 --- /dev/null +++ b/tools/rs-server/RsServerMediaSubsession.cpp @@ -0,0 +1,45 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#include "RsServerMediaSubsession.h" +#include "RsCommon.h" +#include "RsServerMediaSession.h" +#include "RsSimpleRTPSink.h" + +#define CAPACITY 100 + +RsServerMediaSubsession* RsServerMediaSubsession::createNew(UsageEnvironment& t_env, rs2::video_stream_profile& t_videoStreamProfile, std::shared_ptr rsDevice) +{ + return new RsServerMediaSubsession(t_env, t_videoStreamProfile, rsDevice); +} + +RsServerMediaSubsession ::RsServerMediaSubsession(UsageEnvironment& env, rs2::video_stream_profile& t_videoStreamProfile, std::shared_ptr device) + : OnDemandServerMediaSubsession(env, false) + , m_videoStreamProfile(t_videoStreamProfile) +{ + m_frameQueue = rs2::frame_queue(CAPACITY, true); + m_rsDevice = device; +} + +RsServerMediaSubsession::~RsServerMediaSubsession() {} + +rs2::frame_queue& RsServerMediaSubsession::getFrameQueue() +{ + return m_frameQueue; +} + +rs2::video_stream_profile RsServerMediaSubsession::getStreamProfile() +{ + return m_videoStreamProfile; +} + +FramedSource* RsServerMediaSubsession::createNewStreamSource(unsigned /*t_clientSessionId*/, unsigned& t_estBitrate) +{ + t_estBitrate = 20000; + return RsDeviceSource::createNew(envir(), m_videoStreamProfile, m_frameQueue); +} + +RTPSink* RsServerMediaSubsession ::createNewRTPSink(Groupsock* t_rtpGroupsock, unsigned char t_rtpPayloadTypeIfDynamic, FramedSource* /*t_inputSource*/) +{ + return RsSimpleRTPSink::createNew(envir(), t_rtpGroupsock, 96 + m_videoStreamProfile.stream_type(), RTP_TIMESTAMP_FREQ, RS_MEDIA_TYPE.c_str(), RS_PAYLOAD_FORMAT.c_str(), m_videoStreamProfile, m_rsDevice); +} \ No newline at end of file diff --git a/tools/rs-server/RsServerMediaSubsession.h b/tools/rs-server/RsServerMediaSubsession.h new file mode 100644 index 0000000000..a78b01544f --- /dev/null +++ b/tools/rs-server/RsServerMediaSubsession.h @@ -0,0 +1,32 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "RsDevice.hh" +#include + +#ifndef _ON_DEMAND_SERVER_MEDIA_SUBSESSION_HH +#include "OnDemandServerMediaSubsession.hh" +#endif + +#include "RsSource.hh" + +class RsServerMediaSubsession : public OnDemandServerMediaSubsession +{ +public: + static RsServerMediaSubsession* createNew(UsageEnvironment& t_env, rs2::video_stream_profile& t_videoStreamProfile, std::shared_ptr rsDevice); + rs2::frame_queue& getFrameQueue(); + rs2::video_stream_profile getStreamProfile(); + +protected: + RsServerMediaSubsession(UsageEnvironment& t_env, rs2::video_stream_profile& t_video_stream_profile, std::shared_ptr device); + virtual ~RsServerMediaSubsession(); + virtual FramedSource* createNewStreamSource(unsigned t_clientSessionId, unsigned& t_estBitrate); + virtual RTPSink* createNewRTPSink(Groupsock* t_rtpGroupsock, unsigned char t_rtpPayloadTypeIfDynamic, FramedSource* t_inputSource); + +private: + rs2::video_stream_profile m_videoStreamProfile; + rs2::frame_queue m_frameQueue; + std::shared_ptr m_rsDevice; +}; diff --git a/tools/rs-server/RsSimpleRTPSink.cpp b/tools/rs-server/RsSimpleRTPSink.cpp new file mode 100644 index 0000000000..cb2d55acd5 --- /dev/null +++ b/tools/rs-server/RsSimpleRTPSink.cpp @@ -0,0 +1,147 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#include "RsSimpleRTPSink.h" +#include "RsDevice.hh" +#include +#include +#include +#include +#include + +RsSimpleRTPSink* RsSimpleRTPSink::createNew(UsageEnvironment& t_env, + Groupsock* t_RTPgs, + unsigned char t_rtpPayloadFormat, + unsigned t_rtpTimestampFrequency, + char const* t_sdpMediaTypeString, + char const* t_rtpPayloadFormatName, + rs2::video_stream_profile& t_videoStream, + std::shared_ptr device, + unsigned t_numChannels, + Boolean t_allowMultipleFramesPerPacket, + Boolean t_doNormalMBitRule) +{ + CompressionFactory::getIsEnabled() = IS_COMPRESSION_ENABLED; + return new RsSimpleRTPSink(t_env, t_RTPgs, t_rtpPayloadFormat, t_rtpTimestampFrequency, t_sdpMediaTypeString, t_rtpPayloadFormatName, t_videoStream, device, t_numChannels, t_allowMultipleFramesPerPacket, t_doNormalMBitRule); +} + +std::string getSdpLineForField(const char* t_name, int t_val) +{ + std::ostringstream oss; + oss << t_name << "=" << t_val << ";"; + return oss.str(); +} + +std::string getSdpLineForField(const char* t_name, const char* t_val) +{ + std::ostringstream oss; + oss << t_name << "=" << t_val << ";"; + return oss.str(); +} + +std::string extrinsics_to_string(rs2_extrinsics extrinsics) +{ + std::string str; + str.append("rotation:"); + for(float r : extrinsics.rotation) + { + str.append(std::to_string(r)); + str.append(","); + } + str.pop_back(); + str.append("translation:"); + for(float r : extrinsics.translation) + { + str.append(std::to_string(r)); + str.append(","); + } + str.pop_back(); + + return str; +} + +std::string get_extrinsics_string_per_stream(std::shared_ptr device, rs2::video_stream_profile stream) +{ + + if(device == nullptr) + return ""; + + std::string str; + + for(auto relation : device.get()->minimal_extrinsics_map) + { + //check at map for this stream relations + if(relation.first.first == (RsDevice::getPhysicalSensorUniqueKey(stream.stream_type(), stream.stream_index()))) + { + str.append(""); + str.append(extrinsics_to_string(relation.second)); + str.append("&"); + } + } + + return str; +} + +std::string getSdpLineForVideoStream(rs2::video_stream_profile& t_videoStream, std::shared_ptr device) +{ + std::string str; + str.append(getSdpLineForField("width", t_videoStream.width())); + str.append(getSdpLineForField("height", t_videoStream.height())); + str.append(getSdpLineForField("format", t_videoStream.format())); + str.append(getSdpLineForField("uid", t_videoStream.unique_id())); + str.append(getSdpLineForField("fps", t_videoStream.fps())); + str.append(getSdpLineForField("stream_index", t_videoStream.stream_index())); + str.append(getSdpLineForField("stream_type", t_videoStream.stream_type())); + str.append(getSdpLineForField("bpp", RsSensor::getStreamProfileBpp(t_videoStream.format()))); + str.append(getSdpLineForField("cam_serial_num", device.get()->getDevice().get_info(RS2_CAMERA_INFO_SERIAL_NUMBER))); + str.append(getSdpLineForField("usb_type", device.get()->getDevice().get_info(RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR))); + str.append(getSdpLineForField("compression", CompressionFactory::getIsEnabled())); + + str.append(getSdpLineForField("ppx", t_videoStream.get_intrinsics().ppx)); + str.append(getSdpLineForField("ppy", t_videoStream.get_intrinsics().ppy)); + str.append(getSdpLineForField("fx", t_videoStream.get_intrinsics().fx)); + str.append(getSdpLineForField("fy", t_videoStream.get_intrinsics().fy)); + str.append(getSdpLineForField("model", t_videoStream.get_intrinsics().model)); + + for(size_t i = 0; i < 5; i++) + { + str.append(getSdpLineForField("coeff_" + i, t_videoStream.get_intrinsics().coeffs[i])); + } + + str.append(getSdpLineForField("extrinsics", get_extrinsics_string_per_stream(device, t_videoStream).c_str())); + + std::string name = device.get()->getDevice().get_info(RS2_CAMERA_INFO_NAME); + // We don't want to sent spaces over SDP , replace all spaces with '^' + std::replace(name.begin(), name.end(), ' ', '^'); + str.append(getSdpLineForField("cam_name", name.c_str())); + + return str; +} + +RsSimpleRTPSink ::RsSimpleRTPSink(UsageEnvironment& t_env, + Groupsock* t_RTPgs, + unsigned char t_rtpPayloadFormat, + unsigned t_rtpTimestampFrequency, + char const* t_sdpMediaTypeString, + char const* t_rtpPayloadFormatName, + rs2::video_stream_profile& t_videoStream, + std::shared_ptr device, + unsigned t_numChannels, + Boolean t_allowMultipleFramesPerPacket, + Boolean t_doNormalMBitRule) + : SimpleRTPSink(t_env, t_RTPgs, t_rtpPayloadFormat, t_rtpTimestampFrequency, t_sdpMediaTypeString, t_rtpPayloadFormatName, t_numChannels, t_allowMultipleFramesPerPacket, t_doNormalMBitRule) +{ + // Then use this 'config' string to construct our "a=fmtp:" SDP line: + unsigned fmtpSDPLineMaxSize = SDP_MAX_LINE_LENGHT; + m_fFmtpSDPLine = new char[fmtpSDPLineMaxSize]; + std::string sdpStr = getSdpLineForVideoStream(t_videoStream, device); + sprintf(m_fFmtpSDPLine, "a=fmtp:%d;%s\r\n", rtpPayloadType(), sdpStr.c_str()); +} + +char const* RsSimpleRTPSink::auxSDPLine() +{ + return m_fFmtpSDPLine; +} diff --git a/tools/rs-server/RsSimpleRTPSink.h b/tools/rs-server/RsSimpleRTPSink.h new file mode 100644 index 0000000000..173a29871a --- /dev/null +++ b/tools/rs-server/RsSimpleRTPSink.h @@ -0,0 +1,41 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "RsDevice.hh" +#include "liveMedia.hh" +#include + +class RsSimpleRTPSink : public SimpleRTPSink +{ +public: + static RsSimpleRTPSink* createNew(UsageEnvironment& env, + Groupsock* RTPgs, + unsigned char rtpPayloadFormat, + unsigned rtpTimestampFrequency, + char const* sdpMediaTypeString, + char const* rtpPayloadFormatName, + rs2::video_stream_profile& video_stream, + std::shared_ptr device, + unsigned numChannels = 1, + Boolean allowMultipleFramesPerPacket = True, + Boolean doNormalMBitRule = True); + +protected: + RsSimpleRTPSink(UsageEnvironment& env, + Groupsock* RTPgs, + unsigned char rtpPayloadFormat, + unsigned rtpTimestampFrequency, + char const* sdpMediaTypeString, + char const* rtpPayloadFormatName, + rs2::video_stream_profile& video_stream, + std::shared_ptr device, + unsigned numChannels = 1, + Boolean allowMultipleFramesPerPacket = True, + Boolean doNormalMBitRule = True); + +private: + char* m_fFmtpSDPLine; + virtual char const* auxSDPLine(); // for the "a=fmtp:" SDP line +}; diff --git a/tools/rs-server/RsSource.cpp b/tools/rs-server/RsSource.cpp new file mode 100644 index 0000000000..3abe62d313 --- /dev/null +++ b/tools/rs-server/RsSource.cpp @@ -0,0 +1,135 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#include "RsSource.hh" +#include "BasicUsageEnvironment.hh" +#include "RsStatistics.h" +#include +#include +#include +#include +#include +#include + +RsDeviceSource* RsDeviceSource::createNew(UsageEnvironment& t_env, rs2::video_stream_profile& t_videoStreamProfile, rs2::frame_queue& t_queue) +{ + return new RsDeviceSource(t_env, t_videoStreamProfile, t_queue); +} + +RsDeviceSource::RsDeviceSource(UsageEnvironment& t_env, rs2::video_stream_profile& t_videoStreamProfile, rs2::frame_queue& t_queue) + : FramedSource(t_env) +{ + m_framesQueue = &t_queue; + m_streamProfile = &t_videoStreamProfile; +} + +RsDeviceSource::~RsDeviceSource() {} + +void RsDeviceSource::doGetNextFrame() +{ + // This function is called (by our 'downstream' object) when it asks for new data. + + rs2::frame frame; + try + { + if(!m_framesQueue->poll_for_frame(&frame)) + { + nextTask() = envir().taskScheduler().scheduleDelayedTask(0, (TaskFunc*)waitForFrame, this); + } + else + { + frame.keep(); + deliverRSFrame(&frame); + } + } + catch(const std::exception& e) + { + envir() << "RsDeviceSource: " << e.what() << '\n'; + } +} + +void RsDeviceSource::handleWaitForFrame() +{ + // If a new frame of data is immediately available to be delivered, then do this now: + rs2::frame frame; + try + { + if(!(getFramesQueue()->poll_for_frame(&frame))) + { + nextTask() = envir().taskScheduler().scheduleDelayedTask(0, (TaskFunc*)RsDeviceSource::waitForFrame, this); + } + else + { + frame.keep(); + deliverRSFrame(&frame); + } + } + catch(const std::exception& e) + { + envir() << "RsDeviceSource: " << e.what() << '\n'; + } +} + +// The following is called after each delay between packet sends: +void RsDeviceSource::waitForFrame(RsDeviceSource* t_deviceSource) +{ + t_deviceSource->handleWaitForFrame(); +} + +void RsDeviceSource::deliverRSFrame(rs2::frame* t_frame) +{ + if(!isCurrentlyAwaitingData()) + { + envir() << "isCurrentlyAwaitingData returned false\n"; + return; // we're not ready for the data yet + } + + unsigned newFrameSize = t_frame->get_data_size(); + + gettimeofday(&fPresentationTime, NULL); // If you have a more accurate time - e.g., from an encoder - then use that instead. + RsFrameHeader header; + unsigned char* data; + if(CompressionFactory::isCompressionSupported(t_frame->get_profile().format(), t_frame->get_profile().stream_type())) + { + fFrameSize = ((int*)t_frame->get_data())[0]; + data = (unsigned char*)t_frame->get_data() + sizeof(int); + } + else + { + fFrameSize = t_frame->get_data_size(); + data = (unsigned char*)t_frame->get_data(); + } + memmove(fTo + sizeof(RsFrameHeader), data, fFrameSize); + fFrameSize += sizeof(RsMetadataHeader); + header.networkHeader.data.frameSize = fFrameSize; + fFrameSize += sizeof(RsNetworkHeader); + if(t_frame->supports_frame_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP)) + { + header.metadataHeader.data.timestamp = t_frame->get_frame_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP) / 1000; + } + else + { + header.metadataHeader.data.timestamp = t_frame->get_timestamp(); + } + + if(t_frame->supports_frame_metadata(RS2_FRAME_METADATA_FRAME_COUNTER)) + { + header.metadataHeader.data.frameCounter = t_frame->get_frame_metadata(RS2_FRAME_METADATA_FRAME_COUNTER); + } + else + { + header.metadataHeader.data.frameCounter = t_frame->get_frame_number(); + } + + if(t_frame->supports_frame_metadata(RS2_FRAME_METADATA_ACTUAL_FPS)) + { + header.metadataHeader.data.actualFps = t_frame->get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_FPS); + } + + header.metadataHeader.data.timestampDomain = t_frame->get_frame_timestamp_domain(); + + memmove(fTo, &header, sizeof(header)); + + // After delivering the data, inform the reader that it is now available: + FramedSource::afterGetting(this); +} diff --git a/tools/rs-server/RsSource.hh b/tools/rs-server/RsSource.hh new file mode 100644 index 0000000000..4fe2a9df91 --- /dev/null +++ b/tools/rs-server/RsSource.hh @@ -0,0 +1,34 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "DeviceSource.hh" + +#include +#include +#include // Include RealSense Cross Platform API + +class RsDeviceSource : public FramedSource +{ +public: + static RsDeviceSource* createNew(UsageEnvironment& t_env, rs2::video_stream_profile& t_videoStreamProfile, rs2::frame_queue& t_queue); + void handleWaitForFrame(); + static void waitForFrame(RsDeviceSource* t_deviceSource); + +protected: + RsDeviceSource(UsageEnvironment& t_env, rs2::video_stream_profile& t_videoStreamProfile, rs2::frame_queue& t_queue); + virtual ~RsDeviceSource(); + +private: + virtual void doGetNextFrame(); + rs2::frame_queue* getFramesQueue() + { + return m_framesQueue; + }; + void deliverRSFrame(rs2::frame* t_frame); + +private: + rs2::frame_queue* m_framesQueue; + rs2::video_stream_profile* m_streamProfile; +}; diff --git a/tools/rs-server/RsStatistics.h b/tools/rs-server/RsStatistics.h new file mode 100644 index 0000000000..8b57e33207 --- /dev/null +++ b/tools/rs-server/RsStatistics.h @@ -0,0 +1,52 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2020 Intel Corporation. All Rights Reserved. + +#pragma once + +#include + +class RsStatistics +{ +public: + static std::chrono::high_resolution_clock::time_point& getResetPacketStartTp() + { + static std::chrono::high_resolution_clock::time_point tpResetPacketStart = std::chrono::high_resolution_clock::now(); + return tpResetPacketStart; + } + static std::chrono::high_resolution_clock::time_point& getFirstPacketTp() + { + static std::chrono::high_resolution_clock::time_point tpFirstPacket = std::chrono::high_resolution_clock::now(); + return tpFirstPacket; + } + static std::chrono::high_resolution_clock::time_point& getSendPacketTp() + { + static std::chrono::high_resolution_clock::time_point tpSendPacket = std::chrono::high_resolution_clock::now(); + return tpSendPacket; + } + static std::chrono::high_resolution_clock::time_point& getScheduleTp() + { + static std::chrono::high_resolution_clock::time_point tpSchedule = std::chrono::high_resolution_clock::now(); + return tpSchedule; + } + static double& getPrevDiff() + { + static double prevDiff = 0; + return prevDiff; + } + static double isJump() + { + double* prevDiff = &getPrevDiff(); + double diff = 1000 * std::chrono::duration_cast>(std::chrono::high_resolution_clock::now() - RsStatistics::getSendPacketTp()).count(); + double diffOfDiff = diff - *prevDiff; + if(diffOfDiff > 5) + { + *prevDiff = diff; + return diffOfDiff; + } + else + { + *prevDiff = diff; + return 0; + } + } +};