ROS2 URDF importer will parse AMENT_PREFIX_PATH (#226)

* ROS2 URDF importer would parse AMENT_PACKAGE_PATH to find packages during import

Signed-off-by: Michał Pełka <[email protected]>

* Minor revision

Signed-off-by: Michał Pełka <[email protected]>

---------

Signed-off-by: Michał Pełka <[email protected]>

Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp

@@ -147,23 +147,91 @@ namespace ROS2
         return filenames;
     }
 
+    AZStd::optional<AZ::IO::Path> GetResolvedPath(const AZ::IO::Path &packagePath,
+                                                 const AZ::IO::Path &unresolvedPath,
+                                                 const AZStd::function<bool(const AZStd::string&)>& fileExists)
+    {
+        AZ::IO::Path packageXmlCandite = packagePath / "package.xml";
+        if (fileExists(packageXmlCandite.String()))
+        {
+            AZ::IO::Path resolvedPath = packagePath / unresolvedPath;
+            if (fileExists(resolvedPath.String()))
+            {
+                return AZStd::optional<AZ::IO::Path>{resolvedPath};
+            }
+        }
+        return AZStd::optional<AZ::IO::Path>{};
+    }
+
+    AZ::IO::Path GetPathFromSubPath(const AZ::IO::Path::const_iterator& begin, const AZ::IO::Path::const_iterator& end)
+    {
+        AZ::IO::Path subpath;
+        if (begin == end)
+        {
+            return subpath;
+        }
+        for (AZ::IO::Path::iterator pathIt = begin;pathIt!=end;pathIt++)
+        {
+            subpath /= *pathIt;
+        }
+        return subpath;
+    }
+
     /// Finds global path from URDF path
     AZStd::string Utils::ResolveURDFPath(
-        AZStd::string unresolvedPath, const AZStd::string& urdfFilePath, const AZStd::function<bool(const AZStd::string&)>& fileExists)
+        AZStd::string unresolvedPath,
+        const AZStd::string& urdfFilePath,
+        const AZStd::string& amentPrefixPath,
+        const AZStd::function<bool(const AZStd::string&)>& fileExists)
     {
+        AZ_Printf("ResolveURDFPath", "ResolveURDFPath with %s\n", unresolvedPath.c_str());
         if (unresolvedPath.starts_with("package://"))
         {
             AZ::StringFunc::Replace(unresolvedPath, "package://", "", true, true);
-            AZ::IO::Path urdfProperPath(urdfFilePath);
-            AZ::IO::Path packagePath;
-            for (auto it = urdfProperPath.begin(); it != urdfProperPath.end(); it++)
+
+            const AZ::IO::Path unresolvedProperPath(unresolvedPath);
+            if (!unresolvedProperPath.empty())
             {
-                packagePath /= *it;
-                AZStd::string packageXmlCandite = (packagePath / "package.xml").String();
-                if (fileExists(packageXmlCandite))
+                const AZStd::string packageNameCandidate = unresolvedProperPath.begin()->String();
+                AZStd::vector<AZStd::string> amentPathTokenized;
+                AZ::StringFunc::Tokenize(amentPrefixPath, amentPathTokenized, ':');
+                for (const auto& package : amentPathTokenized)
                 {
-                    // package.xml has been found
-                    return (packagePath / unresolvedPath).String();
+                    if (package.ends_with(packageNameCandidate))
+                    {
+                        auto pathIt = unresolvedProperPath.begin();
+                        AZStd::advance(pathIt,1);
+                        if (pathIt != unresolvedProperPath.end())
+                        {
+                            AZ::IO::Path unresolvedPathStripped = GetPathFromSubPath(pathIt, unresolvedProperPath.end());
+
+                            const AZ::IO::Path packagePath = AZ::IO::Path{ package } / "share";
+                            auto resolvedPath =
+                                GetResolvedPath(packagePath / AZ::IO::Path{ packageNameCandidate }, unresolvedPathStripped, fileExists);
+                            if (resolvedPath.has_value())
+                            {
+                                AZ_Printf("ResolveURDFPath", "Resolved to using Ament to : %s\n", resolvedPath->String().c_str());
+                                return resolvedPath->String();
+                            }
+                        }
+                    }
+                }
+            }
+
+            const AZ::IO::Path urdfProperPath(urdfFilePath);
+            if (!urdfProperPath.empty())
+            {
+                auto it = --urdfProperPath.end();
+                for (; it != urdfProperPath.begin(); it--)
+                {
+                    const auto packagePath = GetPathFromSubPath(urdfProperPath.begin(),it);
+                    std::cout << "packagePath : " << packagePath.String().c_str() << std::endl;
+                    const auto resolvedPath = GetResolvedPath(packagePath, unresolvedPath, fileExists);
+                    if (resolvedPath.has_value())
+                    {
+                        AZ_Printf("ResolveURDFPath", "ResolveURDFPath with relative path to : %s\n", resolvedPath->String().c_str());
+                        return resolvedPath->String();
+                    }
                 }
             }
             // No path available
@@ -173,13 +241,16 @@ namespace ROS2
         {
             // Paths that start with 'file:///' are absolute paths
             AZ::StringFunc::Replace(unresolvedPath, "file://", "", true, true);
+            AZ_Printf("ResolveURDFPath", "ResolveURDFPath with absolute path to : %s\n", unresolvedPath.c_str());
             return unresolvedPath;
         }
         // seems to be relative path
         AZ::IO::Path relativePath(unresolvedPath);
         AZ::IO::Path urdfProperPath(urdfFilePath);
         AZ::IO::Path urdfParentPath = urdfProperPath.ParentPath();
-        return (urdfParentPath / relativePath).String();
+        const AZ::IO::Path resolvedPath = urdfParentPath / relativePath;
+        AZ_Printf("ResolveURDFPath", "ResolveURDFPath with relative path to : %s\n", unresolvedPath.c_str());
+        return resolvedPath.String();
     }
 
 } // namespace ROS2

Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h

