move xacro_args loader to utils

env_manager/rbs_runtime/launch/runtime.launch.py

@@ -17,6 +17,7 @@ from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 from robot_builder.external.ros2_control import ControllerManager
 from robot_builder.parser.urdf import URDF_parser
+from rbs_utils.launch import load_xacro_args
 
 
 def launch_setup(context, *args, **kwargs):
@@ -65,33 +66,6 @@ def launch_setup(context, *args, **kwargs):
         description_package_abs_path, "urdf", "xacro_args.yaml"
     )
 
-    # TODO: hide this to another place
-    # Load xacro_args
-    def param_constructor(loader, node, local_vars):
-        value = loader.construct_scalar(node)
-        return LaunchConfiguration(value).perform(
-            local_vars.get("context", "Launch context if not defined")
-        )
-
-    def variable_constructor(loader, node, local_vars):
-        value = loader.construct_scalar(node)
-        return local_vars.get(value, f"Variable '{value}' not found")
-
-    def load_xacro_args(yaml_file, local_vars):
-        # Get valut from ros2 argument
-        yaml.add_constructor(
-            "!param", lambda loader, node: param_constructor(loader, node, local_vars)
-        )
-
-        # Get value from local variable in this code
-        # The local variable should be initialized before the loader was called
-        yaml.add_constructor(
-            "!variable",
-            lambda loader, node: variable_constructor(loader, node, local_vars),
-        )
-
-        with open(yaml_file, "r") as file:
-            return yaml.load(file, Loader=yaml.FullLoader)
 
     mappings_data = load_xacro_args(xacro_config_file, locals())
 

rbs_utils/rbs_utils/rbs_utils/launch.py

@@ -0,0 +1,28 @@
+import yaml
+from launch.substitutions import LaunchConfiguration
+
+def param_constructor(loader, node, local_vars):
+    value = loader.construct_scalar(node)
+    return LaunchConfiguration(value).perform(
+        local_vars.get("context", "Launch context if not defined")
+    )
+
+def variable_constructor(loader, node, local_vars):
+    value = loader.construct_scalar(node)
+    return local_vars.get(value, f"Variable '{value}' not found")
+
+def load_xacro_args(yaml_file, local_vars):
+    # Get valut from ros2 argument
+    yaml.add_constructor(
+        "!param", lambda loader, node: param_constructor(loader, node, local_vars)
+    )
+
+    # Get value from local variable in this code
+    # The local variable should be initialized before the loader was called
+    yaml.add_constructor(
+        "!variable",
+        lambda loader, node: variable_constructor(loader, node, local_vars),
+    )
+
+    with open(yaml_file, "r") as file:
+        return yaml.load(file, Loader=yaml.FullLoader)