Skip to content

Commit

Permalink
fix IsWheelHeuristic, clang-format
Browse files Browse the repository at this point in the history 
Signed-off-by: Jan Hanca <[email protected]>
  • Loading branch information
@jhanca-robotecai
jhanca-robotecai committed Nov 29, 2023
1 parent 972433b commit 8c1e80b
Showing 1 changed file with 68 additions and 41 deletions.
109 changes: 68 additions & 41 deletions Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
#include <AzCore/Asset/AssetManagerBus.h>
#include <AzCore/IO/Path/Path.h>
#include <AzCore/StringFunc/StringFunc.h>
#include <AzCore/std/string/regex.h>
#include <AzCore/Utils/Utils.h>
#include <AzCore/std/string/regex.h>
#include <AzToolsFramework/API/EditorAssetSystemAPI.h>
#include <RobotImporter/Utils/ErrorUtils.h>
#include <string.h>
Expand All @@ -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());
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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<AZ::IO::Path> 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);
};
Expand All @@ -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.
Expand Down Expand Up @@ -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;
}
}
Expand All @@ -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;
}
Expand Down Expand Up @@ -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 {};
}

Expand All @@ -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
Expand All @@ -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 {};
}
Expand All @@ -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()
Expand Down Expand Up @@ -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())
{
Expand All @@ -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;
}
Expand Down

0 comments on commit 8c1e80b

Please sign in to comment.