From 4f2953c05d73ded456260087ce4d5b9ad07c1dc4 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 26 Mar 2024 12:49:05 +0000 Subject: [PATCH] Merge ros2-development into ros2-hkr 26.3.24 --- .github/ISSUE_TEMPLATE.md | 26 +++ .github/workflows/main.yml | 6 +- .github/workflows/pre-release.yml | 3 +- .github/workflows/static_analysis.yaml | 2 + README.md | 136 +++++++++-- realsense2_camera/CMakeLists.txt | 36 ++- .../include/base_realsense_node.h | 19 +- realsense2_camera/include/constants.h | 1 - realsense2_camera/include/dynamic_params.h | 5 +- realsense2_camera/include/gl_window.h | 84 +++++++ realsense2_camera/include/profile_manager.h | 12 +- realsense2_camera/launch/rs_launch.py | 6 +- realsense2_camera/src/base_realsense_node.cpp | 54 ++++- realsense2_camera/src/dynamic_params.cpp | 8 + realsense2_camera/src/gl_gpu_processing.cpp | 47 ++++ realsense2_camera/src/parameters.cpp | 18 ++ realsense2_camera/src/profile_manager.cpp | 212 ++++++++++++------ .../src/realsense_node_factory.cpp | 1 - realsense2_camera/src/ros_sensor.cpp | 104 ++++++--- realsense2_camera/src/rs_node_setup.cpp | 108 ++++++++- realsense2_camera/test/README.md | 9 + .../live_camera/test_camera_aligned_tests.py | 10 +- .../test_camera_all_profile_tests.py | 12 +- .../test/live_camera/test_camera_fps_tests.py | 1 + .../test/live_camera/test_camera_imu_tests.py | 1 + .../test_camera_point_cloud_tests.py | 9 +- .../test/live_camera/test_camera_tf_tests.py | 18 +- .../test/live_camera/test_d415_basic_tests.py | 1 + .../test/live_camera/test_d455_basic_tests.py | 2 + .../test/utils/pytest_live_camera_utils.py | 54 +++-- realsense2_description/launch/launch_utils.py | 7 +- 31 files changed, 816 insertions(+), 196 deletions(-) create mode 100644 .github/ISSUE_TEMPLATE.md create mode 100644 realsense2_camera/include/gl_window.h create mode 100644 realsense2_camera/src/gl_gpu_processing.cpp diff --git a/.github/ISSUE_TEMPLATE.md b/.github/ISSUE_TEMPLATE.md new file mode 100644 index 0000000000..921b769f2e --- /dev/null +++ b/.github/ISSUE_TEMPLATE.md @@ -0,0 +1,26 @@ +* Before opening a new issue, we wanted to provide you with some useful suggestions (Click "Preview" above for a better view): + + * Consider checking our ROS RealSense Wrapper documentation [README](https://github.com/IntelRealSense/realsense-ros/blob/ros2-development/README.md). + * Have you looked in our [Discussions](https://github.com/IntelRealSense/realsense-ros/discussions)? + * Try [searching our GitHub Issues](https://github.com/IntelRealSense/realsense-ros/issues) (open and closed) for a similar issue. + +* All users are welcomed to report bugs, ask questions, suggest or request enhancements and generally feel free to open new issue, even if they haven't followed any of the suggestions above :) + +---------------------------------------------------------------------------------------------------- + +| Required Info | | +|---------------------------------|------------------------------------------------------------ | +| Camera Model | { D405 / D415 / D435 / D435i / D455 / D457 } | +| Firmware Version | (Open RealSense Viewer --> Click info) | +| Operating System & Version | { Win (10/11) / Linux (Ubuntu 18/20/22) / MacOS } | +| Kernel Version (Linux Only) | (e.g. 5.4) | +| Platform | PC/Raspberry Pi/ NVIDIA Jetson / etc.. | +| Librealsense SDK Version | { 2.. } | +| Language | {C/C#/labview/opencv/pcl/python/unity } | +| Segment | {Robot/Smartphone/VR/AR/others } | +| ROS Distro | {Iron/Humble/Rolling/etc.. } | +| RealSense ROS Wrapper Version | {4.51.1, 4.54.1, etc..} | + + +### Issue Description + diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index ecd144d630..93a0bcb4b0 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -13,6 +13,8 @@ on: # Allows you to run this workflow manually from the Actions tab workflow_dispatch: +permissions: read-all + # A workflow run is made up of one or more jobs that can run sequentially or in parallel # This workflow contains a single job called "build" @@ -35,10 +37,10 @@ jobs: steps: - name: Setup ROS2 Workspace - run: | + run: | mkdir -p ${{github.workspace}}/ros2/src - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 with: path: 'ros2/src/realsense-ros' diff --git a/.github/workflows/pre-release.yml b/.github/workflows/pre-release.yml index 4b54702bcc..66d2332deb 100644 --- a/.github/workflows/pre-release.yml +++ b/.github/workflows/pre-release.yml @@ -23,6 +23,7 @@ on: # Allows you to run this workflow manually from the Actions tab workflow_dispatch: +permissions: read-all jobs: build: @@ -44,6 +45,6 @@ jobs: BASEDIR: ${{ github.workspace }}/.work steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 - name: industrial_ci uses: ros-industrial/industrial_ci@master diff --git a/.github/workflows/static_analysis.yaml b/.github/workflows/static_analysis.yaml index 7672fd6956..26cb9bb20d 100644 --- a/.github/workflows/static_analysis.yaml +++ b/.github/workflows/static_analysis.yaml @@ -6,6 +6,8 @@ on: pull_request: branches: ['**'] +permissions: read-all + jobs: cppcheck: name: cppcheck diff --git a/README.md b/README.md index 5625929f09..db4eeac10f 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,4 @@ +

Intel® RealSense™

@@ -24,7 +25,8 @@ ## Table of contents * [ROS1 and ROS2 legacy](#ros1-and-ros2-legacy) - * [Installation](#installation) + * [Installation on Ubuntu](#installation-on-ubuntu) + * [Installation on Windows](#installation-on-windows) * [Usage](#usage) * [Starting the camera node](#start-the-camera-node) * [Camera name and namespace](#camera-name-and-camera-namespace) @@ -78,7 +80,7 @@ -# Installation +# Installation on Ubuntu
@@ -88,14 +90,14 @@ - #### Ubuntu 22.04: - [ROS2 Iron](https://docs.ros.org/en/iron/Installation/Ubuntu-Install-Debians.html) - [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) -
Step 2: Install latest Intel® RealSense™ SDK 2.0 - + **Please choose only one option from the 3 options below (in order to prevent multiple versions installation and workspace conflicts)** + - #### Option 1: Install librealsense2 debian package from Intel servers - Jetson users - use the [Jetson Installation Guide](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation_jetson.md) - Otherwise, install from [Linux Debian Installation Guide](https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md#installing-the-packages) @@ -161,6 +163,76 @@
+# Installation on Windows + **PLEASE PAY ATTENTION: RealSense ROS2 Wrapper is not meant to be supported on Windows by our team, since ROS2 and its packages are still not fully supported over Windows. We added these installation steps below in order to try and make it easier for users who already started working with ROS2 on Windows and want to take advantage of the capabilities of our RealSense cameras** + +
+ + Step 1: Install the ROS2 distribution + + +- #### Windows 10/11 + **Please choose only one option from the two options below (in order to prevent multiple versions installation and workspace conflicts)** + - Manual install from ROS2 formal documentation: + - [ROS2 Iron](https://docs.ros.org/en/iron/Installation/Windows-Install-Binary.html) + - [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Windows-Install-Binary.html) + - Microsoft IOT binary installation: + - https://ms-iot.github.io/ROSOnWindows/GettingStarted/SetupRos2.html + - Pay attention that the examples of install are for Foxy distro (which is not supported anymore by RealSense ROS2 Wrapper) + - Please replace the word "Foxy" with Humble or Iron, as you choose +
+ +
+ + Step 2: Download RealSense™ ROS2 Wrapper and RealSense™ SDK 2.0 source code from github: + + +- Download Intel® RealSense™ ROS2 Wrapper source code from [Intel® RealSense™ ROS2 Wrapper Releases](https://github.com/IntelRealSense/realsense-ros/releases) +- Download the corrosponding supported Intel® RealSense™ SDK 2.0 source code from the **"Supported RealSense SDK" section** of the specific release you chose fronm the link above +- Place the librealsense folder inside the realsense-ros folder, to make the librealsense package set beside realsense2_camera, realsense2_camera_msgs and realsense2_description packages +
+ +
+ + Step 3: Build + + +1. Before starting building of our packages, make sure you have OpenCV for Windows installed on your machine. If you choose the Microsoft IOT way to install it, it will be installed automatically. Later, when colcon build, you might need to expose this installation folder by setting CMAKE_PREFIX_PATH, PATH, or OpenCV_DIR environment variables +2. Run "x64 Native Tools Command Prompt for VS 2019" as administrator +3. Setup ROS2 Environment (Do this for every new terminal/cmd you open): + - If you choose the Microsoft IOT Binary option for installation + ``` + > C:\opt\ros\humble\x64\setup.bat + ``` + + - If you choose the ROS2 formal documentation: + ``` + > call C:\dev\ros2_iron\local_setup.bat + ``` +4. Change directory to realsense-ros folder + ```bash + > cd C:\ros2_ws\realsense-ros + ``` +5. Build librealsense2 package only + ```bash + > colcon build --packages-select librealsense2 --cmake-args -DBUILD_EXAMPLES=OFF -DBUILD_WITH_STATIC_CRT=OFF -DBUILD_GRAPHICAL_EXAMPLES=OFF + ``` + - User can add `--event-handlers console_direct+` parameter to see more debug outputs of the colcon build +6. Build the other packages + ```bash + > colcon build --packages-select realsense2_camera_msgs realsense2_description realsense2_camera + ``` + - User can add `--event-handlers console_direct+` parameter to see more debug outputs of the colcon build + +7. Setup environment with new installed packages (Do this for every new terminal/cmd you open): + ```bash + > call install\setup.bat + ``` +
+ +
+ + # Usage ## Start the camera node @@ -172,7 +244,7 @@ #### with ros2 launch: ros2 launch realsense2_camera rs_launch.py - ros2 launch realsense2_camera rs_launch.py depth_module.profile:=1280x720x30 pointcloud.enable:=true + ros2 launch realsense2_camera rs_launch.py depth_module.depth_profile:=1280x720x30 pointcloud.enable:=true
@@ -251,10 +323,10 @@ User can set the camera name and camera namespace, to distinguish between camera #### Parameters that can be modified during runtime: - All of the filters and sensors inner parameters. - Video Sensor Parameters: (```depth_module``` and ```rgb_camera```) - - They have, at least, the **profile** parameter. + - They have, at least, the **_profile** parameter. - The profile parameter is a string of the following format: \X\X\ (The dividing character can be X, x or ",". Spaces are ignored.) - - For example: ```depth_module.profile:=640x480x30 rgb_camera.profile:=1280x720x30``` - - Since infra, infra1, infra2 and depth are all streams of the depth_module, their width, height and fps are defined by the same param **depth_module.profile** + - For example: ```depth_module.depth_profile:=640x480x30 depth_module.infra_profile:=640x480x30 rgb_camera.color_profile:=1280x720x30``` + - Note: The param **depth_module.infra_profile** is common for all infra streams. i.e., infra 0, 1 & 2. - If the specified combination of parameters is not available by the device, the default or previously set configuration will be used. - Run ```ros2 param describe ``` to get the list of supported profiles. - Note: Should re-enable the stream for the change to take effect. @@ -281,6 +353,7 @@ User can set the camera name and camera namespace, to distinguish between camera - can be any of *infra, infra1, infra2, color, depth, gyro, accel*. - Available values are the following strings: `SYSTEM_DEFAULT`, `DEFAULT`, `PARAMETER_EVENTS`, `SERVICES_DEFAULT`, `PARAMETERS`, `SENSOR_DATA`. - For example: ```depth_qos:=SENSOR_DATA``` + - Pointcloud QoS is controlled with the `pointcloud.pointcloud_qos` parameter in the pointcloud filter, refer to the Post-Processing Filters section for details. - Reference: [ROS2 QoS profiles formal documentation](https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html#qos-profiles) - **Notice:** ****_info_qos** refers to both camera_info topics and metadata topics. - **tf_publish_rate**: @@ -305,6 +378,17 @@ User can set the camera name and camera namespace, to distinguish between camera - 1 -> **copy**: Every gyro message will be attached by the last accel message. - 2 -> **linear_interpolation**: Every gyro message will be attached by an accel message which is interpolated to gyro's timestamp. - Note: When the param *unite_imu_method* is dynamically updated, re-enable either gyro or accel stream for the change to take effect. +- **accelerate_gpu_with_glsl**: + - Boolean: GPU accelerated with GLSL for processing PointCloud and Colorizer filters. + - Note: + - To have smooth transition between the processing blocks when this parameter is updated dynamically, the node will: + - Stop the video sensors + - Do necessary GLSL configuration + - And then, start the video sensors + - To enable GPU acceleration, turn ON `BUILD_ACCELERATE_GPU_WITH_GLSL` during build: + ```bash + colcon build --cmake-args '-DBUILD_ACCELERATE_GPU_WITH_GLSL=ON' + ``` - **safety_camera.safety_mode**: - 0 -> **Run** mode - 1 -> **Standby** mode @@ -505,18 +589,32 @@ The following post processing filters are available: * The texture of the pointcloud can be modified using the `pointcloud.stream_filter` parameter.
* The depth FOV and the texture FOV are not similar. By default, pointcloud is limited to the section of depth containing the texture. You can have a full depth to pointcloud, coloring the regions beyond the texture with zeros, by setting `pointcloud.allow_no_texture_points` to true. * pointcloud is of an unordered format by default. This can be changed by setting `pointcloud.ordered_pc` to true. + * The QoS of the pointcloud topic is independent from depth and color streams and can be controlled with the `pointcloud.pointcloud_qos` parameter. + - The same set of QoS values are supported as other streams, refer to _qos in the Parameters section of this page. + - The launch file should include the parameter with initial QoS value, for example,`{'name': 'pointcloud.pointcloud_qos', 'default': 'SENSOR_DATA', 'description': 'pointcloud qos'}` + - The QoS value can also be overridden at launch with command option, for example, `pointcloud.pointcloud_qos:=SENSOR_DATA` + - At runtime, the QoS can be changed dynamically but require the filter re-enable for the change to take effect, for example, + ```bash + ros2 param set /camera/camera pointcloud.pointcloud_qos SENSOR_DATA + ros2 param set /camera/camera pointcloud.enable false + ros2 param set /camera/camera pointcloud.enable true + ``` - ```hdr_merge```: Allows depth image to be created by merging the information from 2 consecutive frames, taken with different exposure and gain values. - - `depth_module.hdr_enabled`: to enable/disable HDR - - The way to set exposure and gain values for each sequence in runtime is by first selecting the sequence id, using the `depth_module.sequence_id` parameter and then modifying the `depth_module.gain`, and `depth_module.exposure`. - - From FW versions 5.14.x.x and above, if HDR is enabled, the preset configs (like exposure, gain, etc.,) cannot be updated. - - Disable the HDR first using `depth_module.hdr_enabled` parameter and then, update the required presets. - - To view the effect on the infrared image for each sequence id use the `filter_by_sequence_id.sequence_id` parameter. - - To initialize these parameters in start time use the following parameters: - - `depth_module.exposure.1` - - `depth_module.gain.1` - - `depth_module.exposure.2` - - `depth_module.gain.2` - - For in-depth review of the subject please read the accompanying [white paper](https://dev.intelrealsense.com/docs/high-dynamic-range-with-stereoscopic-depth-cameras). + - `depth_module.hdr_enabled`: to enable/disable HDR. The way to set exposure and gain values for each sequence: + - during Runtime: + - is by first selecting the sequence id, using the `depth_module.sequence_id` parameter and then modifying the `depth_module.gain`, and `depth_module.exposure`. + - From FW versions 5.14.x.x and above, if HDR is enabled, the preset configs (like exposure, gain, etc.,) cannot be updated. + - Disable the HDR first using `depth_module.hdr_enabled` parameter and then, update the required presets. + - during Launch time of the node: + - is by setting below parameters + - `depth_module.exposure.1` + - `depth_module.gain.1` + - `depth_module.exposure.2` + - `depth_module.gain.2` + - Make sure to set `depth_module.hdr_enabled` to true, otherwise these parameters won't be considered. + - To view the effect on the infrared image for each sequence id use the `filter_by_sequence_id.sequence_id` parameter. + - For in-depth review of the subject please read the accompanying [white paper](https://dev.intelrealsense.com/docs/high-dynamic-range-with-stereoscopic-depth-cameras). + - **Note**: Auto exposure functionality is not supported when HDR is enabled. i.e., Auto exposure will be auto-disabled if HDR is enabled. - The following filters have detailed descriptions in : https://github.com/IntelRealSense/librealsense/blob/master/doc/post-processing-filters.md - ```disparity_filter``` - convert depth to disparity before applying other filters and back. diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index fb267f590d..c814afb572 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -114,8 +114,12 @@ find_package(nav_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) find_package(diagnostic_updater REQUIRED) +find_package(OpenCV REQUIRED COMPONENTS core) find_package(realsense2 2.55.0) +if (BUILD_ACCELERATE_GPU_WITH_GLSL) + find_package(realsense2-gl 2.55.0) +endif() if(NOT realsense2_FOUND) message(FATAL_ERROR "\n\n Intel RealSense SDK 2.0 is missing, please install it from https://github.com/IntelRealSense/librealsense/releases\n\n") endif() @@ -123,6 +127,8 @@ endif() #set(CMAKE_NO_SYSTEM_FROM_IMPORTED true) include_directories(include) +include_directories(${OpenCV_INCLUDE_DIRS}) # add OpenCV includes to the included dirs + set(node_plugins "") set(SOURCES @@ -140,6 +146,10 @@ set(SOURCES src/tfs.cpp ) +if (BUILD_ACCELERATE_GPU_WITH_GLSL) + list(APPEND SOURCES src/gl_gpu_processing.cpp) +endif() + if(NOT DEFINED ENV{ROS_DISTRO}) message(FATAL_ERROR "ROS_DISTRO is not defined." ) endif() @@ -170,6 +180,10 @@ if(${rclcpp_VERSION} VERSION_GREATER_EQUAL "17.0") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DRCLCPP_HAS_OnSetParametersCallbackType") endif() +if (BUILD_ACCELERATE_GPU_WITH_GLSL) + add_definitions(-DACCELERATE_GPU_WITH_GLSL) +endif() + set(INCLUDES include/constants.h include/realsense_node_factory.h @@ -183,6 +197,9 @@ set(INCLUDES include/profile_manager.h include/image_publisher.h) +if (BUILD_ACCELERATE_GPU_WITH_GLSL) + list(APPEND INCLUDES include/gl_window.h) +endif() if (BUILD_TOOLS) @@ -199,9 +216,17 @@ add_library(${PROJECT_NAME} SHARED ${SOURCES} ) +if (BUILD_ACCELERATE_GPU_WITH_GLSL) + set(link_libraries ${realsense2-gl_LIBRARY}) +else() + set(link_libraries ${realsense2_LIBRARY}) +endif() + +list(APPEND link_libraries ${OpenCV_LIBS}) # add OpenCV libs to link_libraries + target_link_libraries(${PROJECT_NAME} - ${realsense2_LIBRARY} - ) + ${link_libraries} +) set(dependencies cv_bridge @@ -213,11 +238,16 @@ set(dependencies sensor_msgs nav_msgs tf2 - realsense2 tf2_ros diagnostic_updater ) +if (BUILD_ACCELERATE_GPU_WITH_GLSL) + list(APPEND dependencies realsense2-gl) +else() + list(APPEND dependencies realsense2) +endif() + ament_target_dependencies(${PROJECT_NAME} ${dependencies} ) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 2a7500905c..55771f365b 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -50,6 +50,10 @@ #include #include +#if defined (ACCELERATE_GPU_WITH_GLSL) +#include +#endif + #include #include #include @@ -260,11 +264,19 @@ namespace realsense2_camera void setAvailableSensors(); void setCallbackFunctions(); void updateSensors(); + void startUpdatedSensors(); + void stopRequiredSensors(); void publishServices(); void startPublishers(const std::vector& profiles, const RosSensor& sensor); void startRGBDPublisherIfNeeded(); void stopPublishers(const std::vector& profiles); +#if defined (ACCELERATE_GPU_WITH_GLSL) + void initOpenGLProcessing(bool use_gpu_processing); + void shutdownOpenGLProcessing(); + void glfwPollEventCallback(); +#endif + rs2::device _dev; std::map _sensors; std::map> _sensors_callback; @@ -277,7 +289,6 @@ namespace realsense2_camera double _angular_velocity_cov; bool _hold_back_imu_for_frames; - std::map _stream_intrinsics; std::map _enable; bool _publish_tf; double _tf_publish_rate, _diagnostics_period; @@ -347,6 +358,12 @@ namespace realsense2_camera std::shared_ptr _diagnostics_updater; rs2::stream_profile _base_profile; +#if defined (ACCELERATE_GPU_WITH_GLSL) + GLwindow _app; + rclcpp::TimerBase::SharedPtr _timer; + bool _accelerate_gpu_with_glsl; + bool _is_accelerate_gpu_with_glsl_changed; +#endif };//end class } diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index a3c39e1844..45ef4377c8 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -60,7 +60,6 @@ namespace realsense2_camera const uint16_t RS460_PID = 0x0b03; // DS5U const uint16_t RS435_RGB_PID = 0x0b07; // AWGC const uint16_t RS435i_RGB_PID = 0x0B3A; // AWGC_MM - const uint16_t RS465_PID = 0x0b4d; // D465 const uint16_t RS416_RGB_PID = 0x0B52; // F416 RGB const uint16_t RS430i_PID = 0x0b4b; // D430i const uint16_t RS405_PID = 0x0B5B; // DS5U diff --git a/realsense2_camera/include/dynamic_params.h b/realsense2_camera/include/dynamic_params.h index b87caea5c0..d438ad937b 100644 --- a/realsense2_camera/include/dynamic_params.h +++ b/realsense2_camera/include/dynamic_params.h @@ -46,7 +46,10 @@ namespace realsense2_camera template void queueSetRosValue(const std::string& param_name, const T value); - + + template + T getParam(std::string param_name); + private: void monitor_update_functions(); diff --git a/realsense2_camera/include/gl_window.h b/realsense2_camera/include/gl_window.h new file mode 100644 index 0000000000..2bca9d4eb5 --- /dev/null +++ b/realsense2_camera/include/gl_window.h @@ -0,0 +1,84 @@ +// Copyright 2023 Intel Corporation. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include // Include RealSense Cross Platform API + +#if defined (ACCELERATE_GPU_WITH_GLSL) + +#define GL_SILENCE_DEPRECATION +#define GLFW_INCLUDE_GLU +#include +#include +#include + +#include // Include GPU-Processing API + + +#ifndef PI +#define PI 3.14159265358979323846 +#define PI_FL 3.141592f +#endif + + +class GLwindow +{ +public: + + GLwindow(int width, int height, const char* title) + : _width(width), _height(height) + { + glfwInit(); + glfwWindowHint(GLFW_VISIBLE, 0); + win = glfwCreateWindow(width, height, title, nullptr, nullptr); + if (!win) + throw std::runtime_error("Could not open OpenGL window, please check your graphic drivers or use the textual SDK tools"); + glfwMakeContextCurrent(win); + + glfwSetWindowUserPointer(win, this); + + } + + ~GLwindow() + { + glfwDestroyWindow(win); + glfwTerminate(); + } + + void close() + { + glfwSetWindowShouldClose(win, 1); + } + + float width() const { return float(_width); } + float height() const { return float(_height); } + + operator bool() + { + auto res = !glfwWindowShouldClose(win); + + glfwPollEvents(); + + return res; + } + + operator GLFWwindow* () { return win; } + +private: + GLFWwindow* win; + int _width, _height; +}; + +#endif diff --git a/realsense2_camera/include/profile_manager.h b/realsense2_camera/include/profile_manager.h index 3021e95f6c..a63cc7f3b0 100644 --- a/realsense2_camera/include/profile_manager.h +++ b/realsense2_camera/include/profile_manager.h @@ -68,22 +68,22 @@ namespace realsense2_camera VideoProfilesManager(std::shared_ptr parameters, const std::string& module_name, rclcpp::Logger logger, bool force_image_default_qos = false); bool isWantedProfile(const rs2::stream_profile& profile) override; void registerProfileParameters(std::vector all_profiles, std::function update_sensor_func) override; - int getHeight() {return _height;}; - int getWidth() {return _width;}; - int getFPS() {return _fps;}; + int getHeight(rs2_stream stream_type) {return _height[stream_type];}; + int getWidth(rs2_stream stream_type) {return _width[stream_type];}; + int getFPS(rs2_stream stream_type) {return _fps[stream_type];}; private: bool isSameProfileValues(const rs2::stream_profile& profile, const int width, const int height, const int fps, const rs2_format format); + rs2::stream_profile validateAndGetSuitableProfile(rs2_stream stream_type, rs2::stream_profile given_profile); void registerVideoSensorProfileFormat(stream_index_pair sip); void registerVideoSensorParams(std::set sips); - std::string get_profiles_descriptions(); + std::string get_profiles_descriptions(rs2_stream stream_type); std::string getProfileFormatsDescriptions(stream_index_pair sip); private: std::string _module_name; std::map _formats; - int _fps; - int _width, _height; + std::map _fps, _width, _height; bool _is_profile_exist; bool _force_image_default_qos; }; diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index d387dc7e80..9a52aa45a8 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -29,19 +29,21 @@ {'name': 'config_file', 'default': "''", 'description': 'yaml config file'}, {'name': 'json_file_path', 'default': "''", 'description': 'allows advanced configuration'}, {'name': 'initial_reset', 'default': 'false', 'description': "''"}, + {'name': 'accelerate_gpu_with_glsl', 'default': "false", 'description': 'enable GPU acceleration with GLSL'}, {'name': 'rosbag_filename', 'default': "''", 'description': 'A realsense bagfile to run from as a device'}, {'name': 'log_level', 'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'}, {'name': 'output', 'default': 'screen', 'description': 'pipe node output [screen|log]'}, {'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'}, - {'name': 'rgb_camera.profile', 'default': '0,0,0', 'description': 'color image width'}, + {'name': 'rgb_camera.color_profile', 'default': '0,0,0', 'description': 'color stream profile'}, {'name': 'rgb_camera.color_format', 'default': 'RGB8', 'description': 'color stream format'}, {'name': 'rgb_camera.enable_auto_exposure', 'default': 'true', 'description': 'enable/disable auto exposure for color image'}, {'name': 'enable_depth', 'default': 'true', 'description': 'enable depth stream'}, {'name': 'enable_infra', 'default': 'false', 'description': 'enable infra0 stream'}, {'name': 'enable_infra1', 'default': 'false', 'description': 'enable infra1 stream'}, {'name': 'enable_infra2', 'default': 'false', 'description': 'enable infra2 stream'}, - {'name': 'depth_module.profile', 'default': '0,0,0', 'description': 'depth module profile'}, + {'name': 'depth_module.depth_profile', 'default': '0,0,0', 'description': 'depth stream profile'}, {'name': 'depth_module.depth_format', 'default': 'Z16', 'description': 'depth stream format'}, + {'name': 'depth_module.infra_profile', 'default': '0,0,0', 'description': 'infra streams (0/1/2) profile'}, {'name': 'depth_module.infra_format', 'default': 'RGB8', 'description': 'infra0 stream format'}, {'name': 'depth_module.infra1_format', 'default': 'Y8', 'description': 'infra1 stream format'}, {'name': 'depth_module.infra2_format', 'default': 'Y8', 'description': 'infra2 stream format'}, diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 82599a5f82..2e3c57ddab 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -36,7 +36,11 @@ SyncedImuPublisher::SyncedImuPublisher(rclcpp::Publisher: SyncedImuPublisher::~SyncedImuPublisher() { - PublishPendingMessages(); + try + { + PublishPendingMessages(); + } + catch(...){} // Not allowed to throw from Dtor } void SyncedImuPublisher::Publish(sensor_msgs::msg::Imu imu_msg) @@ -117,6 +121,11 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _imu_sync_method(imu_sync_method::NONE), _is_profile_changed(false), _is_align_depth_changed(false) +#if defined (ACCELERATE_GPU_WITH_GLSL) + ,_app(1280, 720, "RS_GLFW_Window"), + _accelerate_gpu_with_glsl(false), + _is_accelerate_gpu_with_glsl_changed(false) +#endif { if ( use_intra_process ) { @@ -146,10 +155,14 @@ BaseRealSenseNode::~BaseRealSenseNode() _monitoring_pc->join(); } clearParameters(); - for(auto&& sensor : _available_ros_sensors) + try { - sensor->stop(); + for(auto&& sensor : _available_ros_sensors) + { + sensor->stop(); + } } + catch(...){} // Not allowed to throw from Dtor } void BaseRealSenseNode::hardwareResetRequest() @@ -231,14 +244,22 @@ void BaseRealSenseNode::setupFilters() _cv_mpc.notify_one(); }; - _colorizer_filter = std::make_shared(std::make_shared(), _parameters, _logger); - _filters.push_back(_colorizer_filter); - +#if defined (ACCELERATE_GPU_WITH_GLSL) + _colorizer_filter = std::make_shared(std::make_shared(), _parameters, _logger); + _pc_filter = std::make_shared(std::make_shared(), _node, _parameters, _logger); +#else + _colorizer_filter = std::make_shared(std::make_shared(), _parameters, _logger); _pc_filter = std::make_shared(std::make_shared(), _node, _parameters, _logger); +#endif + + // Apply PointCloud filter before applying Align-depth as it requires original depth image not aligned-depth image. _filters.push_back(_pc_filter); _align_depth_filter = std::make_shared(std::make_shared(RS2_STREAM_COLOR), update_align_depth_func, _parameters, _logger); _filters.push_back(_align_depth_filter); + + // Apply Colorizer filter after applying Align-Depth to get colorized aligned depth image. + _filters.push_back(_colorizer_filter); } cv::Mat& BaseRealSenseNode::fix_depth_scale(const cv::Mat& from_image, cv::Mat& to_image) @@ -661,7 +682,11 @@ void BaseRealSenseNode::multiple_message_callback(rs2::frame frame, imu_sync_met bool BaseRealSenseNode::setBaseTime(double frame_time, rs2_timestamp_domain time_domain) { - ROS_WARN_ONCE(time_domain == RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME ? "Frame metadata isn't available! (frame_timestamp_domain = RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME)" : ""); + if (time_domain == RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME) + { + ROS_WARN_ONCE("Frame metadata isn't available! (frame_timestamp_domain = RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME)"); + } + if (time_domain == RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK) { ROS_WARN("frame's time domain is HARDWARE_CLOCK. Timestamps may reset periodically."); @@ -726,8 +751,19 @@ void BaseRealSenseNode::updateProfilesStreamCalibData(const std::vector + T Parameters::getParam(std::string param_name) + { + return _node.get_parameter(param_name).get_value(); + } + template void Parameters::setParamT(std::string param_name, bool& param, std::function func, rcl_interfaces::msg::ParameterDescriptor descriptor); template void Parameters::setParamT(std::string param_name, int& param, std::function func, rcl_interfaces::msg::ParameterDescriptor descriptor); template void Parameters::setParamT(std::string param_name, double& param, std::function func, rcl_interfaces::msg::ParameterDescriptor descriptor); @@ -284,4 +290,6 @@ namespace realsense2_camera template void Parameters::queueSetRosValue(const std::string& param_name, const int value); template int Parameters::readAndDeleteParam(std::string param_name, const int& initial_value); + + template bool Parameters::getParam(std::string param_name); } diff --git a/realsense2_camera/src/gl_gpu_processing.cpp b/realsense2_camera/src/gl_gpu_processing.cpp new file mode 100644 index 0000000000..d6b8f883e2 --- /dev/null +++ b/realsense2_camera/src/gl_gpu_processing.cpp @@ -0,0 +1,47 @@ +// Copyright 2023 Intel Corporation. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../include/base_realsense_node.h" +#include + +using namespace realsense2_camera; +using namespace std::chrono_literals; + +#if defined (ACCELERATE_GPU_WITH_GLSL) + +void BaseRealSenseNode::initOpenGLProcessing(bool use_gpu_processing) +{ + // Once we have a window, initialize GL module + // Pass our window to enable sharing of textures between processed frames and the window + // The "use_gpu_processing" is going to control if we will use CPU or GPU for data processing + rs2::gl::init_processing(_app, use_gpu_processing); + if (use_gpu_processing) + rs2::gl::init_rendering(); + + _timer = _node.create_wall_timer(1000ms, std::bind(&BaseRealSenseNode::glfwPollEventCallback, this)); +} + +void BaseRealSenseNode::glfwPollEventCallback() +{ + // Must poll the GLFW events perodically, else window will hang or crash + glfwPollEvents(); +} + +void BaseRealSenseNode::shutdownOpenGLProcessing() +{ + rs2::gl::shutdown_rendering(); + rs2::gl::shutdown_processing(); +} + +#endif diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index 72523cb801..dda0b6133e 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -82,6 +82,24 @@ void BaseRealSenseNode::getParameters() _base_frame_id = _parameters->setParam(param_name, DEFAULT_BASE_FRAME_ID); _base_frame_id = (static_cast(std::ostringstream() << _camera_name << "_" << _base_frame_id)).str(); _parameters_names.push_back(param_name); + +#if defined (ACCELERATE_GPU_WITH_GLSL) + param_name = std::string("accelerate_gpu_with_glsl"); + _parameters->setParam(param_name, false, + [this](const rclcpp::Parameter& parameter) + { + bool temp_value = parameter.get_value(); + if (_accelerate_gpu_with_glsl != temp_value) + { + _accelerate_gpu_with_glsl = temp_value; + std::lock_guard lock_guard(_profile_changes_mutex); + _is_accelerate_gpu_with_glsl_changed = true; + } + _cv_mpc.notify_one(); + }); + _parameters_names.push_back(param_name); +#endif + } void BaseRealSenseNode::setDynamicParams() diff --git a/realsense2_camera/src/profile_manager.cpp b/realsense2_camera/src/profile_manager.cpp index 95dadbbad6..bbe8989532 100644 --- a/realsense2_camera/src/profile_manager.cpp +++ b/realsense2_camera/src/profile_manager.cpp @@ -39,7 +39,9 @@ std::string applyTemplateName(std::string template_name, stream_index_pair sip) const std::string stream_name(create_graph_resource_name(STREAM_NAME(sip))); char* param_name = new char[template_name.size() + stream_name.size()]; sprintf(param_name, template_name.c_str(), stream_name.c_str()); - return std::string(param_name); + std::string full_name(param_name); + delete [] param_name; + return full_name; } void ProfilesManager::registerSensorQOSParam(std::string template_name, @@ -139,6 +141,40 @@ std::map ProfilesManager::getDefaultProf return sip_default_profiles; } +rs2::stream_profile VideoProfilesManager::validateAndGetSuitableProfile(rs2_stream stream_type, rs2::stream_profile given_profile) +{ + rs2::stream_profile suitable_profile = given_profile; + bool is_given_profile_suitable = false; + + for (auto temp_profile : _all_profiles) + { + if (temp_profile.stream_type() == stream_type) + { + auto video_profile = given_profile.as(); + + if (isSameProfileValues(temp_profile, video_profile.width(), video_profile.height(), video_profile.fps(), RS2_FORMAT_ANY)) + { + is_given_profile_suitable = true; + break; + } + } + } + + // If given profile is not suitable, choose the first available profile for the given stream. + if (!is_given_profile_suitable) + { + for (auto temp_profile : _all_profiles) + { + if (temp_profile.stream_type() == stream_type) + { + suitable_profile = temp_profile; + } + } + } + + return suitable_profile; +} + void ProfilesManager::addWantedProfiles(std::vector& wanted_profiles) { std::map found_sips; @@ -241,7 +277,7 @@ bool VideoProfilesManager::isSameProfileValues(const rs2::stream_profile& profil bool VideoProfilesManager::isWantedProfile(const rs2::stream_profile& profile) { stream_index_pair sip = {profile.stream_type(), profile.stream_index()}; - return isSameProfileValues(profile, _width, _height, _fps, _formats[sip]); + return isSameProfileValues(profile, _width[sip.first], _height[sip.first], _fps[sip.first], _formats[sip]); } void VideoProfilesManager::registerProfileParameters(std::vector all_profiles, std::function update_sensor_func) @@ -270,15 +306,18 @@ void VideoProfilesManager::registerProfileParameters(std::vector } } -std::string VideoProfilesManager::get_profiles_descriptions() +std::string VideoProfilesManager::get_profiles_descriptions(rs2_stream stream_type) { std::set profiles_str; for (auto& profile : _all_profiles) { - auto video_profile = profile.as(); - std::stringstream crnt_profile_str; - crnt_profile_str << video_profile.width() << "x" << video_profile.height() << "x" << video_profile.fps(); - profiles_str.insert(crnt_profile_str.str()); + if (stream_type == profile.stream_type()) + { + auto video_profile = profile.as(); + std::stringstream crnt_profile_str; + crnt_profile_str << video_profile.width() << "x" << video_profile.height() << "x" << video_profile.fps(); + profiles_str.insert(crnt_profile_str.str()); + } } std::stringstream descriptors_strm; for (auto& profile_str : profiles_str) @@ -333,25 +372,52 @@ void VideoProfilesManager::registerVideoSensorProfileFormat(stream_index_pair si void VideoProfilesManager::registerVideoSensorParams(std::set sips) { - // Set default values: - std::map sip_default_profiles = getDefaultProfiles(); - - rs2::stream_profile default_profile = sip_default_profiles.begin()->second; + std::set available_streams; - if (sip_default_profiles.find(DEPTH) != sip_default_profiles.end()) - { - default_profile = sip_default_profiles[DEPTH]; - } - else if (sip_default_profiles.find(COLOR) != sip_default_profiles.end()) + for (auto sip : sips) { - default_profile = sip_default_profiles[COLOR]; + available_streams.insert(sip.first); } - auto video_profile = default_profile.as(); + // Set default values: + std::map sip_default_profiles = getDefaultProfiles(); - _width = video_profile.width(); - _height = video_profile.height(); - _fps = video_profile.fps(); + for (auto stream_type : available_streams) + { + rs2::stream_profile default_profile = sip_default_profiles.begin()->second; + switch (stream_type) + { + case RS2_STREAM_COLOR: + if (sip_default_profiles.find(COLOR) != sip_default_profiles.end()) + { + default_profile = sip_default_profiles[COLOR]; + } + break; + case RS2_STREAM_DEPTH: + if (sip_default_profiles.find(DEPTH) != sip_default_profiles.end()) + { + default_profile = sip_default_profiles[DEPTH]; + } + break; + case RS2_STREAM_INFRARED: + if (sip_default_profiles.find(INFRA1) != sip_default_profiles.end()) + { + default_profile = sip_default_profiles[INFRA1]; + } + else if (sip_default_profiles.find(DEPTH) != sip_default_profiles.end()) + { + default_profile = validateAndGetSuitableProfile(stream_type, sip_default_profiles[DEPTH]); + } + break; + default: + default_profile = validateAndGetSuitableProfile(stream_type, sip_default_profiles.begin()->second); + } + + auto video_profile = default_profile.as(); + _width[stream_type] = video_profile.width(); + _height[stream_type] = video_profile.height(); + _fps[stream_type] = video_profile.fps(); + } // Set the stream formats for (auto sip : sips) @@ -364,72 +430,76 @@ void VideoProfilesManager::registerVideoSensorParams(std::set { stream_index_pair sip = sip_default_profile.first; - default_profile = sip_default_profile.second; - video_profile = default_profile.as(); + auto default_profile = sip_default_profile.second; + auto video_profile = default_profile.as(); - if (isSameProfileValues(default_profile, _width, _height, _fps, video_profile.format())) + if (isSameProfileValues(default_profile, _width[sip.first], _height[sip.first], _fps[sip.first], video_profile.format())) { _formats[sip] = video_profile.format(); } } - // Register ROS parameter: - std::string param_name(_module_name + ".profile"); - rcl_interfaces::msg::ParameterDescriptor crnt_descriptor; - crnt_descriptor.description = "Available options are:\n" + get_profiles_descriptions(); - std::stringstream crnt_profile_str; - crnt_profile_str << _width << "x" << _height << "x" << _fps; - _params.getParameters()->setParam(param_name, crnt_profile_str.str(), [this](const rclcpp::Parameter& parameter) - { - std::regex self_regex("\\s*([0-9]+)\\s*[xX,]\\s*([0-9]+)\\s*[xX,]\\s*([0-9]+)\\s*", std::regex_constants::ECMAScript); - std::smatch match; - std::string profile_str(parameter.get_value()); - bool found = std::regex_match(profile_str, match, self_regex); - bool request_default(false); - if (found) + for (auto stream_type : available_streams) + { + // Register ROS parameter: + std::stringstream param_name_str; + param_name_str << _module_name << "." << create_graph_resource_name(ros_stream_to_string(stream_type)) << "_profile"; + rcl_interfaces::msg::ParameterDescriptor crnt_descriptor; + crnt_descriptor.description = "Available options are:\n" + get_profiles_descriptions(stream_type); + std::stringstream crnt_profile_str; + crnt_profile_str << _width[stream_type] << "x" << _height[stream_type] << "x" << _fps[stream_type]; + _params.getParameters()->setParam(param_name_str.str(), crnt_profile_str.str(), [this, stream_type](const rclcpp::Parameter& parameter) { - int temp_width(std::stoi(match[1])), temp_height(std::stoi(match[2])), temp_fps(std::stoi(match[3])); - if (temp_width <= 0 || temp_height <= 0 || temp_fps <= 0) - { - found = false; - request_default = true; - } - else + std::regex self_regex("\\s*([0-9]+)\\s*[xX,]\\s*([0-9]+)\\s*[xX,]\\s*([0-9]+)\\s*", std::regex_constants::ECMAScript); + std::smatch match; + std::string profile_str(parameter.get_value()); + bool found = std::regex_match(profile_str, match, self_regex); + bool request_default(false); + if (found) { - for (const auto& profile : _all_profiles) + int temp_width(std::stoi(match[1])), temp_height(std::stoi(match[2])), temp_fps(std::stoi(match[3])); + if (temp_width <= 0 || temp_height <= 0 || temp_fps <= 0) { found = false; - if (isSameProfileValues(profile, temp_width, temp_height, temp_fps, RS2_FORMAT_ANY)) + request_default = true; + } + else + { + for (const auto& profile : _all_profiles) { - _width = temp_width; - _height = temp_height; - _fps = temp_fps; - found = true; - ROS_WARN_STREAM("re-enable the stream for the change to take effect."); - break; + found = false; + if (isSameProfileValues(profile, temp_width, temp_height, temp_fps, RS2_FORMAT_ANY)) + { + _width[stream_type] = temp_width; + _height[stream_type] = temp_height; + _fps[stream_type] = temp_fps; + found = true; + ROS_WARN_STREAM("re-enable the stream for the change to take effect."); + break; + } } } } - } - if (!found) - { - std::stringstream crnt_profile_str; - crnt_profile_str << _width << "x" << _height << "x" << _fps; - if (request_default) - { - ROS_INFO_STREAM("Set ROS param " << parameter.get_name() << " to default: " << crnt_profile_str.str()); - } - else + if (!found) { - ROS_ERROR_STREAM("Given value, " << parameter.get_value() << " is invalid. " - << "Run 'ros2 param describe " << parameter.get_name() - << "' to get the list of supported profiles. " - << "Setting ROS param back to: " << crnt_profile_str.str()); + std::stringstream crnt_profile_str; + crnt_profile_str << _width[stream_type] << "x" << _height[stream_type] << "x" << _fps[stream_type]; + if (request_default) + { + ROS_INFO_STREAM("Set ROS param " << parameter.get_name() << " to default: " << crnt_profile_str.str()); + } + else + { + ROS_ERROR_STREAM("Given value, " << parameter.get_value() << " is invalid. " + << "Run 'ros2 param describe " << parameter.get_name() + << "' to get the list of supported profiles. " + << "Setting ROS param back to: " << crnt_profile_str.str()); + } + _params.getParameters()->queueSetRosValue(parameter.get_name(), crnt_profile_str.str()); } - _params.getParameters()->queueSetRosValue(parameter.get_name(), crnt_profile_str.str()); - } - }, crnt_descriptor); - _parameters_names.push_back(param_name); + }, crnt_descriptor); + _parameters_names.push_back(param_name_str.str()); + } for (auto sip : sips) { diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index b533b8649e..9e2496704a 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -373,7 +373,6 @@ void RealSenseNodeFactory::startDevice() case RS435i_RGB_PID: case RS455_PID: case RS457_PID: - case RS465_PID: case RS_USB2_PID: case RS_D585_PID: case RS_D585S_PID: diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index dfd5daed99..0c939f0f2d 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -78,8 +78,12 @@ RosSensor::RosSensor(rs2::sensor sensor, RosSensor::~RosSensor() { - clearParameters(); - stop(); + try + { + clearParameters(); + stop(); + } + catch(...){} // Not allowed to throw from Dtor } void RosSensor::setParameters(bool is_rosbag_file) @@ -110,8 +114,35 @@ void RosSensor::UpdateSequenceIdCallback() if (!supports(RS2_OPTION_SEQUENCE_ID)) return; + std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME))); + std::string param_name = module_name + "." + create_graph_resource_name(rs2_option_to_string(RS2_OPTION_ENABLE_AUTO_EXPOSURE)); + + bool user_set_enable_ae_value = _params.getParameters()->getParam(param_name); bool is_hdr_enabled = static_cast(get_option(RS2_OPTION_HDR_ENABLED)); + if (is_hdr_enabled && user_set_enable_ae_value) + { + bool is_ae_enabled = static_cast(get_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE)); + + // If AE got auto-disabled, update the Enable_Auto_Exposure ROS paramerter as well accordingly. + if (!is_ae_enabled) + { + ROS_WARN_STREAM("Auto Exposure functionality is not supported when HDR is enabled. " << + "So, disabling the '" << param_name << "' param."); + + try + { + std::vector > funcs; + funcs.push_back([this](){set_sensor_parameter_to_ros(RS2_OPTION_ENABLE_AUTO_EXPOSURE);}); + _params.getParameters()->pushUpdateFunctions(funcs); + } + catch(const std::exception& e) + { + ROS_WARN_STREAM("Failed to set parameter:" << param_name << " : " << e.what()); + } + } + } + // Deleter to revert back the RS2_OPTION_HDR_ENABLED value at the end. auto deleter_to_revert_hdr = std::unique_ptr>(&is_hdr_enabled, [&](bool* enable_back_hdr) { @@ -121,36 +152,35 @@ void RosSensor::UpdateSequenceIdCallback() } }); - // From FW version 5.14.x.x, if HDR is enabled, updating UVC controls like exposure, gain , etc are restricted. - // So, disable it before updating. if (is_hdr_enabled) { + // From FW version 5.14.x.x, if HDR is enabled, updating UVC controls like exposure, gain , etc are restricted. + // So, disable it before updating. It will be reverted back by the deleter 'deleter_to_revert_hdr'. set_option(RS2_OPTION_HDR_ENABLED, false); - } - int original_seq_id = static_cast(get_option(RS2_OPTION_SEQUENCE_ID)); // To Set back to default. - std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME))); - - // Read initialization parameters and set to sensor: - std::vector options{RS2_OPTION_EXPOSURE, RS2_OPTION_GAIN}; - unsigned int seq_size = get_option(RS2_OPTION_SEQUENCE_SIZE); - for (unsigned int seq_id = 1; seq_id <= seq_size; seq_id++ ) - { - set_option(RS2_OPTION_SEQUENCE_ID, seq_id); - for (rs2_option& option : options) + int original_seq_id = static_cast(get_option(RS2_OPTION_SEQUENCE_ID)); // To Set back to default. + + // Read initialization parameters and set to sensor: + std::vector options{RS2_OPTION_EXPOSURE, RS2_OPTION_GAIN}; + unsigned int seq_size = get_option(RS2_OPTION_SEQUENCE_SIZE); + for (unsigned int seq_id = 1; seq_id <= seq_size; seq_id++ ) { - std::stringstream param_name_str; - param_name_str << module_name << "." << create_graph_resource_name(rs2_option_to_string(option)) << "." << seq_id; - int option_value = get_option(option); - int user_set_option_value = _params.getParameters()->readAndDeleteParam(param_name_str.str(), option_value); - if (option_value != user_set_option_value) + set_option(RS2_OPTION_SEQUENCE_ID, seq_id); + for (rs2_option& option : options) { - ROS_INFO_STREAM("Set " << rs2_option_to_string(option) << "." << seq_id << " to " << user_set_option_value); - set_option(option, user_set_option_value); + std::stringstream param_name_str; + param_name_str << module_name << "." << create_graph_resource_name(rs2_option_to_string(option)) << "." << seq_id; + int option_value = get_option(option); + int user_set_option_value = _params.getParameters()->readAndDeleteParam(param_name_str.str(), option_value); + if (option_value != user_set_option_value) + { + ROS_INFO_STREAM("Set " << rs2_option_to_string(option) << "." << seq_id << " to " << user_set_option_value); + set_option(option, user_set_option_value); + } } } + set_option(RS2_OPTION_SEQUENCE_ID, original_seq_id); // Set back to default. } - set_option(RS2_OPTION_SEQUENCE_ID, original_seq_id); // Set back to default. // Set callback to update ros parameters to gain and exposure matching the selected sequence_id: const std::string option_name(module_name + "." + create_graph_resource_name(rs2_option_to_string(RS2_OPTION_SEQUENCE_ID))); @@ -379,8 +409,18 @@ void RosSensor::set_sensor_auto_exposure_roi() { try { - int width = std::dynamic_pointer_cast(_profile_managers[0])->getWidth(); - int height = std::dynamic_pointer_cast(_profile_managers[0])->getHeight(); + rs2_stream stream_type; + if (this->rs2::sensor::is()) + { + stream_type = RS2_STREAM_DEPTH; + } + else if (this->rs2::sensor::is()) + { + stream_type = RS2_STREAM_COLOR; + } + + int width = std::dynamic_pointer_cast(_profile_managers[0])->getWidth(stream_type); + int height = std::dynamic_pointer_cast(_profile_managers[0])->getHeight(stream_type); bool update_roi_range(false); if (_auto_exposure_roi.max_x > width) @@ -411,8 +451,18 @@ void RosSensor::registerAutoExposureROIOptions() if (this->rs2::sensor::is()) { - int width = std::dynamic_pointer_cast(_profile_managers[0])->getWidth(); - int height = std::dynamic_pointer_cast(_profile_managers[0])->getHeight(); + rs2_stream stream_type; + if (this->rs2::sensor::is()) + { + stream_type = RS2_STREAM_DEPTH; + } + else if (this->rs2::sensor::is()) + { + stream_type = RS2_STREAM_COLOR; + } + + int width = std::dynamic_pointer_cast(_profile_managers[0])->getWidth(stream_type); + int height = std::dynamic_pointer_cast(_profile_managers[0])->getHeight(stream_type); int max_x(width-1); int max_y(height-1); diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 591051a41a..d165596c66 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -22,6 +22,10 @@ using namespace rs2; void BaseRealSenseNode::setup() { +#if defined (ACCELERATE_GPU_WITH_GLSL) + initOpenGLProcessing(_accelerate_gpu_with_glsl); + _is_accelerate_gpu_with_glsl_changed = false; +#endif setDynamicParams(); startDiagnosticsUpdater(); setAvailableSensors(); @@ -39,8 +43,20 @@ void BaseRealSenseNode::monitoringProfileChanges() std::function func = [this, time_interval](){ std::unique_lock lock(_profile_changes_mutex); while(_is_running) { - _cv_mpc.wait_for(lock, std::chrono::milliseconds(time_interval), [&]{return (!_is_running || _is_profile_changed || _is_align_depth_changed);}); - if (_is_running && (_is_profile_changed || _is_align_depth_changed)) + _cv_mpc.wait_for(lock, std::chrono::milliseconds(time_interval), + [&]{return (!_is_running || _is_profile_changed + || _is_align_depth_changed + #if defined (ACCELERATE_GPU_WITH_GLSL) + || _is_accelerate_gpu_with_glsl_changed + #endif + );}); + + if (_is_running && (_is_profile_changed + || _is_align_depth_changed + #if defined (ACCELERATE_GPU_WITH_GLSL) + || _is_accelerate_gpu_with_glsl_changed + #endif + )) { ROS_DEBUG("Profile has changed"); try @@ -53,6 +69,10 @@ void BaseRealSenseNode::monitoringProfileChanges() } _is_profile_changed = false; _is_align_depth_changed = false; + + #if defined (ACCELERATE_GPU_WITH_GLSL) + _is_accelerate_gpu_with_glsl_changed = false; + #endif } } }; @@ -394,6 +414,34 @@ void BaseRealSenseNode::startRGBDPublisherIfNeeded() void BaseRealSenseNode::updateSensors() { std::lock_guard lock_guard(_update_sensor_mutex); + try{ + stopRequiredSensors(); + + #if defined (ACCELERATE_GPU_WITH_GLSL) + if (_is_accelerate_gpu_with_glsl_changed) + { + shutdownOpenGLProcessing(); + + initOpenGLProcessing(_accelerate_gpu_with_glsl); + } + #endif + + startUpdatedSensors(); + } + catch(const std::exception& ex) + { + ROS_ERROR_STREAM(__FILE__ << ":" << __LINE__ << ":" << "An exception has been thrown: " << ex.what()); + throw; + } + catch(...) + { + ROS_ERROR_STREAM(__FILE__ << ":" << __LINE__ << ":" << "Unknown exception has occured!"); + throw; + } +} + +void BaseRealSenseNode::stopRequiredSensors() +{ try{ for(auto&& sensor : _available_ros_sensors) { @@ -404,21 +452,61 @@ void BaseRealSenseNode::updateSensors() bool is_profile_changed(sensor->getUpdatedProfiles(wanted_profiles)); bool is_video_sensor = (sensor->is() || sensor->is()); - // do all updates if profile has been changed, or if the align depth filter status has been changed + // do all updates if profile has been changed, or if the align depth filter or gpu acceleration status has been changed // and we are on a video sensor. TODO: explore better options to monitor and update changes // without resetting the whole sensors and topics. - if (is_profile_changed || (_is_align_depth_changed && is_video_sensor)) + if (is_profile_changed || (is_video_sensor && (_is_align_depth_changed + #if defined (ACCELERATE_GPU_WITH_GLSL) + || _is_accelerate_gpu_with_glsl_changed + #endif + ))) { std::vector active_profiles = sensor->get_active_streams(); - if(is_profile_changed) + if (is_profile_changed + #if defined (ACCELERATE_GPU_WITH_GLSL) + || _is_accelerate_gpu_with_glsl_changed + #endif + ) { - // Start/stop sensors only if profile was changed + // Start/stop sensors only if profile or gpu acceleration status was changed // No need to start/stop sensors if align_depth was changed ROS_INFO_STREAM("Stopping Sensor: " << module_name); sensor->stop(); } stopPublishers(active_profiles); + } + } + } + catch(const std::exception& ex) + { + ROS_ERROR_STREAM(__FILE__ << ":" << __LINE__ << ":" << "An exception has been thrown: " << ex.what()); + throw; + } + catch(...) + { + ROS_ERROR_STREAM(__FILE__ << ":" << __LINE__ << ":" << "Unknown exception has occured!"); + throw; + } +} +void BaseRealSenseNode::startUpdatedSensors() +{ + try{ + for(auto&& sensor : _available_ros_sensors) + { + std::string module_name(rs2_to_ros(sensor->get_info(RS2_CAMERA_INFO_NAME))); + // if active_profiles != wanted_profiles: stop sensor. + std::vector wanted_profiles; + + bool is_profile_changed(sensor->getUpdatedProfiles(wanted_profiles)); + bool is_video_sensor = (sensor->is() || sensor->is()); + + if (is_profile_changed || (is_video_sensor && (_is_align_depth_changed + #if defined (ACCELERATE_GPU_WITH_GLSL) + || _is_accelerate_gpu_with_glsl_changed + #endif + ))) + { if (!wanted_profiles.empty()) { startPublishers(wanted_profiles, *sensor); @@ -432,9 +520,13 @@ void BaseRealSenseNode::updateSensors() } } - if(is_profile_changed) + if (is_profile_changed + #if defined (ACCELERATE_GPU_WITH_GLSL) + || _is_accelerate_gpu_with_glsl_changed + #endif + ) { - // Start/stop sensors only if profile was changed + // Start/stop sensors only if profile or gpu acceleration was changed // No need to start/stop sensors if align_depth was changed ROS_INFO_STREAM("Starting Sensor: " << module_name); sensor->start(wanted_profiles); diff --git a/realsense2_camera/test/README.md b/realsense2_camera/test/README.md index ea3a043a92..15b882a2f8 100644 --- a/realsense2_camera/test/README.md +++ b/realsense2_camera/test/README.md @@ -126,6 +126,15 @@ The xml files mentioned by the command can be directly opened also. ### Running pytests directly +Note : +1. All the commands for test execution has to be executed from realsense-ros folder. For example: If the ROS2 workspace was created based on Step 3 [Option2] of [this](https://github.com/IntelRealSense/realsense-ros/blob/ros2-development/README.md#installation). +Then, the path to execute the tests would be ~/ros2_ws/src/realsense-ros. + + cd ~/ros2_ws/src/realsense-ros + +2. Please setup below required environment variables. If not, Please prefix them for every test execution. + + PYTHONPATH=$PYTHONPATH:$PWD/realsense2_camera/test/utils:$PWD/realsense2_camera//launch:$PWD/realsense2_camera//scripts User can run all the tests in a pytest file directly using the below command: diff --git a/realsense2_camera/test/live_camera/test_camera_aligned_tests.py b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py index 0c17e76e47..f09b8a0de6 100644 --- a/realsense2_camera/test/live_camera/test_camera_aligned_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py @@ -79,6 +79,7 @@ def test_camera_align_depth_color(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: print("Device not found? : " + params['device_type']) + assert False return themes = [ {'topic':get_node_heirarchy(params)+'/color/image_raw', @@ -144,9 +145,9 @@ def test_camera_align_depth_color(self,launch_descr_with_parameters): 'rgb_camera.profile':'640x480x30', 'align_depth.enable':'true' } -test_params_all_profiles_d435 = { - 'camera_name': 'D435', - 'device_type': 'D435', +test_params_all_profiles_d435i = { + 'camera_name': 'D435I', + 'device_type': 'D435I', 'enable_color':'true', 'enable_depth':'true', 'depth_module.profile':'848x480x30', @@ -166,7 +167,7 @@ def test_camera_align_depth_color(self,launch_descr_with_parameters): @pytest.mark.parametrize("launch_descr_with_parameters", [ pytest.param(test_params_all_profiles_d455, marks=pytest.mark.d455), pytest.param(test_params_all_profiles_d415, marks=pytest.mark.d415), - pytest.param(test_params_all_profiles_d435, marks=pytest.mark.d435),] + pytest.param(test_params_all_profiles_d435i, marks=pytest.mark.d435i),] ,indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) class TestCamera_AllAlignDepthColor(pytest_rs_utils.RsTestBaseClass): @@ -178,6 +179,7 @@ def test_camera_all_align_depth_color(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: print("Device not found? : " + params['device_type']) + assert False return themes = [ {'topic':get_node_heirarchy(params)+'/color/image_raw', diff --git a/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py b/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py index b337e6f8d5..ab66cf26be 100644 --- a/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py @@ -127,9 +127,9 @@ def check_if_skip_test(profile, format): 'camera_name': 'D415', 'device_type': 'D415', } -test_params_all_profiles_d435 = { - 'camera_name': 'D435', - 'device_type': 'D435', +test_params_all_profiles_d435i = { + 'camera_name': 'D435I', + 'device_type': 'D435I', } @@ -144,7 +144,7 @@ def check_if_skip_test(profile, format): @pytest.mark.parametrize("launch_descr_with_parameters", [ pytest.param(test_params_all_profiles_d455, marks=pytest.mark.d455), pytest.param(test_params_all_profiles_d415, marks=pytest.mark.d415), - pytest.param(test_params_all_profiles_d435, marks=pytest.mark.d435),] + pytest.param(test_params_all_profiles_d435i, marks=pytest.mark.d435i),] ,indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) class TestLiveCamera_Change_Resolution(pytest_rs_utils.RsTestBaseClass): @@ -154,6 +154,10 @@ def test_LiveCamera_Change_Resolution(self,launch_descr_with_parameters): num_passed = 0 num_failed = 0 params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: + print("Device not found? : " + params['device_type']) + assert False + return themes = [{'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image,'expected_data_chunks':1}] config = pytest_live_camera_utils.get_profile_config(get_node_heirarchy(params)) try: diff --git a/realsense2_camera/test/live_camera/test_camera_fps_tests.py b/realsense2_camera/test/live_camera/test_camera_fps_tests.py index c4b91354d4..ef67597014 100644 --- a/realsense2_camera/test/live_camera/test_camera_fps_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_fps_tests.py @@ -74,6 +74,7 @@ def test_camera_test_fps(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: print("Device not found? : " + params['device_type']) + assert False return try: ''' diff --git a/realsense2_camera/test/live_camera/test_camera_imu_tests.py b/realsense2_camera/test/live_camera/test_camera_imu_tests.py index 384d249f47..cad8e8033e 100644 --- a/realsense2_camera/test/live_camera/test_camera_imu_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_imu_tests.py @@ -57,6 +57,7 @@ def test_LiveCamera_check_motion_sensor(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: print("Device not found? : " + params['device_type']) + assert False return themes = [{'topic':get_node_heirarchy(params)+'/imu', 'msg_type':msg_Imu,'expected_data_chunks':1}, {'topic':get_node_heirarchy(params)+'/gyro/sample', 'msg_type':msg_Imu,'expected_data_chunks':1}, diff --git a/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py b/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py index 14def86ca9..3f012f133b 100644 --- a/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py @@ -55,9 +55,9 @@ 'device_type': 'D455', 'pointcloud.enable': 'true' } -test_params_points_cloud_d435 = { - 'camera_name': 'D435', - 'device_type': 'D435', +test_params_points_cloud_d435i = { + 'camera_name': 'D435I', + 'device_type': 'D435I', 'pointcloud.enable': 'true' } @@ -76,7 +76,7 @@ ''' @pytest.mark.parametrize("launch_descr_with_parameters", [ pytest.param(test_params_points_cloud_d455, marks=pytest.mark.d455), - pytest.param(test_params_points_cloud_d435, marks=pytest.mark.d435), + pytest.param(test_params_points_cloud_d435i, marks=pytest.mark.d435i), pytest.param(test_params_points_cloud_d415, marks=pytest.mark.d415), ],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) @@ -85,6 +85,7 @@ def test_camera_test_point_cloud(self,launch_descr_with_parameters): self.params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(self.params['device_type']) == False: print("Device not found? : " + self.params['device_type']) + assert False return themes = [ { diff --git a/realsense2_camera/test/live_camera/test_camera_tf_tests.py b/realsense2_camera/test/live_camera/test_camera_tf_tests.py index a31f74e77c..15edc4d6f9 100644 --- a/realsense2_camera/test/live_camera/test_camera_tf_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_tf_tests.py @@ -58,9 +58,9 @@ 'enable_accel': 'true', 'enable_gyro': 'true', } -test_params_tf_static_change_d435 = { - 'camera_name': 'D435', - 'device_type': 'D435', +test_params_tf_static_change_d435i = { + 'camera_name': 'D435I', + 'device_type': 'D435I', 'enable_infra1': 'false', 'enable_infra2': 'true', 'enable_accel': 'true', @@ -77,7 +77,7 @@ } @pytest.mark.parametrize("launch_descr_with_parameters", [ pytest.param(test_params_tf_static_change_d455, marks=pytest.mark.d455), - pytest.param(test_params_tf_static_change_d435, marks=pytest.mark.d435), + pytest.param(test_params_tf_static_change_d435i, marks=pytest.mark.d435i), pytest.param(test_params_tf_static_change_d415, marks=pytest.mark.d415), ],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) @@ -86,6 +86,7 @@ def test_camera_test_tf_static_change(self,launch_descr_with_parameters): self.params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(self.params['device_type']) == False: print("Device not found? : " + self.params['device_type']) + assert False return themes = [ {'topic':'/tf_static', @@ -145,9 +146,9 @@ def process_data(self, themes, enable_infra1): 'tf_publish_rate': '1.1', } -test_params_tf_d435 = { - 'camera_name': 'D435', - 'device_type': 'D435', +test_params_tf_d435i = { + 'camera_name': 'D435I', + 'device_type': 'D435I', 'publish_tf': 'true', 'tf_publish_rate': '1.1', } @@ -160,7 +161,7 @@ def process_data(self, themes, enable_infra1): } @pytest.mark.parametrize("launch_descr_with_parameters", [ pytest.param(test_params_tf_d455, marks=pytest.mark.d455), - pytest.param(test_params_tf_d435, marks=pytest.mark.d435), + pytest.param(test_params_tf_d435i, marks=pytest.mark.d435i), pytest.param(test_params_tf_d415, marks=pytest.mark.d415), ],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) @@ -169,6 +170,7 @@ def test_camera_test_tf_dyn(self,launch_descr_with_parameters): self.params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(self.params['device_type']) == False: print("Device not found? : " + self.params['device_type']) + assert False return themes = [ {'topic':'/tf', diff --git a/realsense2_camera/test/live_camera/test_d415_basic_tests.py b/realsense2_camera/test/live_camera/test_d415_basic_tests.py index 649f84e7bd..7454d75a78 100644 --- a/realsense2_camera/test/live_camera/test_d415_basic_tests.py +++ b/realsense2_camera/test/live_camera/test_d415_basic_tests.py @@ -57,6 +57,7 @@ def test_D415_Change_Resolution(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: print("Device not found? : " + params['device_type']) + assert False return failed_tests = [] num_passed = 0 diff --git a/realsense2_camera/test/live_camera/test_d455_basic_tests.py b/realsense2_camera/test/live_camera/test_d455_basic_tests.py index fb2f58cab6..3b501e8e6c 100644 --- a/realsense2_camera/test/live_camera/test_d455_basic_tests.py +++ b/realsense2_camera/test/live_camera/test_d455_basic_tests.py @@ -58,6 +58,7 @@ def test_D455_Change_Resolution(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: print("Device not found? : " + params['device_type']) + assert False return themes = [ @@ -116,6 +117,7 @@ def test_D455_Seq_ID_update(self,launch_descr_with_parameters): params = launch_descr_with_parameters[1] if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False: print("Device not found? : " + params['device_type']) + assert False return try: diff --git a/realsense2_camera/test/utils/pytest_live_camera_utils.py b/realsense2_camera/test/utils/pytest_live_camera_utils.py index 9b5bfeac70..64ff6eeae6 100644 --- a/realsense2_camera/test/utils/pytest_live_camera_utils.py +++ b/realsense2_camera/test/utils/pytest_live_camera_utils.py @@ -72,20 +72,29 @@ def get_depth_profiles(long_data, start_index, end_index): if len(long_data[line_no]) == 0: break debug_print("depth profile processing:" + long_data[line_no]) - depth_profile = long_data[line_no].split() - if len(depth_profile) == 5: - profile = depth_profile[0] - value = depth_profile[1]+"x"+depth_profile[3] - format = depth_profile[4] - elif len(depth_profile) == 6: - profile = depth_profile[0]+depth_profile[1] - value = depth_profile[2]+"x"+depth_profile[4] - format = depth_profile[5] + enumerate_devices_line_splitted = long_data[line_no].split() + if len(enumerate_devices_line_splitted) == 7: + stream0_idx = 1 + stream1_idx = 0 + resolution_idx = 2 + frequency_idx = 5 + format_idx = 3 + elif len(enumerate_devices_line_splitted) == 8: + stream0_idx = 1 + stream1_idx = 2 + resolution_idx = 3 + frequency_idx = 6 + format_idx = 4 else: assert false, "Seems that the depth profile info format printed by rs-enumerate-devices changed" - value = value[:-2] - debug_print("depth profile added: " + profile, value, format) - cap.append([profile, value, format]) + if stream1_idx != 0: + depth_camera_stream = enumerate_devices_line_splitted[stream0_idx]+enumerate_devices_line_splitted[stream1_idx] + else: + depth_camera_stream = enumerate_devices_line_splitted[stream0_idx] + depth_profile_param = enumerate_devices_line_splitted[resolution_idx]+"x"+enumerate_devices_line_splitted[frequency_idx] + depth_format_param = enumerate_devices_line_splitted[format_idx] + debug_print("depth profile added: " + depth_camera_stream, depth_profile_param, depth_format_param) + cap.append([depth_camera_stream, depth_profile_param, depth_format_param]) debug_print(cap) return cap @@ -96,16 +105,19 @@ def get_color_profiles(long_data, start_index, end_index): if len(long_data[line_no]) == 0: break debug_print("color profile processing:" + long_data[line_no]) - color_profile = long_data[line_no].split() - if len(color_profile) == 5: - profile = color_profile[0] - value = color_profile[1]+"x"+color_profile[3] - format = color_profile[4] + enumerate_devices_line_splitted = long_data[line_no].split() + if len(enumerate_devices_line_splitted) == 7: + stream_idx = 1 + resolution_idx = 2 + frequency_idx = 5 + format_idx = 3 else: assert false, "Seems that the color profile info format printed by rs-enumerate-devices changed" - value = value[:-2] - debug_print("color profile added: " + profile, value, format) - cap.append([profile, value, format]) + color_camera_stream = enumerate_devices_line_splitted[stream_idx] + color_profile_param = enumerate_devices_line_splitted[resolution_idx]+"x"+enumerate_devices_line_splitted[frequency_idx] + color_format_param = enumerate_devices_line_splitted[format_idx] + debug_print("color profile added: " + color_camera_stream, color_profile_param, color_format_param) + cap.append([color_camera_stream, color_profile_param, color_format_param]) debug_print(cap) return cap @@ -147,7 +159,7 @@ def parse_device_info(long_data, start_index, end_index, device_type, serial_no) return capability def get_camera_capabilities(device_type, serial_no=None): - long_data = os.popen("rs-enumerate-devices").read().splitlines() + long_data = os.popen("rs-enumerate-devices -v").read().splitlines() debug_print(serial_no) index = 0 while index < len(long_data): diff --git a/realsense2_description/launch/launch_utils.py b/realsense2_description/launch/launch_utils.py index 2f65eeb220..10e43fb064 100644 --- a/realsense2_description/launch/launch_utils.py +++ b/realsense2_description/launch/launch_utils.py @@ -21,12 +21,13 @@ def to_urdf(xacro_path, parameters=None): * xacro_path -- the path to the xacro file * parameters -- to be used when xacro file is parsed. """ - urdf_path = tempfile.TemporaryFile(prefix="%s_" % os.path.basename(xacro_path)) + with tempfile.NamedTemporaryFile(prefix="%s_" % os.path.basename(xacro_path), delete=False) as xacro_file: + urdf_path = xacro_file.name # open and process file doc = xacro.process_file(xacro_path, mappings=parameters) # open the output file - out = xacro.open_output(urdf_path) - out.write(doc.toprettyxml(indent=' ')) + with open(urdf_path, 'w') as urdf_file: + urdf_file.write(doc.toprettyxml(indent=' ')) return urdf_path