Merge pull request #538 from llanesc/pr-remove-tmp-yaml

remove tmp yaml files generated for server and motion capture. Add motion capture yaml import feature.

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',