Xacro support in URDFLoader

include/threepp/loaders/URDFLoader.hpp

@@ -6,7 +6,9 @@
 #include "threepp/objects/Robot.hpp"
 
 #include <filesystem>
+#include <map>
 #include <memory>
+#include <string>
 
 namespace threepp {
 
@@ -20,6 +22,10 @@ namespace threepp {
         // By default, the loader uses ModelLoader to load geometry files referenced in the URDF.
         URDFLoader& setGeometryLoader(std::shared_ptr<Loader<Group>> loader);
 
+        // Set xacro argument overrides, equivalent to passing `name:=value` on the xacro CLI.
+        // These override <xacro:arg default="..."/> values and are available as ${name} / $(arg name).
+        URDFLoader& setArgs(std::map<std::string, std::string> args);
+
         std::shared_ptr<Robot> load(const std::filesystem::path& path);
 
         std::shared_ptr<Robot> parse(const std::filesystem::path& baseDir, const std::string& xml);

src/threepp/loaders/URDFLoader.cpp

@@ -13,7 +13,10 @@
 #include "pugixml.hpp"
 #include "threepp/loaders/ModelLoader.hpp"
 
+#include <cmath>
 #include <iostream>
+#include <list>
+#include <map>
 
 
 using namespace threepp;
@@ -96,43 +99,32 @@ namespace {
                 .child = node.child("child").attribute("link").value()};
     }
 
-    std::filesystem::path getModelPath(const std::filesystem::path& basePath, std::string fileName) {
+    std::filesystem::path findPackageRoot(const std::filesystem::path& start) {
+        for (auto path = start; path != path.parent_path(); path = path.parent_path()) {
+            if (exists(path / "package.xml")) {
+                return path;
+            }
 
-        if (fileName.find("package://") != std::string::npos) {
+        }
+        return {};
+    }
 
-            fileName = fileName.substr(10);
+    std::filesystem::path getModelPath(const std::filesystem::path& basePath, std::string_view fileName) {
+        if (!fileName.starts_with("package://")) {
+            return basePath / fileName;
+        }
 
-            if (std::filesystem::exists(basePath / fileName)) {
-                return basePath / fileName;
-            }
+        const auto relative = std::filesystem::path(fileName.substr(10));
 
-            //find parent path with package.xml
-            bool packageFound = false;
-            auto packagePath = basePath;
-            for (int i = 0; i < 10; ++i) {
-                if (exists(packagePath / "package.xml") || exists(packagePath / "source-information.json")) {
-                    packageFound = true;
-                    break;
-                }
-                packagePath = packagePath.parent_path();
-            }
-            if (!packageFound) {
-                return "";
-            }
+        if (auto p = basePath / relative; exists(p)) return p;
 
-            std::filesystem::path path = packagePath / fileName;
-            if (exists(path)) {
-                return path;
-            }
-            path = packagePath.parent_path() / fileName;
-            if (exists(path)) {
-                return path;
-            }
+        const auto pkgRoot = findPackageRoot(basePath);
+        if (pkgRoot.empty()) return {};
 
-            return "";
-        }
+        if (auto p = pkgRoot / relative; exists(p)) return p;
+        if (auto p = pkgRoot.parent_path() / relative; exists(p)) return p;
 
-        return basePath / fileName;
+        return {};
     }
 
     std::shared_ptr<Object3D> parseGeometryNode(const std::filesystem::path& path, Loader<Group>* loader, const pugi::xml_node& geometry) {
@@ -181,9 +173,13 @@ namespace {
 
 }// namespace
 
+#include "XacroProcessor.hpp"
+
+
 struct URDFLoader::Impl {
 
     std::shared_ptr<Loader<Group>> loader;
+    std::map<std::string, std::string> xacroArgs;
 
     Impl(): loader(std::make_shared<ModelLoader>()) {}
 
@@ -193,6 +189,14 @@ struct URDFLoader::Impl {
 
         if (!result) return nullptr;
 
+        const std::string ext = utils::toLower(path.extension().string());
+        if (ext == ".xacro" || xacro::Processor::needsProcessing(doc)) {
+            xacro::Processor proc(path.parent_path(), xacroArgs);
+            pugi::xml_document processed;
+            proc.process(doc, processed);
+            return loadFromXml(processed, path);
+        }
+
         return loadFromXml(doc, path);
     }
 
@@ -202,6 +206,13 @@ struct URDFLoader::Impl {
 
         if (!result) return nullptr;
 
+        if (xacro::Processor::needsProcessing(doc)) {
+            xacro::Processor proc(baseDir, xacroArgs);
+            pugi::xml_document processed;
+            proc.process(doc, processed);
+            return loadFromXml(processed, baseDir);
+        }
+
         return loadFromXml(doc, baseDir);
     }
 
@@ -297,6 +308,11 @@ URDFLoader& URDFLoader::setGeometryLoader(std::shared_ptr<Loader<Group>> loader)
     return *this;
 }
 
+URDFLoader& URDFLoader::setArgs(std::map<std::string, std::string> args) {
+    pimpl_->xacroArgs = std::move(args);
+    return *this;
+}
+
 std::shared_ptr<Robot> URDFLoader::load(const std::filesystem::path& path) {
 
     return pimpl_->load(path);