53 lines
1.7 KiB
Python
53 lines
1.7 KiB
Python
import os
|
|
import yaml
|
|
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetEnvironmentVariable
|
|
from launch.substitutions import LaunchConfiguration
|
|
from launch_ros.actions import Node
|
|
|
|
|
|
def create_nodes(context):
|
|
package_dir = get_package_share_directory('odin_ros_driver')
|
|
config_file = LaunchConfiguration('config_file').perform(context)
|
|
|
|
with open(config_file, 'r', encoding='utf-8') as stream:
|
|
reprojection_params = yaml.safe_load(stream) or {}
|
|
|
|
reprojection_params['calib_file_path'] = os.path.join(package_dir, 'config', 'calib.yaml')
|
|
|
|
pcd2depth_node = Node(
|
|
package='odin_ros_driver',
|
|
executable='pcd2depth_ros2_node',
|
|
name='pcd2depth_ros2_node',
|
|
output='screen',
|
|
parameters=[reprojection_params]
|
|
)
|
|
|
|
cloud_reprojection_node = Node(
|
|
package='odin_ros_driver',
|
|
executable='cloud_reprojection_ros2_node',
|
|
name='cloud_reprojection_ros2_node',
|
|
output='screen',
|
|
parameters=[reprojection_params]
|
|
)
|
|
|
|
return [pcd2depth_node, cloud_reprojection_node]
|
|
|
|
|
|
def generate_launch_description():
|
|
package_dir = get_package_share_directory('odin_ros_driver')
|
|
|
|
config_file_arg = DeclareLaunchArgument(
|
|
'config_file',
|
|
default_value=os.path.join(package_dir, 'config', 'control_command.yaml'),
|
|
description='Path to the control config YAML file'
|
|
)
|
|
|
|
ld = LaunchDescription()
|
|
ld.add_action(SetEnvironmentVariable('RCUTILS_COLORIZED_OUTPUT', '1'))
|
|
ld.add_action(config_file_arg)
|
|
ld.add_action(OpaqueFunction(function=lambda context: create_nodes(context)))
|
|
return ld
|