@@ -61,11 +61,13 @@ namespace ROS2
         //! Resolves path from unresolved URDF path.
         //! @param unresolvedPath - unresolved URDF path, example : `package://meshes/foo.dae`.
         //! @param urdfFilePath - the absolute path of URDF file which contains the path that is to be resolved.
+        //! @param amentPrefixPath - the string that contains available packages' path, separated by ':' signs.
         //! @param fileExists - functor to check if the given file exists. Exposed for unit test, default one should be used.
         //! @returns resolved path to the mesh
         AZStd::string ResolveURDFPath(
             AZStd::string unresolvedPath,
             const AZStd::string& urdfFilePath,
+            const AZStd::string& amentPrefixPath,
             const AZStd::function<bool(const AZStd::string&)>& fileExists = FileExistsCall);
 
     } // namespace Utils

Gems/ROS2/Code/Source/RobotImporter/Utils/SourceAssetsStorage.cpp

@@ -105,12 +105,16 @@ namespace ROS2::Utils
 
     UrdfAssetMap FindAssetsForUrdf(const AZStd::unordered_set<AZStd::string>& meshesFilenames, const AZStd::string& urdFilename)
     {
+        auto enviromentalVariable = std::getenv("AMENT_PREFIX_PATH");
+        AZ_Error("UrdfAssetMap", enviromentalVariable, "AMENT_PREFIX_PATH is not found.");
+        AZStd::string amentPrefixPath {enviromentalVariable};
+
         UrdfAssetMap urdfToAsset;
         for (const auto& t : meshesFilenames)
         {
             Utils::UrdfAsset asset;
             asset.m_urdfPath = t;
-            asset.m_resolvedUrdfPath = Utils::ResolveURDFPath(asset.m_urdfPath, urdFilename);
+            asset.m_resolvedUrdfPath = Utils::ResolveURDFPath(asset.m_urdfPath, urdFilename, amentPrefixPath);
             asset.m_urdfFileCRC = Utils::GetFileCRC(asset.m_resolvedUrdfPath);
             urdfToAsset.emplace(t, AZStd::move(asset));
         }

Gems/ROS2/Code/Tests/UrdfParserTest.cpp

@@ -412,7 +412,7 @@ namespace UnitTest
         AZStd::string urdf = "/home/foo/ros_ws/install/foo_robot/foo_robot.urdf";
         auto result = ROS2::Utils::ResolveURDFPath(
             dae,
-            urdf,
+            urdf, "",
             [](const AZStd::string& p) -> bool
             {
                 return false;
@@ -426,7 +426,7 @@ namespace UnitTest
         AZStd::string urdf = "/home/foo/ros_ws/install/foo_robot/foo_robot.urdf";
         auto result = ROS2::Utils::ResolveURDFPath(
             dae,
-            urdf,
+            urdf, "",
             [](const AZStd::string& p) -> bool
             {
                 return false;
@@ -439,12 +439,27 @@ namespace UnitTest
         AZStd::string dae = "package://meshes/bar.dae";
         AZStd::string urdf = "/home/foo/ros_ws/install/foo_robot/description/foo_robot.urdf";
         AZStd::string xml = "/home/foo/ros_ws/install/foo_robot/package.xml";
+        AZStd::string resolvedDae = "/home/foo/ros_ws/install/foo_robot/meshes/bar.dae";
         auto mockFileSystem = [&](const AZStd::string& p) -> bool
         {
-            return (p == xml);
+            return (p == xml || p == resolvedDae);
         };
-        auto result = ROS2::Utils::ResolveURDFPath(dae, urdf, mockFileSystem);
-        EXPECT_EQ(result, "/home/foo/ros_ws/install/foo_robot/meshes/bar.dae");
+        auto result = ROS2::Utils::ResolveURDFPath(dae, urdf, "", mockFileSystem);
+        EXPECT_EQ(result, resolvedDae);
+    }
+
+    TEST_F(UrdfParserTest, TestPathResolvementExplicitPackageName)
+    {
+        AZStd::string dae = "package://foo_robot/meshes/bar.dae";
+        AZStd::string urdf = "/home/foo/ros_ws/install/foo_robot/share/foo_robot/description/foo_robot.urdf";
+        AZStd::string xml = "/home/foo/ros_ws/install/foo_robot/share/foo_robot/package.xml";
+        AZStd::string resolvedDae = "/home/foo/ros_ws/install/foo_robot/share/foo_robot/meshes/bar.dae";
+        auto mockFileSystem = [&](const AZStd::string& p) -> bool
+        {
+            return (p == xml || p == resolvedDae);
+        };
+        auto result = ROS2::Utils::ResolveURDFPath(dae, urdf, "/home/foo/ros_ws/install/foo_robot", mockFileSystem);
+        EXPECT_EQ(result, resolvedDae);
     }
 
     TEST_F(UrdfParserTest, XacroParseArgs)