Bug fiix import patron.xacro and append packages realsense2
This commit is contained in:
Даниил Грабарь
@@ -0,0 +1,169 @@
|
||||
# Copyright 2023 RealSense, Inc. 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, LogInfo
|
||||
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': 'enable_motion', 'default': 'false', 'description': "'enable motion stream (IMU) for DDS devices'"},
|
||||
{'name': 'gyro_fps', 'default': '0', 'description': "''"},
|
||||
{'name': 'accel_fps', 'default': '0', 'description': "''"},
|
||||
{'name': 'motion_fps', 'default': '0', 'description': "'motion stream samples per second'"},
|
||||
{'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'},
|
||||
{'name': 'base_frame_id', 'default': 'link', 'description': 'Root frame of the sensors transform tree'},
|
||||
{'name': 'tf_prefix', 'default': '', 'description': 'prefix to be prepended to all frame IDs'},
|
||||
{'name': 'decimation_filter.filter_magnitude', 'default': '2', 'description': 'decimation filter magnitude'},
|
||||
{'name': 'enable_safety', 'default': 'false', 'description': "'enable safety stream'"},
|
||||
{'name': 'safety_camera.safety_mode', 'default': '0', 'description': '[int] 0-Run, 1-Standby, 2-Service'},
|
||||
{'name': 'enable_labeled_point_cloud', 'default': 'false', 'description': "'enable labeled point cloud stream'"},
|
||||
{'name': 'depth_mapping_camera.labeled_point_cloud_profile', 'default': '0,0,0', 'description': "'Label PointCloud stream profile'"},
|
||||
{'name': 'enable_occupancy', 'default': 'false', 'description': "'enable occupancy stream'"},
|
||||
{'name': 'depth_mapping_camera.occupancy_profile', 'default': '0,0,0', 'description': "'Occupancy stream profile'"},
|
||||
]
|
||||
|
||||
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)
|
||||
|
||||
# Get list of supported parameters
|
||||
supported_params = set(param['name'] for param in configurable_parameters)
|
||||
|
||||
# Check for unsupported parameters in command line arguments
|
||||
# Warn for any launch arguments not in supported_params
|
||||
for param_name in context.launch_configurations.keys():
|
||||
if param_name not in supported_params:
|
||||
print(f"\033[33mWarning: Parameter '{param_name}' is not supported. Supported parameters are:\n{sorted(supported_params)}\033[0m")
|
||||
|
||||
# Check for unsupported parameters in config file
|
||||
if params_from_file:
|
||||
for param_name in params_from_file.keys():
|
||||
if param_name not in supported_params:
|
||||
print(f"\033[33mWarning: Parameter '{param_name}' in config file is not supported. Supported parameters are:\n{sorted(supported_params)}\033[0m")
|
||||
|
||||
# Load lifecycle nodes setting from YAML dynamically generated by CMAKE instead of environment variable
|
||||
lifecycle_param_file = os.path.join(
|
||||
os.path.dirname(__file__), '..', 'config', 'global_settings.yaml'
|
||||
)
|
||||
lifecycle_params = yaml_to_dict(lifecycle_param_file)
|
||||
use_lifecycle_node = lifecycle_params.get("use_lifecycle_node", False)
|
||||
|
||||
_output = LaunchConfiguration('output' + param_name_suffix)
|
||||
|
||||
# Dynamically choose Node or LifecycleNode
|
||||
node_action = launch_ros.actions.LifecycleNode if use_lifecycle_node else launch_ros.actions.Node
|
||||
log_message = "Launching as LifecycleNode" if use_lifecycle_node else "Launching as Normal ROS Node"
|
||||
|
||||
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 [
|
||||
LogInfo(msg=f"🚀 {log_message}"),
|
||||
node_action(
|
||||
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)})
|
||||
])
|
||||
Reference in New Issue
Block a user