From 48803e1658b4e61559af03aa34b84f3c9a2f0ecd Mon Sep 17 00:00:00 2001 From: knmcguire Date: Fri, 19 Apr 2024 15:46:46 +0200 Subject: [PATCH] fix yaml handling --- .../multiranger_simple_mapper_launch.py | 36 +++++++++++++++++-- 1 file changed, 33 insertions(+), 3 deletions(-) diff --git a/crazyflie_examples/launch/multiranger_simple_mapper_launch.py b/crazyflie_examples/launch/multiranger_simple_mapper_launch.py index d8e4b4952..990ed7a9c 100644 --- a/crazyflie_examples/launch/multiranger_simple_mapper_launch.py +++ b/crazyflie_examples/launch/multiranger_simple_mapper_launch.py @@ -4,9 +4,11 @@ from launch import LaunchDescription from launch_ros.actions import Node import yaml - +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PythonExpression def generate_launch_description(): + # load crazyflies crazyflies_yaml = os.path.join( get_package_share_directory('crazyflie'), @@ -16,17 +18,45 @@ def generate_launch_description(): with open(crazyflies_yaml, 'r') as ymlfile: crazyflies = yaml.safe_load(ymlfile) - server_params = crazyflies + # server params + server_yaml = os.path.join( + get_package_share_directory('crazyflie'), + 'config', + 'server.yaml') + + with open(server_yaml, 'r') as ymlfile: + server_yaml_content = yaml.safe_load(ymlfile) + + server_yaml_content["/crazyflie_server"]["ros__parameters"]['robots'] = crazyflies['robots'] + server_yaml_content["/crazyflie_server"]["ros__parameters"]['robot_types'] = crazyflies['robot_types'] + server_yaml_content["/crazyflie_server"]["ros__parameters"]['all'] = crazyflies['all'] + + # robot description + urdf = os.path.join( + get_package_share_directory('crazyflie'), + 'urdf', + 'crazyflie_description.urdf') + with open(urdf, 'r') as f: + + robot_desc = f.read() + server_yaml_content["/crazyflie_server"]["ros__parameters"]["robot_description"] = robot_desc + + # Save server and mocap in temp file such that nodes can read it out later + with open('tmp_server.yaml', 'w') as outfile: + yaml.dump(server_yaml_content, outfile, default_flow_style=False, sort_keys=False) + crazyflie_name = '/cf231' + return LaunchDescription([ + DeclareLaunchArgument('server_yaml_file', default_value=''), Node( package='crazyflie', executable='crazyflie_server.py', name='crazyflie_server', output='screen', - parameters=[server_params], + parameters= [PythonExpression(["'tmp_server.yaml' if '", LaunchConfiguration('server_yaml_file'), "' == '' else '", LaunchConfiguration('server_yaml_file'), "'"])], ), Node( package='crazyflie',