server yaml file substitute with temporary files (#454)

* got external yaml file to be entered in launch

* used python expression to choose parameter file

* implement param load for mocap and teleop

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:

crazyflie/launch/launch.py

@@ -10,6 +10,7 @@
 
 
 def generate_launch_description():
+
     # load crazyflies
     crazyflies_yaml = os.path.join(
         get_package_share_directory('crazyflie'),
@@ -26,18 +27,21 @@ 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_params = [crazyflies] + [server_yaml_contents["/crazyflie_server"]["ros__parameters"]]
+    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_params[1]["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,40 +50,44 @@ def generate_launch_description():
         'motion_capture.yaml')
 
     with open(motion_capture_yaml, 'r') as ymlfile:
-        motion_capture = yaml.safe_load(ymlfile)
+        motion_capture_content = yaml.safe_load(ymlfile)
 
-    motion_capture_params = motion_capture["/motion_capture_tracking"]["ros__parameters"]
-    motion_capture_params["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_params["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_params[1]["poses_qos_deadline"] = motion_capture_params["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"]
 
-    # teleop params
-    teleop_params = os.path.join(
-        get_package_share_directory('crazyflie'),
-        'config',
-        'teleop.yaml')
+    # 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)
 
     return LaunchDescription([
         DeclareLaunchArgument('backend', default_value='cpp'),
         DeclareLaunchArgument('debug', default_value='False'),
         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',
@@ -94,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',
@@ -107,15 +115,15 @@ def generate_launch_description():
             condition=LaunchConfigurationEquals('backend','cflib'),
             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',
             executable='crazyflie_server',
             condition=LaunchConfigurationEquals('backend','cpp'),
             name='crazyflie_server',
             output='screen',
-            parameters=server_params,
+            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(
@@ -125,7 +133,7 @@ def generate_launch_description():
             name='crazyflie_server',
             output='screen',
             emulate_tty=True,
-            parameters=server_params
+            parameters= [PythonExpression(["'tmp_server.yaml' if '", LaunchConfiguration('server_yaml_file'), "' == '' else '", LaunchConfiguration('server_yaml_file'), "'"])],
         ),
         Node(
             condition=LaunchConfigurationEquals('rviz', 'True'),