[sensors] support multiple realsense cameras

urc_perception/launch/perception.launch.py

@@ -64,10 +64,11 @@ def generate_launch_description():
     realsense = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             [PathJoinSubstitution([FindPackageShare(
-                "realsense2_camera"), "launch", "rs_launch.py"])]
+                "urc_perception"), "launch", "rs_multi_camera_launch.py"])]
         ),
     )
 
+
     return LaunchDescription([
         aruco_detector_node,
         aruco_location_node,

urc_perception/launch/rs_launch.py

@@ -0,0 +1,117 @@
+# 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.
+
+"""Launch realsense2_camera node."""
+import os
+import yaml
+from launch import LaunchDescription
+import launch_ros.actions
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+
+
+configurable_parameters = [{'name': 'camera_name',                  'default': 'camera', 'description': 'camera unique name'},
+                           {'name': 'camera_namespace',             'default': 'camera', 'description': 'namespace for camera'},
+                           {'name': 'serial_no',                    'default': "''", 'description': 'choose device by serial number'},
+                           {'name': 'usb_port_id',                  'default': "''", 'description': 'choose device by usb port id'},
+                           {'name': 'device_type',                  'default': "''", 'description': 'choose device by type'},
+                           {'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.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.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'},
+                           {'name': 'depth_module.exposure',        'default': '8500', 'description': 'Depth module manual exposure value'},
+                           {'name': 'depth_module.gain',            'default': '16', 'description': 'Depth module manual gain value'},
+                           {'name': 'depth_module.hdr_enabled',     'default': 'false', 'description': 'Depth module hdr enablement flag. Used for hdr_merge filter'},
+                           {'name': 'depth_module.enable_auto_exposure', 'default': 'true', 'description': 'enable/disable auto exposure for depth image'},
+                           {'name': 'depth_module.exposure.1',      'default': '7500', 'description': 'Depth module first exposure value. Used for hdr_merge filter'},
+                           {'name': 'depth_module.gain.1',          'default': '16', 'description': 'Depth module first gain value. Used for hdr_merge filter'},
+                           {'name': 'depth_module.exposure.2',      'default': '1', 'description': 'Depth module second exposure value. Used for hdr_merge filter'},
+                           {'name': 'depth_module.gain.2',          'default': '16', 'description': 'Depth module second gain value. Used for hdr_merge filter'},
+                           {'name': 'enable_sync',                  'default': 'false', 'description': "'enable sync mode'"},
+                           {'name': 'enable_rgbd',                  'default': 'false', 'description': "'enable rgbd topic'"},
+                           {'name': 'enable_gyro',                  'default': 'false', 'description': "'enable gyro stream'"},
+                           {'name': 'enable_accel',                 'default': 'false', 'description': "'enable accel stream'"},
+                           {'name': 'gyro_fps',                     'default': '0', 'description': "''"},
+                           {'name': 'accel_fps',                    'default': '0', 'description': "''"},
+                           {'name': 'unite_imu_method',             'default': "0", 'description': '[0-None, 1-copy, 2-linear_interpolation]'},
+                           {'name': 'clip_distance',                'default': '-2.', 'description': "''"},
+                           {'name': 'angular_velocity_cov',         'default': '0.01', 'description': "''"},
+                           {'name': 'linear_accel_cov',             'default': '0.01', 'description': "''"},
+                           {'name': 'diagnostics_period',           'default': '0.0', 'description': 'Rate of publishing diagnostics. 0=Disabled'},
+                           {'name': 'publish_tf',                   'default': 'true', 'description': '[bool] enable/disable publishing static & dynamic TF'},
+                           {'name': 'tf_publish_rate',              'default': '0.0', 'description': '[double] rate in Hz for publishing dynamic TF'},
+                           {'name': 'pointcloud.enable',            'default': 'false', 'description': ''},
+                           {'name': 'pointcloud.stream_filter',     'default': '2', 'description': 'texture stream for pointcloud'},
+                           {'name': 'pointcloud.stream_index_filter','default': '0', 'description': 'texture stream index for pointcloud'},
+                           {'name': 'pointcloud.ordered_pc',        'default': 'false', 'description': ''},
+                           {'name': 'pointcloud.allow_no_texture_points', 'default': 'false', 'description': "''"},
+                           {'name': 'align_depth.enable',           'default': 'false', 'description': 'enable align depth filter'},
+                           {'name': 'colorizer.enable',             'default': 'false', 'description': 'enable colorizer filter'},
+                           {'name': 'decimation_filter.enable',     'default': 'false', 'description': 'enable_decimation_filter'},
+                           {'name': 'spatial_filter.enable',        'default': 'false', 'description': 'enable_spatial_filter'},
+                           {'name': 'temporal_filter.enable',       'default': 'false', 'description': 'enable_temporal_filter'},
+                           {'name': 'disparity_filter.enable',      'default': 'false', 'description': 'enable_disparity_filter'},
+                           {'name': 'hole_filling_filter.enable',   'default': 'false', 'description': 'enable_hole_filling_filter'},
+                           {'name': 'hdr_merge.enable',             'default': 'false', 'description': 'hdr_merge filter enablement flag'},
+                           {'name': 'wait_for_device_timeout',      'default': '-1.', 'description': 'Timeout for waiting for device to connect (Seconds)'},
+                           {'name': 'reconnect_timeout',            'default': '6.', 'description': 'Timeout(seconds) between consequtive reconnection attempts'},
+                          ]
+
+def declare_configurable_parameters(parameters):
+    return [DeclareLaunchArgument(param['name'], default_value=param['default'], description=param['description']) for param in parameters]
+
+def set_configurable_parameters(parameters):
+    return dict([(param['name'], LaunchConfiguration(param['name'])) for param in parameters])
+
+def yaml_to_dict(path_to_yaml):
+    with open(path_to_yaml, "r") as f:
+        return yaml.load(f, Loader=yaml.SafeLoader)
+
+def launch_setup(context, params, param_name_suffix=''):
+    _config_file = LaunchConfiguration('config_file' + param_name_suffix).perform(context)
+    params_from_file = {} if _config_file == "''" else yaml_to_dict(_config_file)
+    return [
+        launch_ros.actions.Node(
+            package='realsense2_camera',
+            namespace=LaunchConfiguration('camera_namespace' + param_name_suffix),
+            name=LaunchConfiguration('camera_name' + param_name_suffix),
+            executable='realsense2_camera_node',
+            parameters=[params, params_from_file],
+            output=LaunchConfiguration('output' + param_name_suffix),
+            arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level' + param_name_suffix)],
+            emulate_tty=True,
+            )
+    ]
+
+def generate_launch_description():
+    return LaunchDescription(declare_configurable_parameters(configurable_parameters) + [
+        OpaqueFunction(function=launch_setup, kwargs = {'params' : set_configurable_parameters(configurable_parameters)})
+    ])

urc_perception/launch/rs_multi_camera_launch.py

@@ -0,0 +1,91 @@
+# 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 2 devices.
+# The Parameters available for definition in the command line for each camera are described in rs_launch.configurable_parameters
+# For each device, the parameter name was changed to include an index.
+# For example: to set camera_name for device1 set parameter camera_name1.
+# command line example:
+# ros2 launch realsense2_camera rs_multi_camera_launch.py camera_name1:=D400 device_type2:=l5. device_type1:=d4..
+
+"""Launch realsense2_camera node."""
+import copy
+from launch import LaunchDescription, LaunchContext
+import launch_ros.actions
+from launch.actions import IncludeLaunchDescription, OpaqueFunction
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.absolute()))
+import rs_launch
+
+local_parameters = [{'name': 'camera_name1', 'default': 'camera1', 'description': 'camera1 unique name'},
+                    {'name': 'camera_name2', 'default': 'camera2', 'description': 'camera2 unique name'},
+                    {'name': 'camera_name3', 'default': 'camera3', 'description': 'camera3 unique name'},
+                    {'name': 'camera_namespace1', 'default': 'camera1', 'description': 'camera1 namespace'},
+                    {'name': 'camera_namespace2', 'default': 'camera2', 'description': 'camera2 namespace'},
+                    {'name': 'camera_namespace3', 'default': 'camera3', 'description': 'camera3 namespace'}]
+
+def set_configurable_parameters(local_params):
+    return dict([(param['original_name'], LaunchConfiguration(param['name'])) for param in local_params])
+
+def duplicate_params(general_params, posix):
+    local_params = copy.deepcopy(general_params)
+    for param in local_params:
+        param['original_name'] = param['name']
+        param['name'] += posix
+    return local_params
+
+def launch_static_transform_publisher_node(context : LaunchContext):
+    # dummy static transformation from camera1 to camera2
+    node = launch_ros.actions.Node(
+            package = "tf2_ros",
+            executable = "static_transform_publisher",
+            arguments = ["0", "0", "0", "0", "0", "0", # if integration of all 3 cameras does not work, potentially add two more zeroes
+                          context.launch_configurations['camera_name1'] + "_link",
+                          context.launch_configurations['camera_name2'] + "_link",
+                          context.launch_configurations['camera_name3'] + "_link"]
+    )
+    return [node]
+
+def generate_launch_description():
+    #If modifying the given serial numbers, make sure that the values that are being set are of the form:
+    # "'serialNumber'" or if the serial number begins with a 0
+    # "'_serialNumber'"
+    params1 = duplicate_params(rs_launch.configurable_parameters, '1')
+    params1[2]["default"] = "'_018322070641'" # Setting the serial number for camera 1
+    params2 = duplicate_params(rs_launch.configurable_parameters, '2')
+    params2[2]["default"] = "'_017322073488'"  # Setting the serial number for camera 2
+    params3 = duplicate_params(rs_launch.configurable_parameters, '3')
+    params3[2]["default"] = "'_044322070306'"  # Setting the serial number for camera 3
+    return LaunchDescription(
+        rs_launch.declare_configurable_parameters(local_parameters) +
+        rs_launch.declare_configurable_parameters(params1) +
+        rs_launch.declare_configurable_parameters(params2) +
+        rs_launch.declare_configurable_parameters(params3) +
+        [
+        OpaqueFunction(function=rs_launch.launch_setup,
+                       kwargs = {'params'           : set_configurable_parameters(params1),
+                                 'param_name_suffix': '1'}),
+        OpaqueFunction(function=rs_launch.launch_setup,
+                       kwargs = {'params'           : set_configurable_parameters(params2),
+                                 'param_name_suffix': '2'}),
+        OpaqueFunction(function=rs_launch.launch_setup,
+                       kwargs = {'params'           : set_configurable_parameters(params3),
+                                 'param_name_suffix': '3'}),
+        OpaqueFunction(function=launch_static_transform_publisher_node)
+    ])