-
Notifications
You must be signed in to change notification settings - Fork 1.8k
/
Copy pathrs_launch.py
132 lines (122 loc) · 11.8 KB
/
rs_launch.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
# 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': 'rosbag_loop', 'default': 'false', 'description': 'Enable loop playback when playing a bagfile'},
{'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.color_profile', 'default': '0,0,0', 'description': 'Depth module color stream profile for d405'},
{'name': 'depth_module.color_format', 'default': 'RGB8', 'description': 'color stream format for d405'},
{'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': 'depth_module.inter_cam_sync_mode', 'default': "0", 'description': '[0-Default, 1-Master, 2-Slave]'},
{'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': 'enable_motion', 'default': 'false', 'description': "'enable motion stream (IMU) for DDS devices'"},
{'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': 'rotation_filter.enable', 'default': 'false', 'description': 'enable rotation_filter'},
{'name': 'rotation_filter.rotation', 'default': '0.0', 'description': 'rotation value: 0.0, 90.0, -90.0, 180.0'},
{'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)
_output = LaunchConfiguration('output' + param_name_suffix)
if(os.getenv('ROS_DISTRO') == 'foxy'):
# Foxy doesn't support output as substitution object (LaunchConfiguration object)
# but supports it as string, so we fetch the string from this substitution object
# see related PR that was merged for humble, iron, rolling: https://github.com/ros2/launch/pull/577
_output = context.perform_substitution(_output)
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=_output,
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)})
])