fix yaml handling

IMRCLab · Apr 19, 2024 · 48803e1 · 48803e1
1 parent bf00cb7
commit 48803e1
Showing 1 changed file with 33 additions and 3 deletions.
diff --git a/crazyflie_examples/launch/multiranger_simple_mapper_launch.py b/crazyflie_examples/launch/multiranger_simple_mapper_launch.py
@@ -4,9 +4,11 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
 import yaml
-
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PythonExpression
 
 def generate_launch_description():
+
     # load crazyflies
     crazyflies_yaml = os.path.join(
         get_package_share_directory('crazyflie'),
@@ -16,17 +18,45 @@ def generate_launch_description():
     with open(crazyflies_yaml, 'r') as ymlfile:
         crazyflies = yaml.safe_load(ymlfile)
 
-    server_params = crazyflies
+    # server params
+    server_yaml = os.path.join(
+        get_package_share_directory('crazyflie'),
+        'config',
+        'server.yaml')
+
+    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']
+
+    # 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_yaml_content["/crazyflie_server"]["ros__parameters"]["robot_description"] = robot_desc
+
+    # 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)
+
 
     crazyflie_name = '/cf231'
 
+
     return LaunchDescription([
+        DeclareLaunchArgument('server_yaml_file', default_value=''),
         Node(
             package='crazyflie',
             executable='crazyflie_server.py',
             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',