diff --git a/crazyflie/launch/launch.py b/crazyflie/launch/launch.py index 8c4264c1c..961d1f447 100644 --- a/crazyflie/launch/launch.py +++ b/crazyflie/launch/launch.py @@ -10,8 +10,8 @@ def parse_yaml(context): # Load the crazyflies YAML file - crazyflies_yaml_file = LaunchConfiguration('crazyflies_yaml_file').perform(context) - with open(crazyflies_yaml_file, 'r') as file: + crazyflies_yaml = LaunchConfiguration('crazyflies_yaml_file').perform(context) + with open(crazyflies_yaml, 'r') as file: crazyflies = yaml.safe_load(file) # server params @@ -23,10 +23,7 @@ def parse_yaml(context): 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'] - + server_params = [crazyflies] + [server_yaml_content['/crazyflie_server']['ros__parameters']] # robot description urdf = os.path.join( get_package_share_directory('crazyflie'), @@ -36,43 +33,73 @@ def parse_yaml(context): with open(urdf, 'r') as f: robot_desc = f.read() - server_yaml_content["/crazyflie_server"]["ros__parameters"]["robot_description"] = robot_desc + server_params[1]['robot_description'] = robot_desc # construct motion_capture_configuration - motion_capture_yaml = os.path.join( - get_package_share_directory('crazyflie'), - 'config', - 'motion_capture.yaml') + motion_capture_yaml = LaunchConfiguration('motion_capture_yaml_file').perform(context) with open(motion_capture_yaml, 'r') as ymlfile: motion_capture_content = yaml.safe_load(ymlfile) - motion_capture_content["/motion_capture_tracking"]["ros__parameters"]["rigid_bodies"] = dict() - for key, value in crazyflies["robots"].items(): - type = crazyflies["robot_types"][value["type"]] - if value["enabled"] and type["motion_capture"]["enabled"]: - motion_capture_content["/motion_capture_tracking"]["ros__parameters"]["rigid_bodies"][key] = { - "initial_position": value["initial_position"], - "marker": type["motion_capture"]["marker"], - "dynamics": type["motion_capture"]["dynamics"], + motion_capture_params = motion_capture_content['/motion_capture_tracking']['ros__parameters'] + motion_capture_params['rigid_bodies'] = dict() + for key, value in crazyflies['robots'].items(): + type = crazyflies['robot_types'][value['type']] + if value['enabled'] and type['motion_capture']['enabled']: + motion_capture_params['rigid_bodies'][key] = { + 'initial_position': value['initial_position'], + 'marker': type['motion_capture']['marker'], + 'dynamics': type['motion_capture']['dynamics'], } # copy relevent settings to server params - server_yaml_content["/crazyflie_server"]["ros__parameters"]["poses_qos_deadline"] = motion_capture_content[ - "/motion_capture_tracking"]["ros__parameters"]["topics"]["poses"]["qos"]["deadline"] - - # 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) - - with open('tmp_motion_capture.yaml', 'w') as outfile: - yaml.dump(motion_capture_content, outfile, default_flow_style=False, sort_keys=False) - + server_params[1]['poses_qos_deadline'] = motion_capture_params['topics']['poses']['qos']['deadline'] + + return [ + Node( + package='motion_capture_tracking', + executable='motion_capture_tracking_node', + condition=IfCondition(PythonExpression(["'", LaunchConfiguration('backend'), "' != 'sim' and '", LaunchConfiguration('mocap'), "' == 'True'"])), + name='motion_capture_tracking', + output='screen', + parameters= [motion_capture_params], + ), + Node( + package='crazyflie', + executable='crazyflie_server.py', + condition=LaunchConfigurationEquals('backend','cflib'), + name='crazyflie_server', + output='screen', + parameters= server_params, + ), + Node( + package='crazyflie', + executable='crazyflie_server', + condition=LaunchConfigurationEquals('backend','cpp'), + name='crazyflie_server', + output='screen', + parameters= server_params, + prefix=PythonExpression(['"xterm -e gdb -ex run --args" if ', LaunchConfiguration('debug'), ' else ""']), + ), + Node( + package='crazyflie_sim', + executable='crazyflie_server', + condition=LaunchConfigurationEquals('backend','sim'), + name='crazyflie_server', + output='screen', + emulate_tty=True, + parameters= server_params, + )] def generate_launch_description(): default_crazyflies_yaml_path = os.path.join( get_package_share_directory('crazyflie'), 'config', 'crazyflies.yaml') + + default_motion_capture_yaml_path = os.path.join( + get_package_share_directory('crazyflie'), + 'config', + 'motion_capture.yaml') telop_yaml_path = os.path.join( get_package_share_directory('crazyflie'), @@ -82,23 +109,15 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('crazyflies_yaml_file', default_value=default_crazyflies_yaml_path), - OpaqueFunction(function=parse_yaml), + DeclareLaunchArgument('motion_capture_yaml_file', + default_value=default_motion_capture_yaml_path), DeclareLaunchArgument('backend', default_value='cpp'), DeclareLaunchArgument('debug', default_value='False'), DeclareLaunchArgument('rviz', default_value='False'), DeclareLaunchArgument('gui', default_value='True'), DeclareLaunchArgument('mocap', default_value='True'), - DeclareLaunchArgument('server_yaml_file', default_value=''), DeclareLaunchArgument('teleop_yaml_file', default_value=''), - DeclareLaunchArgument('mocap_yaml_file', default_value=''), - Node( - package='motion_capture_tracking', - executable='motion_capture_tracking_node', - condition=IfCondition(PythonExpression(["'", LaunchConfiguration('backend'), "' != 'sim' and '", LaunchConfiguration('mocap'), "' == 'True'"])), - name='motion_capture_tracking', - output='screen', - parameters= [PythonExpression(["'tmp_motion_capture.yaml' if '", LaunchConfiguration('mocap_yaml_file'), "' == '' else '", LaunchConfiguration('mocap_yaml_file'), "'"])], - ), + OpaqueFunction(function=parse_yaml), Node( package='crazyflie', executable='teleop', @@ -120,32 +139,6 @@ def generate_launch_description(): executable='joy_node', name='joy_node' # by default id=0 ), - Node( - package='crazyflie', - executable='crazyflie_server.py', - condition=LaunchConfigurationEquals('backend','cflib'), - name='crazyflie_server', - output='screen', - parameters= [PythonExpression(["'tmp_server.yaml' if '", LaunchConfiguration('server_yaml_file'), "' == '' else '", LaunchConfiguration('server_yaml_file'), "'"])], - ), - Node( - package='crazyflie', - executable='crazyflie_server', - condition=LaunchConfigurationEquals('backend','cpp'), - name='crazyflie_server', - output='screen', - parameters= [PythonExpression(["'tmp_server.yaml' if '", LaunchConfiguration('server_yaml_file'), "' == '' else '", LaunchConfiguration('server_yaml_file'), "'"])], - prefix=PythonExpression(['"xterm -e gdb -ex run --args" if ', LaunchConfiguration('debug'), ' else ""']), - ), - Node( - package='crazyflie_sim', - executable='crazyflie_server', - condition=LaunchConfigurationEquals('backend','sim'), - name='crazyflie_server', - output='screen', - emulate_tty=True, - parameters= [PythonExpression(["'tmp_server.yaml' if '", LaunchConfiguration('server_yaml_file'), "' == '' else '", LaunchConfiguration('server_yaml_file'), "'"])], - ), Node( condition=LaunchConfigurationEquals('rviz', 'True'), package='rviz2',