Merge branch 'IntelRealSense:ros2-development' into ros2

.github/workflows/main.yml

@@ -40,7 +40,7 @@ jobs:
       run: |
         mkdir -p ${{github.workspace}}/ros2/src
 
-    - uses: actions/checkout@v3
+    - uses: actions/checkout@v4
       with:
         path: 'ros2/src/realsense-ros'
 

.github/workflows/pre-release.yml

@@ -45,6 +45,6 @@ jobs:
       BASEDIR: ${{ github.workspace }}/.work
 
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - name: industrial_ci
         uses: ros-industrial/industrial_ci@master

README.md

@@ -1,3 +1,4 @@
+
 <h1 align="center">
    <img src="https://www.intelrealsense.com/wp-content/uploads/2020/09/intel-realsense-logo-360px.png" alt="Intel® RealSense™" title="Intel® RealSense™" />
 </h1>
@@ -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 @@
 </details>
 
 
-# Installation
+# Installation on Ubuntu
 
 <details>
   <summary>
@@ -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)
-
 </details>
 
 <details>
   <summary>
     Step 2: Install latest Intel&reg; RealSense&trade; SDK 2.0
   </summary>
-
+  **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 @@
 
 <hr>
 
+# 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**
+
+<details>
+  <summary>
+    Step 1: Install the ROS2 distribution 
+  </summary>
+
+- #### 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
+</details>
+
+<details>
+  <summary>
+    Step 2: Download RealSense&trade; ROS2 Wrapper and RealSense&trade; SDK 2.0 source code from github:
+  </summary>
+
+- Download Intel&reg; RealSense&trade; ROS2 Wrapper source code from [Intel&reg; RealSense&trade; ROS2 Wrapper Releases](https://github.com/IntelRealSense/realsense-ros/releases)
+- Download the corrosponding supported Intel&reg; RealSense&trade; 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
+</details>
+
+<details>
+  <summary>
+    Step 3: Build
+  </summary>
+
+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
+      ```
+</details>
+
+<hr>
+
+
 # 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
 
 <hr>
 
@@ -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 **<stream_type>_profile** parameter.
     - The profile parameter is a string of the following format: \<width>X\<height>X\<fps> (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 <your_node_name> <param_name>``` to get the list of supported profiles.
     - Note: Should re-enable the stream for the change to take effect.

realsense2_camera/CMakeLists.txt

@@ -115,6 +115,7 @@ 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.54.1)
 if (BUILD_ACCELERATE_GPU_WITH_GLSL)
@@ -127,6 +128,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
@@ -220,6 +223,8 @@ else()
   set(link_libraries ${realsense2_LIBRARY})
 endif()
 
+list(APPEND link_libraries ${OpenCV_LIBS})  # add OpenCV libs to link_libraries
+
 target_link_libraries(${PROJECT_NAME}
     ${link_libraries}
 )

realsense2_camera/include/profile_manager.h

@@ -68,22 +68,22 @@ namespace realsense2_camera
             VideoProfilesManager(std::shared_ptr<Parameters> 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<stream_profile> all_profiles, std::function<void()> 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<stream_index_pair> 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<stream_index_pair, rs2_format>  _formats;
-            int      _fps;
-            int _width, _height;
+            std::map<rs2_stream, int> _fps, _width, _height;
             bool _is_profile_exist;
             bool _force_image_default_qos;
     };

realsense2_camera/launch/rs_launch.py

@@ -34,15 +34,16 @@
                            {'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'},

realsense2_camera/src/base_realsense_node.cpp

@@ -250,11 +250,14 @@ void BaseRealSenseNode::setupFilters()
     _pc_filter = std::make_shared<PointcloudFilter>(std::make_shared<rs2::pointcloud>(), _node, _parameters, _logger);
 #endif
 
-    _filters.push_back(_colorizer_filter);
+    // 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<AlignDepthFilter>(std::make_shared<rs2::align>(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)