Merge ros2-development into ros2-hkr

realsense2_camera/CMakeLists.txt

@@ -143,34 +143,18 @@ set(SOURCES
 if(NOT DEFINED ENV{ROS_DISTRO})
   message(FATAL_ERROR "ROS_DISTRO is not defined." )
 endif()
-if("$ENV{ROS_DISTRO}" STREQUAL "dashing")
-  message(STATUS "Build for ROS2 Dashing")
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DDASHING")
-  set(SOURCES "${SOURCES}" src/ros_param_backend_dashing.cpp)
-elseif("$ENV{ROS_DISTRO}" STREQUAL "eloquent")
-  message(STATUS "Build for ROS2 eloquent")
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DELOQUENT")
-  set(SOURCES "${SOURCES}" src/ros_param_backend_foxy.cpp)
-elseif("$ENV{ROS_DISTRO}" STREQUAL "foxy")
-  message(STATUS "Build for ROS2 Foxy")
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DFOXY")
-  set(SOURCES "${SOURCES}" src/ros_param_backend_foxy.cpp)
-elseif("$ENV{ROS_DISTRO}" STREQUAL "galactic")
-  message(STATUS "Build for ROS2 Galactic")
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DGALACTIC")
-  set(SOURCES "${SOURCES}" src/ros_param_backend_foxy.cpp)
-elseif("$ENV{ROS_DISTRO}" STREQUAL "humble")
+if("$ENV{ROS_DISTRO}" STREQUAL "humble")
   message(STATUS "Build for ROS2 Humble")
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DHUMBLE")
-  set(SOURCES "${SOURCES}" src/ros_param_backend_foxy.cpp)
+  set(SOURCES "${SOURCES}" src/ros_param_backend.cpp)
 elseif("$ENV{ROS_DISTRO}" STREQUAL "iron")
   message(STATUS "Build for ROS2 Iron")
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DIRON")
-  set(SOURCES "${SOURCES}" src/ros_param_backend_foxy.cpp)
+  set(SOURCES "${SOURCES}" src/ros_param_backend.cpp)
 elseif("$ENV{ROS_DISTRO}" STREQUAL "rolling")
   message(STATUS "Build for ROS2 Rolling")
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DROLLING")
-  set(SOURCES "${SOURCES}" src/ros_param_backend_rolling.cpp)
+  set(SOURCES "${SOURCES}" src/ros_param_backend.cpp)
 else()
   message(FATAL_ERROR "Unsupported ROS Distribution: " "$ENV{ROS_DISTRO}")
 endif()
@@ -271,6 +255,12 @@ install(DIRECTORY
     DESTINATION share/${PROJECT_NAME}
     )
 
+# Install example files
+install(DIRECTORY 
+    examples
+    DESTINATION share/${PROJECT_NAME}
+)
+
 # Test
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
@@ -313,6 +303,36 @@ if(BUILD_TESTING)
     )
     endforeach()
   endforeach()
+
+  unset(_pytest_folders)
+
+  set(rs_query_cmd "rs-enumerate-devices -s")
+  execute_process(COMMAND bash -c ${rs_query_cmd}
+      WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}
+      RESULT_VARIABLE rs_result
+      OUTPUT_VARIABLE RS_DEVICE_INFO)
+  message(STATUS "rs_device_info:")
+  message(STATUS "${RS_DEVICE_INFO}")
+  if((RS_DEVICE_INFO MATCHES "D455") OR (RS_DEVICE_INFO MATCHES "D415") OR (RS_DEVICE_INFO MATCHES "D435"))
+    message(STATUS "D455 device found")
+    set(_pytest_live_folders
+      test/live_camera
+    )
+  endif()
+
+  foreach(test_folder ${_pytest_live_folders})
+    file(GLOB files "${test_folder}/test_*.py")
+    foreach(file ${files})
+
+    get_filename_component(_test_name ${file} NAME_WE)
+    ament_add_pytest_test(${_test_name} ${file}
+      APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}:${CMAKE_SOURCE_DIR}/test/utils:${CMAKE_SOURCE_DIR}/launch:${CMAKE_SOURCE_DIR}/scripts
+      TIMEOUT 500
+      WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+    )
+    endforeach()
+  endforeach()
+
 endif()
 
 # Ament exports

