[launch]Add new launch and yaml description files for v2-main-dev

2026-10-04 03:57:46 +08:00 · 2024-12-18 17:46:48 +08:00
parent bc83801070
commit eca5beffcc
20 changed files with 2972 additions and 0 deletions
@@ -0,0 +1,112 @@
+import os
+import yaml
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, TimerAction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+import sys
+sys.path.append(os.path.dirname(os.path.realpath(__file__)))
+from utils_yaml import *
+
+
+default_package_name = 'orbbec_camera'
+default_launch_file = 'orbbec_camera.launch.py'
+default_multi_camera_config_file = 'multicamera.yaml' #'multi_camera_synced.ymal'
+default_shared_container_name = 'shared_orbbec_container'
+
+
+def generate_launch_arguments():
+    launch_arguments = [
+        #general config
+        DeclareLaunchArgument('config_file_path', default_value=default_multi_camera_config_file),
+        #instra-process demo set
+        DeclareLaunchArgument('use_intra_process_comms', default_value='false'), #false
+        DeclareLaunchArgument('attach_component_container_enable', default_value='false'), #false
+        DeclareLaunchArgument('attach_component_container_name', default_value=default_shared_container_name),
+    ]
+    return launch_arguments
+
+
+def generate_camera_node(args, launch_file_dir, camera_params, launch_file=default_launch_file):
+    config_params = {key: value for key, value in camera_params.items()}
+    #load launch_arguments and meger to yaml params
+    args_params = {arg.name: arg.default_value for arg in args}
+    #print_params(args_params)
+    default_params = update_params(args_params, config_params)
+    #print_params(default_params)
+    return IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, launch_file)
+        ),
+        launch_arguments=default_params.items()
+    )
+
+
+def load_yaml_params(config_file_path):
+    config_yaml_path = config_file_path or os.path.join(get_package_share_directory(default_package_name), 'config', config_file_path)
+    config_yaml_path = get_absolute_path(config_yaml_path, package_name=default_package_name)
+    print(f"load convert Absolute config_yaml_path: {config_yaml_path}")
+    return load_yaml(config_yaml_path)
+
+
+def generate_launch_description():
+    args=generate_launch_arguments()
+    package_dir = get_package_share_directory(default_package_name)
+    launch_file_dir = os.path.join(package_dir, "launch")
+
+
+    def create_share_container(attach_component_container_name, attach_enable_condition):
+        shared_container = Node(
+            name=attach_component_container_name,
+            package='rclcpp_components',
+            executable='component_container_mt',
+            output='screen',
+            condition=IfCondition(attach_enable_condition)
+        )
+        return shared_container
+
+
+    def create_node_action(context, args):
+        attach_enable_condition = LaunchConfiguration("attach_component_container_enable", default=False).perform(context)
+        attach_component_container_name = LaunchConfiguration("attach_component_container_name", default=default_shared_container_name).perform(context)
+        use_intra_process_comms = LaunchConfiguration("use_intra_process_comms", default=False).perform(context)
+        print(f"attach_enable_condition: {attach_enable_condition}", f"attach_component_container_name: {attach_component_container_name}")
+        print(f"use_intra_process_comms: {use_intra_process_comms}")
+        shared_container_node = create_share_container(attach_component_container_name, attach_enable_condition)
+
+        config_file_path = LaunchConfiguration("config_file_path", default=default_multi_camera_config_file).perform(context)
+        print(f"config_file_path: {config_file_path}")
+        #camera_config_file = os.path.join(package_dir, "config", config_file_path)
+        config_params = load_yaml_params(config_file_path)
+        if not config_params:
+            raise Exception("Failed to load multi-camera configuration.")
+
+        camera_params = config_params['orbbec_ros']['multi_camera_parameters']
+        camera_actions = []
+        for camera_key in camera_params:
+            if camera_key.startswith('launch_device_camera'):
+                camera_config = camera_params[camera_key]
+                #print_params(camera_config)
+                period_value = float(camera_config.pop('period', '0.0'))
+                print(f"period_value: {period_value}", f"camera_config: {camera_config}")
+
+                camera_node = generate_camera_node(args, launch_file_dir, camera_config)
+                camera_action = TimerAction(period=period_value, actions=[camera_node])
+                camera_actions.append(camera_action)
+
+        nodes = [
+                shared_container_node,
+                *camera_actions,
+            ]
+        return nodes
+
+    return LaunchDescription(
+        args + [
+            OpaqueFunction(function=lambda context: create_node_action(context, args))
+        ]
+    )