mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-09-12 19:20:20 +08:00
266 lines
10 KiB
Python
266 lines
10 KiB
Python
import os
|
|
import yaml
|
|
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument, OpaqueFunction, GroupAction
|
|
from launch.substitutions import LaunchConfiguration
|
|
from launch_ros.actions import PushRosNamespace, ComposableNodeContainer, Node
|
|
from launch_ros.descriptions import ComposableNode
|
|
|
|
|
|
def load_yaml(file_path):
|
|
with open(file_path, 'r') as f:
|
|
return yaml.safe_load(f)
|
|
|
|
|
|
def merge_params(default_params, yaml_params):
|
|
for key, value in yaml_params.items():
|
|
if key in default_params:
|
|
default_params[key] = value
|
|
return default_params
|
|
|
|
|
|
def convert_value(value):
|
|
if isinstance(value, str):
|
|
try:
|
|
return int(value)
|
|
except ValueError:
|
|
pass
|
|
try:
|
|
return float(value)
|
|
except ValueError:
|
|
pass
|
|
if value.lower() == 'true':
|
|
return True
|
|
elif value.lower() == 'false':
|
|
return False
|
|
return value
|
|
|
|
|
|
def load_parameters(context, args):
|
|
default_params = {arg.name: LaunchConfiguration(arg.name).perform(context) for arg in args}
|
|
config_file_path = LaunchConfiguration('config_file_path').perform(context)
|
|
if config_file_path:
|
|
yaml_params = load_yaml(config_file_path)
|
|
default_params = merge_params(default_params, yaml_params)
|
|
skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
|
|
return {
|
|
key: (value if key in skip_convert else convert_value(value))
|
|
for key, value in default_params.items()
|
|
}
|
|
|
|
|
|
def generate_launch_description():
|
|
args = [
|
|
DeclareLaunchArgument(
|
|
'device_type',
|
|
default_value='lidar',
|
|
description='Device type to start: lidar or camera. Set lidar to start LiDAR; set camera to start a camera device.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'camera_name',
|
|
default_value='lidar',
|
|
description='Node namespace/device name. Used as prefix for topics, parameters, and TF.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'device_num',
|
|
default_value='1',
|
|
description='Number of devices to start. Required when launching multiple devices.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'upgrade_firmware',
|
|
default_value='',
|
|
description='Firmware file path. If set, attempts firmware upgrade on startup; empty means no upgrade.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'connection_delay',
|
|
default_value='10',
|
|
description='Reopen delay after hot-plug (milliseconds). Prevents firmware issues caused by immediate reconnect.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'publish_tf',
|
|
default_value='true',
|
|
description='Whether to publish TF frames. true to enable, false to disable.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'tf_publish_rate',
|
|
default_value='0.0',
|
|
description='TF publish frequency in Hz. <= 0 means use default or publish on demand.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'lidar_format',
|
|
default_value='ANY', # LIDAR_POINT, LIDAR_SPHERE_POINT, LIDAR_SCAN
|
|
description='LiDAR output format: LIDAR_POINT, LIDAR_SPHERE_POINT, or LIDAR_SCAN. ANY lets device/config decide.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'lidar_rate',
|
|
default_value='20',
|
|
description='LiDAR scan/publish rate in Hz. Supported range depends on device model.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'publish_n_pkts',
|
|
default_value='1',
|
|
description='Number of frames to accumulate before publishing. Range: 1-12000. Used for multi-frame data merging.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'enable_scan_to_point',
|
|
default_value='false',
|
|
description='Convert LaserScan to PointCloud2 and publish. true to enable, false to disable.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'repetitive_scan_mode',
|
|
default_value='-1',
|
|
description='Repetitive scan mode. -1 uses device default; 0 non-repeating; 1/2/3 for different modes.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'filter_level',
|
|
default_value='-1',
|
|
description='Filtering level. -1 uses default; non-negative integers increase filtering strength, 0-5 are valid levels.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'vertical_fov',
|
|
default_value='-1.0',
|
|
description='Vertical field of view in degrees. -1 uses device default.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'min_angle',
|
|
default_value='-135.0',
|
|
description='Minimum scan angle in degrees (e.g., -135.0).'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'max_angle',
|
|
default_value='135.0',
|
|
description='Maximum scan angle in degrees (e.g., 135.0).'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'min_range',
|
|
default_value='0.05',
|
|
description='Minimum measurable range in meters.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'max_range',
|
|
default_value='30.0',
|
|
description='Maximum measurable range in meters.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'echo_mode',
|
|
default_value='single channel',
|
|
description='Echo mode. Examples: single channel, First Echo, Last Echo. Actual options depend on device.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'point_cloud_qos',
|
|
default_value='default',
|
|
description='QoS for PointCloud/LaserScan: SYSTEM_DEFAULT, DEFAULT, PARAMETER_EVENTS, SERVICES_DEFAULT, PARAMETERS, SENSOR_DATA (case-insensitive).'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'enumerate_net_device',
|
|
default_value='true',
|
|
description='Automatically enumerate network devices. true for auto-discovery; false to use specified IP/port only.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'net_device_ip',
|
|
default_value='',
|
|
description='IP address of the network device. Leave empty to rely on enumeration or device defaults.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'net_device_port',
|
|
default_value='0',
|
|
description='Port of the network device. 0 uses device/driver defaults.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'log_level',
|
|
default_value='info',
|
|
description='Shared SDK and ROS log level: none, debug, info, warn, error, fatal.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'log_file_name',
|
|
default_value='',
|
|
description='Custom log file name for SDK logs. If empty, default naming is used.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'time_domain',
|
|
default_value='device', # global, device, system
|
|
description='Timestamp domain: device (device clock), global (time sync), or system (host clock).'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'timestamp_clock_type',
|
|
default_value='',
|
|
description='SDK timestamp clock type: realtime or monotonic, default is realtime.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'config_file_path',
|
|
default_value='',
|
|
description='Path to a YAML config file. If provided, overrides same-name launch arguments; otherwise defaults apply.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'enable_heartbeat',
|
|
default_value='false',
|
|
description='Send heartbeat to device.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'enable_imu',
|
|
default_value='false',
|
|
description='Enable IMU (accelerometer and gyroscope) data publishing.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'imu_rate',
|
|
default_value='50hz',
|
|
description='IMU publish rate, e.g., 50hz, 100hz. Actual options depend on device.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'accel_range',
|
|
default_value='2g',
|
|
description='Accelerometer range, e.g., 2g, 4g, 8g, 16g. Device-dependent.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'gyro_range',
|
|
default_value='1000dps',
|
|
description='Gyroscope range, e.g., 250dps, 500dps, 1000dps, 2000dps. Device-dependent.'
|
|
),
|
|
]
|
|
|
|
def get_params(context, args):
|
|
return [load_parameters(context, args)]
|
|
|
|
def create_node_action(context, args):
|
|
params = get_params(context, args)
|
|
ros_distro = os.environ.get("ROS_DISTRO", "humble")
|
|
if ros_distro == "foxy":
|
|
return [
|
|
Node(
|
|
package="orbbec_camera",
|
|
executable="orbbec_camera_node",
|
|
name="ob_camera_node",
|
|
namespace=LaunchConfiguration("camera_name"),
|
|
parameters=params,
|
|
output="log",
|
|
)
|
|
]
|
|
else:
|
|
return [
|
|
GroupAction([
|
|
PushRosNamespace(LaunchConfiguration("camera_name")),
|
|
ComposableNodeContainer(
|
|
name="camera_container",
|
|
namespace="",
|
|
package="rclcpp_components",
|
|
executable="component_container",
|
|
composable_node_descriptions=[
|
|
ComposableNode(
|
|
package="orbbec_camera",
|
|
plugin="orbbec_camera::OBCameraNodeDriver",
|
|
name=LaunchConfiguration("camera_name"),
|
|
parameters=params,
|
|
),
|
|
],
|
|
# prefix=["xterm -e gdb -ex run --args"],
|
|
output="log",
|
|
)
|
|
])
|
|
]
|
|
|
|
return LaunchDescription(
|
|
args + [
|
|
OpaqueFunction(function=lambda context: create_node_action(context, args))
|
|
]
|
|
)
|