From 6b376f40a6cbd4bbed1a44477eeeb13a9d815e74 Mon Sep 17 00:00:00 2001
From: Bill Finger <ur.narmak@gmail.com>
Date: Thu, 6 Feb 2025 15:21:00 +0300
Subject: [PATCH] move xacro_args loader to utils

---
 .../rbs_runtime/launch/runtime.launch.py      | 28 +------------------
 rbs_utils/rbs_utils/rbs_utils/launch.py       | 28 +++++++++++++++++++
 2 files changed, 29 insertions(+), 27 deletions(-)
 create mode 100644 rbs_utils/rbs_utils/rbs_utils/launch.py

diff --git a/env_manager/rbs_runtime/launch/runtime.launch.py b/env_manager/rbs_runtime/launch/runtime.launch.py
index 6f56440..fa5387f 100644
--- a/env_manager/rbs_runtime/launch/runtime.launch.py
+++ b/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())
 
diff --git a/rbs_utils/rbs_utils/rbs_utils/launch.py b/rbs_utils/rbs_utils/rbs_utils/launch.py
new file mode 100644
index 0000000..7960199
--- /dev/null
+++ b/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)