From 3fda7ebdf4a3709e4f0099f3a46e8e5c2d812f6f Mon Sep 17 00:00:00 2001 From: Christian Llanes Date: Fri, 28 Jun 2024 13:59:00 -0400 Subject: [PATCH 1/4] switch from " to ' to be more consistent --- crazyflie/launch/launch.py | 33 ++++++++++++++++----------------- 1 file changed, 16 insertions(+), 17 deletions(-) diff --git a/crazyflie/launch/launch.py b/crazyflie/launch/launch.py index 8c4264c1c..df4619ab6 100644 --- a/crazyflie/launch/launch.py +++ b/crazyflie/launch/launch.py @@ -22,10 +22,9 @@ 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_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( @@ -36,7 +35,7 @@ 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_yaml_content['/crazyflie_server']['ros__parameters']['robot_description'] = robot_desc # construct motion_capture_configuration motion_capture_yaml = os.path.join( @@ -46,27 +45,27 @@ def parse_yaml(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_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'], } # 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"] - + 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) - + def generate_launch_description(): default_crazyflies_yaml_path = os.path.join( From 237bb3f9a0e0f9a351a4293237e786cf0bf40517 Mon Sep 17 00:00:00 2001 From: Christian Llanes Date: Fri, 28 Jun 2024 14:01:23 -0400 Subject: [PATCH 2/4] remove temp yaml files generated for server and motion capture --- crazyflie/launch/launch.py | 81 ++++++++++++++++++-------------------- 1 file changed, 38 insertions(+), 43 deletions(-) diff --git a/crazyflie/launch/launch.py b/crazyflie/launch/launch.py index df4619ab6..df62d2d75 100644 --- a/crazyflie/launch/launch.py +++ b/crazyflie/launch/launch.py @@ -59,13 +59,44 @@ def parse_yaml(context): 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) + server_parameters = server_yaml_content['/crazyflie_server']['ros__parameters'] + motion_capture_parameters = motion_capture_content['/motion_capture_tracking']['ros__parameters'] - with open('tmp_motion_capture.yaml', 'w') as outfile: - yaml.dump(motion_capture_content, outfile, default_flow_style=False, sort_keys=False) - + 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_parameters], + ), + Node( + package='crazyflie', + executable='crazyflie_server.py', + condition=LaunchConfigurationEquals('backend','cflib'), + name='crazyflie_server', + output='screen', + parameters= [server_parameters], + ), + Node( + package='crazyflie', + executable='crazyflie_server', + condition=LaunchConfigurationEquals('backend','cpp'), + name='crazyflie_server', + output='screen', + parameters= [server_parameters], + 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_parameters], + )] def generate_launch_description(): default_crazyflies_yaml_path = os.path.join( @@ -81,23 +112,13 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('crazyflies_yaml_file', default_value=default_crazyflies_yaml_path), - OpaqueFunction(function=parse_yaml), 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', @@ -119,32 +140,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', From 62e69b1d2ead1d6efcfc54b83cfdd098ae20f6c7 Mon Sep 17 00:00:00 2001 From: Christian Llanes Date: Fri, 28 Jun 2024 14:50:59 -0400 Subject: [PATCH 3/4] clean up node and ros__parameters prefix --- crazyflie/launch/launch.py | 26 +++++++++++--------------- 1 file changed, 11 insertions(+), 15 deletions(-) diff --git a/crazyflie/launch/launch.py b/crazyflie/launch/launch.py index df62d2d75..51ae3721b 100644 --- a/crazyflie/launch/launch.py +++ b/crazyflie/launch/launch.py @@ -22,10 +22,8 @@ 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'), @@ -35,33 +33,31 @@ 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') + 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() + 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_content['/motion_capture_tracking']['ros__parameters']['rigid_bodies'][key] = { + 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'] + server_params[1]['poses_qos_deadline'] = motion_capture_params['topics']['poses']['qos']['deadline'] - server_parameters = server_yaml_content['/crazyflie_server']['ros__parameters'] - motion_capture_parameters = motion_capture_content['/motion_capture_tracking']['ros__parameters'] - return [ Node( package='motion_capture_tracking', @@ -69,7 +65,7 @@ def parse_yaml(context): condition=IfCondition(PythonExpression(["'", LaunchConfiguration('backend'), "' != 'sim' and '", LaunchConfiguration('mocap'), "' == 'True'"])), name='motion_capture_tracking', output='screen', - parameters= [motion_capture_parameters], + parameters= [motion_capture_params], ), Node( package='crazyflie', @@ -77,7 +73,7 @@ def parse_yaml(context): condition=LaunchConfigurationEquals('backend','cflib'), name='crazyflie_server', output='screen', - parameters= [server_parameters], + parameters= server_params, ), Node( package='crazyflie', @@ -85,7 +81,7 @@ def parse_yaml(context): condition=LaunchConfigurationEquals('backend','cpp'), name='crazyflie_server', output='screen', - parameters= [server_parameters], + parameters= server_params, prefix=PythonExpression(['"xterm -e gdb -ex run --args" if ', LaunchConfiguration('debug'), ' else ""']), ), Node( @@ -95,7 +91,7 @@ def parse_yaml(context): name='crazyflie_server', output='screen', emulate_tty=True, - parameters= [server_parameters], + parameters= server_params, )] def generate_launch_description(): From ee46445e1f31f975a47eae1d0317f5d4300f721a Mon Sep 17 00:00:00 2001 From: Christian Llanes Date: Fri, 28 Jun 2024 14:59:11 -0400 Subject: [PATCH 4/4] add ability for user to specify their own motion capture yaml file --- crazyflie/launch/launch.py | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/crazyflie/launch/launch.py b/crazyflie/launch/launch.py index 51ae3721b..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 @@ -36,11 +36,7 @@ def parse_yaml(context): 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) @@ -99,6 +95,11 @@ def generate_launch_description(): 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'), @@ -108,6 +109,8 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('crazyflies_yaml_file', default_value=default_crazyflies_yaml_path), + 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'),