diff --git a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp index 05d66ff7fe..f283884f7e 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp @@ -12,8 +12,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -34,7 +34,7 @@ namespace ROS2::Utils auto wheelMatcher = [](AZStd::string_view name) { // StringFunc matches are case-insensitive by default - return AZ::StringFunc::StartsWith(name, "wheel_") || AZ::StringFunc::EndsWith(name, "_wheel"); + return AZ::StringFunc::Contains(name, "wheel"); }; const AZStd::string linkName(link->Name().c_str(), link->Name().size()); @@ -161,6 +161,7 @@ namespace ROS2::Utils public: LinkVisitorCallback m_linkVisitorCB; bool m_recurseModels{}; + private: // Stack storing the current composition of models visited so far ModelStack m_modelStack; @@ -227,6 +228,7 @@ namespace ROS2::Utils public: JointVisitorCallback m_jointVisitorCB; bool m_recurseModels{}; + private: // Stack storing the current composition of models visited so far ModelStack m_modelStack; @@ -513,8 +515,7 @@ namespace ROS2::Utils const sdf::Model* GetModelContainingLink(const sdf::Root& root, AZStd::string_view fullyQualifiedLinkName) { const sdf::Model* resultModel{}; - auto IsLinkInModel = [&fullyQualifiedLinkName, - &resultModel](const sdf::Model& model, const ModelStack&) -> VisitModelResponse + auto IsLinkInModel = [&fullyQualifiedLinkName, &resultModel](const sdf::Model& model, const ModelStack&) -> VisitModelResponse { const std::string stdLinkName(fullyQualifiedLinkName.data(), fullyQualifiedLinkName.size()); if (const sdf::Link* searchLink = model.LinkByName(stdLinkName); searchLink != nullptr) @@ -710,16 +711,12 @@ namespace ROS2::Utils return filenames; } - AZ::IO::Path ResolveAmentPrefixPath( - AZ::IO::Path unresolvedPath, - AZStd::string_view amentPrefixPath, - const FileExistsCB& fileExistsCB) + AZ::IO::Path ResolveAmentPrefixPath(AZ::IO::Path unresolvedPath, AZStd::string_view amentPrefixPath, const FileExistsCB& fileExistsCB) { AZStd::vector amentPrefixPaths; // Parse the AMENT_PREFIX_PATH environment variable into a set of distinct paths. - auto AmentPrefixPathVisitor = [&amentPrefixPaths]( - AZStd::string_view prefixPath) + auto AmentPrefixPathVisitor = [&amentPrefixPaths](AZStd::string_view prefixPath) { amentPrefixPaths.push_back(prefixPath); }; @@ -730,7 +727,7 @@ namespace ROS2::Utils AZ::IO::PathView strippedPath; // The AMENT_PREFIX_PATH is only used for lookups if the URI starts with "model://" or "package://" - constexpr AZStd::string_view ValidAmentPrefixes[] = {"model://", "package://"}; + constexpr AZStd::string_view ValidAmentPrefixes[] = { "model://", "package://" }; for (const auto& prefix : ValidAmentPrefixes) { // Perform a case-sensitive check to look for the prefix. @@ -769,8 +766,12 @@ namespace ROS2::Utils if (const AZ::IO::Path candidateResolvedPath = amentSharePath / strippedPath; fileExistsCB(packageManifestPath) && fileExistsCB(candidateResolvedPath)) { - AZ_Trace("ResolveAssetPath", R"(Resolved using AMENT_PREFIX_PATH: "%.*s" -> "%.*s")" "\n", - AZ_PATH_ARG(unresolvedPath), AZ_PATH_ARG(candidateResolvedPath)); + AZ_Trace( + "ResolveAssetPath", + R"(Resolved using AMENT_PREFIX_PATH: "%.*s" -> "%.*s")" + "\n", + AZ_PATH_ARG(unresolvedPath), + AZ_PATH_ARG(candidateResolvedPath)); return candidateResolvedPath; } } @@ -794,7 +795,8 @@ namespace ROS2::Utils // If the settings tell us to try the AMENT_PREFIX_PATH, use that first to try and resolve path. if (pathResolverSettings.m_useAmentPrefixPath) { - if (AZ::IO::Path amentResolvedPath = ResolveAmentPrefixPath(unresolvedPath, amentPrefixPath, fileExistsCB); !amentResolvedPath.empty()) + if (AZ::IO::Path amentResolvedPath = ResolveAmentPrefixPath(unresolvedPath, amentPrefixPath, fileExistsCB); + !amentResolvedPath.empty()) { return amentResolvedPath; } @@ -844,7 +846,11 @@ namespace ROS2::Utils // There's no match. if (replacedUriPath.empty()) { - AZ_Trace("ResolveAssetPath", R"(Resolved Path is empty: "%.*s" -> "")" "\n", AZ_PATH_ARG(unresolvedPath)); + AZ_Trace( + "ResolveAssetPath", + R"(Resolved Path is empty: "%.*s" -> "")" + "\n", + AZ_PATH_ARG(unresolvedPath)); return {}; } @@ -854,8 +860,12 @@ namespace ROS2::Utils { if (fileExistsCB(replacedUriPath)) { - AZ_Trace("ResolveAssetPath", R"(Resolved Absolute Path: "%.*s" -> "%.*s")" "\n", - AZ_PATH_ARG(unresolvedPath), AZ_PATH_ARG(replacedUriPath)); + AZ_Trace( + "ResolveAssetPath", + R"(Resolved Absolute Path: "%.*s" -> "%.*s")" + "\n", + AZ_PATH_ARG(unresolvedPath), + AZ_PATH_ARG(replacedUriPath)); return replacedUriPath; } else @@ -868,31 +878,39 @@ namespace ROS2::Utils // The URI path is not absolute, so attempt to append it to the ancestor directories of the URDF/SDF file for (const AZ::IO::Path& ancestorPath : ancestorPaths) { - if (const AZ::IO::Path candidateResolvedPath = ancestorPath / replacedUriPath; - fileExistsCB(candidateResolvedPath)) + if (const AZ::IO::Path candidateResolvedPath = ancestorPath / replacedUriPath; fileExistsCB(candidateResolvedPath)) { - AZ_Trace("ResolveAssetPath", R"(Resolved using ancestor paths: "%.*s" -> "%.*s")" "\n", - AZ_PATH_ARG(unresolvedPath), AZ_PATH_ARG(candidateResolvedPath)); + AZ_Trace( + "ResolveAssetPath", + R"(Resolved using ancestor paths: "%.*s" -> "%.*s")" + "\n", + AZ_PATH_ARG(unresolvedPath), + AZ_PATH_ARG(candidateResolvedPath)); return candidateResolvedPath; } } } } - // At this point, the path has no identified URI prefix. If it's an absolute path, try to locate and return it. // Otherwise, return an empty path as an error. if (unresolvedPath.IsAbsolute()) { if (fileExistsCB(unresolvedPath)) { - AZ_Trace("ResolveAssetPath", R"(Resolved Absolute Path: "%.*s")" "\n", + AZ_Trace( + "ResolveAssetPath", + R"(Resolved Absolute Path: "%.*s")" + "\n", AZ_PATH_ARG(unresolvedPath)); return unresolvedPath; } else { - AZ_Trace("ResolveAssetPath", R"(Failed to resolve Absolute Path: "%.*s")" "\n", + AZ_Trace( + "ResolveAssetPath", + R"(Failed to resolve Absolute Path: "%.*s")" + "\n", AZ_PATH_ARG(unresolvedPath)); return {}; } @@ -904,13 +922,21 @@ namespace ROS2::Utils if (fileExistsCB(relativePath)) { - AZ_Trace("ResolveAssetPath", R"(Resolved Relative Path: "%.*s" -> "%.*s")" "\n", - AZ_PATH_ARG(unresolvedPath), AZ_PATH_ARG(relativePath)); + AZ_Trace( + "ResolveAssetPath", + R"(Resolved Relative Path: "%.*s" -> "%.*s")" + "\n", + AZ_PATH_ARG(unresolvedPath), + AZ_PATH_ARG(relativePath)); return relativePath; } - AZ_Trace("ResolveAssetPath", R"(Failed to resolve Relative Path: "%.*s" -> "%.*s")" "\n", - AZ_PATH_ARG(unresolvedPath), AZ_PATH_ARG(relativePath)); + AZ_Trace( + "ResolveAssetPath", + R"(Failed to resolve Relative Path: "%.*s" -> "%.*s")" + "\n", + AZ_PATH_ARG(unresolvedPath), + AZ_PATH_ARG(relativePath)); return {}; } AmentPrefixString GetAmentPrefixPath() @@ -1011,7 +1037,7 @@ namespace ROS2::Utils::SDFormat for (auto& [prefix, pathList] : settings.m_resolverSettings.m_uriPrefixMap) { std::string uriPath; - for(auto& path : pathList) + for (auto& path : pathList) { if (!uriPath.empty()) { @@ -1030,20 +1056,21 @@ namespace ROS2::Utils::SDFormat // If any files couldn't be found using our supplied prefix mappings, this callback will get called. // Attempt to use our full path resolution, and print a warning if it still couldn't be resolved. - sdfConfig.SetFindCallback([settings, baseFilePath](const std::string &fileName) -> std::string - { - auto amentPrefixPath = Utils::GetAmentPrefixPath(); - - auto resolved = Utils::ResolveAssetPath(AZ::IO::Path(fileName.c_str()), baseFilePath, amentPrefixPath, settings); - if (!resolved.empty()) + sdfConfig.SetFindCallback( + [settings, baseFilePath](const std::string& fileName) -> std::string { - AZ_Trace("SdfParserConfig", "SDF SetFindCallback resolved '%s' -> '%s'", fileName.c_str(), resolved.c_str()); - return resolved.c_str(); - } + auto amentPrefixPath = Utils::GetAmentPrefixPath(); + + auto resolved = Utils::ResolveAssetPath(AZ::IO::Path(fileName.c_str()), baseFilePath, amentPrefixPath, settings); + if (!resolved.empty()) + { + AZ_Trace("SdfParserConfig", "SDF SetFindCallback resolved '%s' -> '%s'", fileName.c_str(), resolved.c_str()); + return resolved.c_str(); + } - AZ_Warning("SdfParserConfig", false, "SDF SetFindCallback failed to resolve '%s'", fileName.c_str()); - return fileName; - }); + AZ_Warning("SdfParserConfig", false, "SDF SetFindCallback failed to resolve '%s'", fileName.c_str()); + return fileName; + }); return sdfConfig; }