realsense2_camera/examples/align_depth/README.md

@@ -0,0 +1,12 @@
+# Align Depth to Color
+This example shows how to start the camera node and align depth stream to color stream.
+```
+ros2 launch realsense2_camera rs_align_depth_launch.py
+```
+
+The aligned image will be published to the topic "/aligned_depth_to_color/image_raw"
+
+Also, align depth to color can enabled by following cmd:
+```
+ros2 launch realsense2_camera rs_launch.py align_depth.enable:=true
+```

realsense2_camera/examples/align_depth/rs_align_depth_launch.py

@@ -0,0 +1,56 @@
+# 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.
+
+# DESCRIPTION #
+# ----------- #
+# Use this launch file to launch a device and align depth to color.
+# The Parameters available for definition in the command line for the camera are described in rs_launch.configurable_parameters
+# command line example:
+# ros2 launch realsense2_camera rs_align_depth_launch.py
+
+"""Launch realsense2_camera node."""
+from launch import LaunchDescription
+import launch_ros.actions
+from launch.actions import OpaqueFunction
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.absolute()))
+import os
+from ament_index_python.packages import get_package_share_directory
+sys.path.append(os.path.join(get_package_share_directory('realsense2_camera'), 'launch'))
+import rs_launch
+
+local_parameters = [{'name': 'camera_name',                  'default': 'camera', 'description': 'camera unique name'},
+                    {'name': 'camera_namespace',             'default': 'camera', 'description': 'camera namespace'},
+                    {'name': 'enable_color',                 'default': 'true', 'description': 'enable color stream'},
+                    {'name': 'enable_depth',                 'default': 'true', 'description': 'enable depth stream'},
+                    {'name': 'align_depth.enable',           'default': 'true', 'description': 'enable align depth filter'},
+                    {'name': 'enable_sync',                  'default': 'true', 'description': 'enable sync mode'},
+                   ]
+
+def set_configurable_parameters(local_params):
+    return dict([(param['name'], LaunchConfiguration(param['name'])) for param in local_params])
+
+
+def generate_launch_description():
+    params = rs_launch.configurable_parameters
+    return LaunchDescription(
+        rs_launch.declare_configurable_parameters(local_parameters) +
+        rs_launch.declare_configurable_parameters(params) + 
+        [
+        OpaqueFunction(function=rs_launch.launch_setup,
+                kwargs = {'params' : set_configurable_parameters(params)}
+        )
+    ])

realsense2_camera/examples/dual_camera/README.md

