move xacro_args loader to utils

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)