Add function to process xacro macros.

flake.nix

@@ -3,6 +3,7 @@
 
   inputs = {
     gepetto.url = "github:gepetto/nix";
+    flakoboros.follows = "gepetto/flakoboros";
     gazebros2nix.follows = "gepetto/gazebros2nix";
     flake-parts.follows = "gepetto/flake-parts";
     nixpkgs.follows = "gepetto/nixpkgs";
@@ -24,9 +25,9 @@
         imports = [
           inputs.gepetto.flakeModule
           {
-            gazebros2nix = {
+            flakoboros = {
               overlays = [ inputs.hpp-manipulation.overlays.default ];
-              pyOverrides.hpp-python =
+              pyOverrideAttrs.hpp-python =
                 _final: python-final:
                 (super: {
                   propagatedBuildInputs = super.propagatedBuildInputs ++ [

src/CMakeLists.txt

@@ -171,3 +171,7 @@ add_python_library(
   pyhpp/manipulation/urdf FILES pyhpp/manipulation/urdf/util.cc
   pyhpp/manipulation/urdf/bindings.cc LINK_LIBRARIES
   hpp-manipulation-urdf::hpp-manipulation-urdf)
+
+# Install tool submodule
+python_install_on_site(pyhpp/tools __init__.py)
+python_install_on_site(pyhpp/tools xacro.py)

src/pyhpp/tools/__init__.py

@@ -0,0 +1 @@
+from .xacro import process_xacro, retrieve_resource  # noqa: F401

src/pyhpp/tools/xacro.py

@@ -0,0 +1,76 @@
+import os
+import sys
+import xml
+
+
+try:  # python 2
+    _basestr = basestring
+    encoding = {"encoding": "utf-8"}
+except NameError:  # python 3
+    _basestr = str
+    unicode = str
+    encoding = {}
+
+
+def process_xacro(*args):
+    import xacro
+
+    opts, input_file_name = xacro.process_args(args)
+    try:
+        # If AMENT_PREFIX_PATH is defined (ROS 2), pass the content to retrieve resources
+        ament_prefix_path = os.getenv("AMENT_PREFIX_PATH")
+        if ament_prefix_path is not None:
+            dirs = list(map(lambda s: s + "/share", ament_prefix_path.split(":")))
+        # open and process file
+        doc = xacro.process_file(retrieve_resource(input_file_name, dirs), **vars(opts))
+
+    # error handling
+    except xml.parsers.expat.ExpatError as e:
+        xacro.error(f"XML parsing error: {unicode(e)}", alt_text=None)
+        if xacro.verbosity > 0:
+            xacro.print_location()
+            print(file=sys.stderr)  # add empty separator line before error
+            print("Check that:", file=sys.stderr)
+            print(" - Your XML is well-formed", file=sys.stderr)
+            print(
+                " - You have the xacro xmlns declaration:",
+                'xmlns:xacro="http://www.ros.org/wiki/xacro"',
+                file=sys.stderr,
+            )
+        sys.exit(2)  # indicate failure, but don't print stack trace on XML errors
+
+    except Exception as e:
+        msg = unicode(e)
+        if not msg:
+            msg = repr(e)
+        xacro.error(msg)
+        if xacro.verbosity > 0:
+            xacro.print_location()
+        if xacro.verbosity > 1:
+            print(file=sys.stderr)  # add empty separator line before error
+            raise  # create stack trace
+        else:
+            sys.exit(2)  # gracefully exit with error condition
+
+    # write output
+    return doc.toprettyxml(indent="  ", **encoding)
+
+
+def retrieve_resource(path, dirs=None, env_var="ROS_PACKAGE_PATH"):
+    """
+    Retrieve resource of the form "package://", resolving the package in the list of
+    dirs.
+    If the list of dirs is None, it is initialized with
+    the content of the environnement variable env_var.
+    """
+    if path.startswith("package://"):
+        relpath = path[len("package://") :]
+        import os
+
+        if dirs is None:
+            dirs = os.environ[env_var].split(":")
+        for dir in dirs:
+            abspath = os.path.join(dir, relpath)
+            if os.path.isfile(abspath):
+                return abspath
+    return path