@@ -0,0 +1,123 @@
+# Launching Dual RS ROS2 nodes
+The following example lanches two RS ROS2 nodes.
+```
+ros2 launch realsense2_camera rs_dual_camera_launch.py serial_no1:=<serial number of the first camera> serial_no2:=<serial number of the second camera>
+```
+
+## Example:
+Let's say the serial numbers of two RS cameras are 207322251310 and 234422060144.
+```
+ros2 launch realsense2_camera rs_dual_camera_launch.py serial_no1:="'207322251310'" serial_no2:="'234422060144'"
+```
+or
+```
+ros2 launch realsense2_camera rs_dual_camera_launch.py serial_no1:=_207322251310 serial_no2:=_234422060144
+```
+
+## How to know the serial number?
+Method 1: Using the rs-enumerate-devices tool
+```
+rs-enumerate-devices | grep "Serial Number"
+```
+
+Method 2: Connect single camera and run
+```
+ros2 launch realsense2_camera rs_launch.py
+```
+and look for the serial number in the log printed to screen under "[INFO][...] Device Serial No:".
+
+# Using Multiple RS camera by launching each in differnet terminals
+Make sure you set a different name and namespace for each camera.
+
+Terminal 1:
+```
+ros2 launch realsense2_camera rs_launch.py serial_no:="'207322251310'" camera_name:='camera1' camera_namespace:='camera1'
+```
+Terminal 2:
+```
+ros2 launch realsense2_camera rs_launch.py serial_no:="'234422060144'" camera_name:='camera2' camera_namespace:='camera2'
+```
+
+# Multiple cameras showing a semi-unified pointcloud
+The D430 series of RealSense cameras use stereo based algorithm to calculate depth. This mean, a couple of cameras can operate on the same scene. For the purpose of this demonstration, let's say 2 cameras can be coupled to look at the same scene from 2 different points of view. See image:
+
+![multi_cameras](https://user-images.githubusercontent.com/127019120/268692789-1b3d5d8b-a41f-4a97-995d-81d44b4bcacb.jpg)
+
+The schematic settings could be described as:  
+X--------------------------------->cam_2  
+|&emsp;&emsp;&emsp;&emsp;(70 cm)  
+|  
+|  
+|&ensp;(60 cm)  
+|  
+|  
+/  
+cam_1  
+
+The cameras have no data regarding their relative position. That’s up to a third party program to set. To simplify things, the coordinate system of cam_1 can be considered as the refernce coordinate system for the whole scene.
+
+The estimated translation of cam_2 from cam_1 is 70(cm) on X-axis and 60(cm) on Y-axis. Also, the estimated yaw angle of cam_2 relative to cam_1 as 90(degrees) clockwise. These are the initial parameters to be set for setting the transformation between the 2 cameras as follows:
+
+```
+ros2 launch realsense2_camera rs_dual_camera_launch.py serial_no1:=_207322251310 serial_no2:=_234422060144 tf.translation.x:=0.7 tf.translation.y:=0.6 tf.translation.z:=0.0 tf.rotation.yaw:=-90.0 tf.rotation.pitch:=0.0 tf.rotation.roll:=0.0
+```
+
+If the unified pointcloud result is not good, follow the below steps to fine-tune the calibaration.   
+
+## Visualizing the pointclouds and fine-tune the camera calibration
+Launch 2 cameras in separate terminals:
+
+**Terminal 1:**
+```
+ros2 launch realsense2_camera rs_launch.py serial_no:="'207322251310'" camera_name:='camera1' camera_namespace:='camera1'
+```
+**Terminal 2:**
+```
+ros2 launch realsense2_camera rs_launch.py serial_no:="'234422060144'" camera_name:='camera2' camera_namespace:='camera2'
+```
+**Terminal 3:**  
+```
+rviz2
+```
+Open rviz and set '“Fixed Frame'” to “camera1_link”  
+Add Pointcloud2-> By topic -> /camera1/camera1/depth/color/points  
+Add Pointcloud2 -> By topic -> /camera2/camera2/depth/color/points  
+
+**Terminal 4:**  
+Run the 'set_cams_transforms.py' tool. It can be used to fine-tune the calibaration.
+```
+python src/realsense-ros/realsense2_camera/scripts/set_cams_transforms.py camera1_link camera2_link 0.7 0.6 0 -90 0 0
+```
+
+**Instructions printed by the tool:**
+```
+Using default file /home/user_name/ros2_ws/src/realsense-ros/realsense2_camera/scripts/_set_cams_info_file.txt
+
+Use given initial values.
+
+Press the following keys to change mode: x, y, z, (a)zimuth, (p)itch, (r)oll
+
+For each mode, press 6 to increase by step and 4 to decrease
+
+Press + to multiply step by 2 or - to divide
+
+Press Q to quit
+```
+
+Note that the tool prints the path of the current configuration file. It saves its last configuration automatically, all the time, to be used on the next run.
+
+After a lot of fiddling around, unified pointcloud looked better with the following calibaration:
+```
+x = 0.75  
+y = 0.575  
+z = 0  
+azimuth = -91.25  
+pitch = 0.75  
+roll = 0  
+```
+
+Now, use the above results in the launch file:
+```
+ros2 launch realsense2_camera rs_dual_camera_launch.py serial_no1:=_207322251310 serial_no2:=_234422060144 tf.translation.x:=0.75 tf.translation.y:=0.575 tf.translation.z:=0.0 tf.rotation.yaw:=-91.25 tf.rotation.pitch:=0.75 tf.rotation.roll:=0.0
+```
+