From ba1f5063e9e6efd420265bbc816dd11f2f0c5281 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 18 Mar 2024 16:30:43 +0100 Subject: [PATCH 1/5] got external yaml file to be entered in launch --- crazyflie/launch/launch.py | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/crazyflie/launch/launch.py b/crazyflie/launch/launch.py index f20f49550..e53a4ec90 100644 --- a/crazyflie/launch/launch.py +++ b/crazyflie/launch/launch.py @@ -10,6 +10,9 @@ def generate_launch_description(): + + server_yaml_file = LaunchConfiguration('server_yaml_file') + # load crazyflies crazyflies_yaml = os.path.join( get_package_share_directory('crazyflie'), @@ -62,6 +65,11 @@ def generate_launch_description(): # copy relevent settings to server params server_params[1]["poses_qos_deadline"] = motion_capture_params["topics"]["poses"]["qos"]["deadline"] + #print as yaml + print(yaml.dump(server_params, default_flow_style=False, sort_keys=False)) + #with open('/data.yml', 'w') as outfile: + # yaml.dump(server_params, outfile, default_flow_style=False, sort_keys=False) + # teleop params teleop_params = os.path.join( get_package_share_directory('crazyflie'), @@ -72,7 +80,8 @@ def generate_launch_description(): DeclareLaunchArgument('backend', default_value='cpp'), DeclareLaunchArgument('debug', default_value='False'), DeclareLaunchArgument('rviz', default_value='False'), - DeclareLaunchArgument('gui', default_value='True'), + DeclareLaunchArgument('gui', default_value='False'), + DeclareLaunchArgument('server_yaml_file', default_value="test.yml"), Node( package='motion_capture_tracking', executable='motion_capture_tracking_node', @@ -107,7 +116,7 @@ def generate_launch_description(): condition=LaunchConfigurationEquals('backend','cflib'), name='crazyflie_server', output='screen', - parameters=server_params + parameters=[server_yaml_file] ), Node( package='crazyflie', @@ -115,7 +124,7 @@ def generate_launch_description(): condition=LaunchConfigurationEquals('backend','cpp'), name='crazyflie_server', output='screen', - parameters=server_params, + parameters=[server_yaml_file], prefix=PythonExpression(['"xterm -e gdb -ex run --args" if ', LaunchConfiguration('debug'), ' else ""']), ), Node( @@ -125,7 +134,7 @@ def generate_launch_description(): name='crazyflie_server', output='screen', emulate_tty=True, - parameters=server_params + parameters=[server_yaml_file] ), Node( condition=LaunchConfigurationEquals('rviz', 'True'), From 075de4e0492a1e030b8e09afb6f9fa587a206132 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 18 Mar 2024 17:28:24 +0100 Subject: [PATCH 2/5] used python expression to choose parameter file --- crazyflie/config/crazyflies.yaml | 4 ++-- crazyflie/launch/launch.py | 29 +++++++++++++++++++++++------ 2 files changed, 25 insertions(+), 8 deletions(-) diff --git a/crazyflie/config/crazyflies.yaml b/crazyflie/config/crazyflies.yaml index 5a49a0e0e..1c13f9ebd 100644 --- a/crazyflie/config/crazyflies.yaml +++ b/crazyflie/config/crazyflies.yaml @@ -3,7 +3,7 @@ robots: cf231: enabled: true uri: radio://0/80/2M/E7E7E7E7E7 - initial_position: [0, 0, 0] + initial_position: [0.0, 0.0, 0.0] type: cf21 # see robot_types # firmware_params: # kalman: @@ -17,7 +17,7 @@ robots: cf5: enabled: false uri: radio://0/80/2M/E7E7E7E705 - initial_position: [0, -0.5, 0] + initial_position: [0.0, -0.5, 0.0] type: cf21 # see robot_types # firmware_params: # kalman: diff --git a/crazyflie/launch/launch.py b/crazyflie/launch/launch.py index e53a4ec90..d5bf20c74 100644 --- a/crazyflie/launch/launch.py +++ b/crazyflie/launch/launch.py @@ -33,6 +33,14 @@ def generate_launch_description(): server_params = [crazyflies] + [server_yaml_contents["/crazyflie_server"]["ros__parameters"]] + + + server_yaml_contents["/crazyflie_server"]["ros__parameters"]['robots'] = crazyflies['robots'] + server_yaml_contents["/crazyflie_server"]["ros__parameters"]['robot_types'] = crazyflies['robot_types'] + server_yaml_contents["/crazyflie_server"]["ros__parameters"]['all'] = crazyflies['all'] + + #print(server_params) + # robot description urdf = os.path.join( get_package_share_directory('crazyflie'), @@ -41,6 +49,8 @@ def generate_launch_description(): with open(urdf, 'r') as f: robot_desc = f.read() server_params[1]["robot_description"] = robot_desc + server_yaml_contents["/crazyflie_server"]["ros__parameters"]["robot_description"] = robot_desc + # construct motion_capture_configuration motion_capture_yaml = os.path.join( @@ -65,10 +75,17 @@ def generate_launch_description(): # copy relevent settings to server params server_params[1]["poses_qos_deadline"] = motion_capture_params["topics"]["poses"]["qos"]["deadline"] + server_yaml_contents["/crazyflie_server"]["ros__parameters"]["poses_qos_deadline"] = motion_capture_params["topics"]["poses"]["qos"]["deadline"] + + #print(server_yaml_contents) #print as yaml - print(yaml.dump(server_params, default_flow_style=False, sort_keys=False)) - #with open('/data.yml', 'w') as outfile: - # yaml.dump(server_params, outfile, default_flow_style=False, sort_keys=False) + print('hello') + print(yaml.dump(server_yaml_contents, default_flow_style=False, sort_keys=False)) + print('hello2') + + yaml_file = yaml.dump(server_params, default_flow_style=False, sort_keys=False) + with open('temp.yml', 'w') as outfile: + yaml.dump(server_yaml_contents, outfile, default_flow_style=False, sort_keys=False) # teleop params teleop_params = os.path.join( @@ -80,8 +97,8 @@ def generate_launch_description(): DeclareLaunchArgument('backend', default_value='cpp'), DeclareLaunchArgument('debug', default_value='False'), DeclareLaunchArgument('rviz', default_value='False'), - DeclareLaunchArgument('gui', default_value='False'), - DeclareLaunchArgument('server_yaml_file', default_value="test.yml"), + DeclareLaunchArgument('gui', default_value='True'), + DeclareLaunchArgument('server_yaml_file', default_value=''), Node( package='motion_capture_tracking', executable='motion_capture_tracking_node', @@ -134,7 +151,7 @@ def generate_launch_description(): name='crazyflie_server', output='screen', emulate_tty=True, - parameters=[server_yaml_file] + parameters= [PythonExpression(["'temp.yml' if '", LaunchConfiguration('server_yaml_file'), "' == '' else '", LaunchConfiguration('server_yaml_file'), "'"])], ), Node( condition=LaunchConfigurationEquals('rviz', 'True'), From 062c89fa1ae59f3472c604ed23a9d1c0f64e9f70 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Mon, 18 Mar 2024 18:48:14 +0100 Subject: [PATCH 3/5] clean up --- crazyflie/launch/launch.py | 29 ++++++----------------------- 1 file changed, 6 insertions(+), 23 deletions(-) diff --git a/crazyflie/launch/launch.py b/crazyflie/launch/launch.py index d5bf20c74..487febb03 100644 --- a/crazyflie/launch/launch.py +++ b/crazyflie/launch/launch.py @@ -11,8 +11,6 @@ def generate_launch_description(): - server_yaml_file = LaunchConfiguration('server_yaml_file') - # load crazyflies crazyflies_yaml = os.path.join( get_package_share_directory('crazyflie'), @@ -31,27 +29,20 @@ def generate_launch_description(): with open(server_yaml, 'r') as ymlfile: server_yaml_contents = yaml.safe_load(ymlfile) - server_params = [crazyflies] + [server_yaml_contents["/crazyflie_server"]["ros__parameters"]] - - - server_yaml_contents["/crazyflie_server"]["ros__parameters"]['robots'] = crazyflies['robots'] server_yaml_contents["/crazyflie_server"]["ros__parameters"]['robot_types'] = crazyflies['robot_types'] server_yaml_contents["/crazyflie_server"]["ros__parameters"]['all'] = crazyflies['all'] - #print(server_params) - # 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_params[1]["robot_description"] = robot_desc server_yaml_contents["/crazyflie_server"]["ros__parameters"]["robot_description"] = robot_desc - # construct motion_capture_configuration motion_capture_yaml = os.path.join( get_package_share_directory('crazyflie'), @@ -73,18 +64,10 @@ def generate_launch_description(): } # copy relevent settings to server params - server_params[1]["poses_qos_deadline"] = motion_capture_params["topics"]["poses"]["qos"]["deadline"] - server_yaml_contents["/crazyflie_server"]["ros__parameters"]["poses_qos_deadline"] = motion_capture_params["topics"]["poses"]["qos"]["deadline"] - #print(server_yaml_contents) - #print as yaml - print('hello') - print(yaml.dump(server_yaml_contents, default_flow_style=False, sort_keys=False)) - print('hello2') - - yaml_file = yaml.dump(server_params, default_flow_style=False, sort_keys=False) - with open('temp.yml', 'w') as outfile: + # Save in temp file such that nodes can read it out later + with open('tmp.yaml', 'w') as outfile: yaml.dump(server_yaml_contents, outfile, default_flow_style=False, sort_keys=False) # teleop params @@ -133,7 +116,7 @@ def generate_launch_description(): condition=LaunchConfigurationEquals('backend','cflib'), name='crazyflie_server', output='screen', - parameters=[server_yaml_file] + parameters= [PythonExpression(["'tmp.yaml' if '", LaunchConfiguration('server_yaml_file'), "' == '' else '", LaunchConfiguration('server_yaml_file'), "'"])], ), Node( package='crazyflie', @@ -141,7 +124,7 @@ def generate_launch_description(): condition=LaunchConfigurationEquals('backend','cpp'), name='crazyflie_server', output='screen', - parameters=[server_yaml_file], + parameters= [PythonExpression(["'tmp.yaml' if '", LaunchConfiguration('server_yaml_file'), "' == '' else '", LaunchConfiguration('server_yaml_file'), "'"])], prefix=PythonExpression(['"xterm -e gdb -ex run --args" if ', LaunchConfiguration('debug'), ' else ""']), ), Node( @@ -151,7 +134,7 @@ def generate_launch_description(): name='crazyflie_server', output='screen', emulate_tty=True, - parameters= [PythonExpression(["'temp.yml' if '", LaunchConfiguration('server_yaml_file'), "' == '' else '", LaunchConfiguration('server_yaml_file'), "'"])], + parameters= [PythonExpression(["'tmp.yaml' if '", LaunchConfiguration('server_yaml_file'), "' == '' else '", LaunchConfiguration('server_yaml_file'), "'"])], ), Node( condition=LaunchConfigurationEquals('rviz', 'True'), From 8974aa9224ee2a9537a49e1d50a6afd6f593f6af Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 9 Apr 2024 11:10:31 +0200 Subject: [PATCH 4/5] implement param load for mocap and teleop --- 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 487febb03..a7ae026d8 100644 --- a/crazyflie/launch/launch.py +++ b/crazyflie/launch/launch.py @@ -50,31 +50,28 @@ def generate_launch_description(): 'motion_capture.yaml') with open(motion_capture_yaml, 'r') as ymlfile: - motion_capture = yaml.safe_load(ymlfile) + motion_capture_contents = yaml.safe_load(ymlfile) - motion_capture_params = motion_capture["/motion_capture_tracking"]["ros__parameters"] - motion_capture_params["rigid_bodies"] = dict() + motion_capture_contents["/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_params["rigid_bodies"][key] = { + motion_capture_contents["/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_contents["/crazyflie_server"]["ros__parameters"]["poses_qos_deadline"] = motion_capture_params["topics"]["poses"]["qos"]["deadline"] + server_yaml_contents["/crazyflie_server"]["ros__parameters"]["poses_qos_deadline"] = motion_capture_contents[ + "/motion_capture_tracking"]["ros__parameters"]["topics"]["poses"]["qos"]["deadline"] - # Save in temp file such that nodes can read it out later - with open('tmp.yaml', 'w') as outfile: + # 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_contents, outfile, default_flow_style=False, sort_keys=False) - # teleop params - teleop_params = os.path.join( - get_package_share_directory('crazyflie'), - 'config', - 'teleop.yaml') + with open('tmp_motion_capture.yaml', 'w') as outfile: + yaml.dump(motion_capture_contents, outfile, default_flow_style=False, sort_keys=False) return LaunchDescription([ DeclareLaunchArgument('backend', default_value='cpp'), @@ -82,13 +79,15 @@ def generate_launch_description(): DeclareLaunchArgument('rviz', default_value='False'), DeclareLaunchArgument('gui', 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=LaunchConfigurationNotEquals('backend','sim'), name='motion_capture_tracking', output='screen', - parameters=[motion_capture_params] + parameters= [PythonExpression(["'tmp_motion_capture.yaml' if '", LaunchConfiguration('mocap_yaml_file'), "' == '' else '", LaunchConfiguration('mocap_yaml_file'), "'"])], ), Node( package='crazyflie', @@ -103,7 +102,7 @@ def generate_launch_description(): # ('cmd_full_state', 'cf6/cmd_full_state'), # ('notify_setpoints_stop', 'cf6/notify_setpoints_stop'), ], - parameters=[teleop_params] + parameters= [PythonExpression(["'teleop.yaml' if '", LaunchConfiguration('teleop_yaml_file'), "' == '' else '", LaunchConfiguration('teleop_yaml_file'), "'"])], ), Node( package='joy', @@ -116,7 +115,7 @@ def generate_launch_description(): condition=LaunchConfigurationEquals('backend','cflib'), name='crazyflie_server', output='screen', - parameters= [PythonExpression(["'tmp.yaml' if '", LaunchConfiguration('server_yaml_file'), "' == '' else '", LaunchConfiguration('server_yaml_file'), "'"])], + parameters= [PythonExpression(["'tmp_server.yaml' if '", LaunchConfiguration('server_yaml_file'), "' == '' else '", LaunchConfiguration('server_yaml_file'), "'"])], ), Node( package='crazyflie', @@ -124,7 +123,7 @@ def generate_launch_description(): condition=LaunchConfigurationEquals('backend','cpp'), name='crazyflie_server', output='screen', - parameters= [PythonExpression(["'tmp.yaml' if '", LaunchConfiguration('server_yaml_file'), "' == '' else '", LaunchConfiguration('server_yaml_file'), "'"])], + 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( @@ -134,7 +133,7 @@ def generate_launch_description(): name='crazyflie_server', output='screen', emulate_tty=True, - parameters= [PythonExpression(["'tmp.yaml' if '", LaunchConfiguration('server_yaml_file'), "' == '' else '", LaunchConfiguration('server_yaml_file'), "'"])], + parameters= [PythonExpression(["'tmp_server.yaml' if '", LaunchConfiguration('server_yaml_file'), "' == '' else '", LaunchConfiguration('server_yaml_file'), "'"])], ), Node( condition=LaunchConfigurationEquals('rviz', 'True'), From a5cf5b7036e1fc6d8940c708af439d731a14feac Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 9 Apr 2024 11:13:27 +0200 Subject: [PATCH 5/5] variable change --- crazyflie/launch/launch.py | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/crazyflie/launch/launch.py b/crazyflie/launch/launch.py index a7ae026d8..d5df4b37d 100644 --- a/crazyflie/launch/launch.py +++ b/crazyflie/launch/launch.py @@ -27,11 +27,11 @@ def generate_launch_description(): 'server.yaml') with open(server_yaml, 'r') as ymlfile: - server_yaml_contents = yaml.safe_load(ymlfile) + server_yaml_content = yaml.safe_load(ymlfile) - server_yaml_contents["/crazyflie_server"]["ros__parameters"]['robots'] = crazyflies['robots'] - server_yaml_contents["/crazyflie_server"]["ros__parameters"]['robot_types'] = crazyflies['robot_types'] - server_yaml_contents["/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( @@ -41,7 +41,7 @@ def generate_launch_description(): with open(urdf, 'r') as f: robot_desc = f.read() - server_yaml_contents["/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( @@ -50,28 +50,28 @@ def generate_launch_description(): 'motion_capture.yaml') with open(motion_capture_yaml, 'r') as ymlfile: - motion_capture_contents = yaml.safe_load(ymlfile) + motion_capture_content = yaml.safe_load(ymlfile) - motion_capture_contents["/motion_capture_tracking"]["ros__parameters"]["rigid_bodies"] = dict() + 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_contents["/motion_capture_tracking"]["ros__parameters"]["rigid_bodies"][key] = { + 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_contents["/crazyflie_server"]["ros__parameters"]["poses_qos_deadline"] = motion_capture_contents[ + 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_contents, outfile, default_flow_style=False, sort_keys=False) + 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_contents, outfile, default_flow_style=False, sort_keys=False) + yaml.dump(motion_capture_content, outfile, default_flow_style=False, sort_keys=False) return LaunchDescription([ DeclareLaunchArgument('backend', default_value='cpp'),