From aeab601c98012ff8ee60ef3b76951ab8d05f8133 Mon Sep 17 00:00:00 2001 From: lumberyard-employee-dm <56135373+lumberyard-employee-dm@users.noreply.github.com> Date: Tue, 19 Sep 2023 14:46:39 -0500 Subject: [PATCH 1/8] Updated the URDFPrefabMaker to avoid referencing SDF link names (#523) Updated the URDFPrefabMaker to avoid referencing SDF link names as they are not globally unique The `CreatePrefabTemplateFromUrdfOrSdf` function has been modified to create a JointMapper object which stores a mapping of joints to all their parent and child links. It also creates a mapping of joints to the model they are directly attached to. A similar LinkMapper structure has been added for SDF links, which stores a mapping of the link to their attached model. Instead of `CreatePrefabTemplateFromUrdfOrSdf` creating a mapping of SDF link name to O3DE Entity, that mapping has now been updated toi map the SDF link pointer to O3DE Entity. This ensures that SDF links with same name are not lost when mapping to O3DE entities. This is needed as SDF links and joints are only required to be unique relative to their attached model. Two links on different models, can have the same link name (http://sdformat.org/tutorials?tut=composition_proposal#1-6-proposed-parsing-stages) Updated the Robot Importer Visitor callback to require "ModelStack" parameters, which passes in the stack of `sdf::Model` objects that were visited on the way to the sdf element being visited be that a link, joint or nested model. Also updated the GetAllJoints and GetAllLinks function to store a fully qualified name for the joint/link including the ancestor model names when returning the name to joint and name to link map respectively. Signed-off-by: lumberyard-employee-dm <56135373+lumberyard-employee-dm@users.noreply.github.com> Addressed PR feedback for o3de/o3de-extra#523 around invalid text and grammar Signed-off-by: lumberyard-employee-dm <56135373+lumberyard-employee-dm@users.noreply.github.com> --- ...ROS2RobotImporterEditorSystemComponent.cpp | 3 +- .../RobotImporter/RobotImporterWidget.cpp | 3 +- .../RobotImporter/URDF/URDFPrefabMaker.cpp | 241 ++++++++++-------- .../RobotImporter/URDF/URDFPrefabMaker.h | 4 +- .../Utils/RobotImporterUtils.cpp | 135 ++++++---- .../RobotImporter/Utils/RobotImporterUtils.h | 77 +++--- Gems/ROS2/Code/Tests/SdfParserTest.cpp | 67 ++++- Gems/ROS2/Code/Tests/UrdfParserTest.cpp | 46 ++-- 8 files changed, 355 insertions(+), 221 deletions(-) diff --git a/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp b/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp index d7237b06f..c418959ea 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp @@ -222,7 +222,8 @@ namespace ROS2 }; // Use the URDF/SDF file name stem the prefab name - AZStd::string prefabName = AZStd::string(AZ::IO::PathView(filePath).Stem().Native()); + auto fileStem = AZ::IO::PathView(filePath).Stem(); + AZStd::string prefabName = AZStd::string::format("%.*s.prefab", AZ_PATH_ARG(fileStem)); if (prefabName.empty()) { diff --git a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp index 62b3be881..87e5178fb 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp @@ -348,7 +348,7 @@ namespace ROS2 void RobotImporterWidget::FillPrefabMakerPage() { // Use the URDF/SDF file name stem the prefab name - AZStd::string robotName = AZStd::string(m_urdfPath.Stem().Native()) + ".prefab"; + AZStd::string robotName = AZStd::string::format("%.*s.prefab", AZ_PATH_ARG(m_urdfPath.Stem())); m_prefabMakerPage->setProposedPrefabName(robotName); QWizard::button(PrefabCreationButtonId)->setText(tr("Create Prefab")); QWizard::setOption(HavePrefabCreationButton, true); @@ -439,7 +439,6 @@ namespace ROS2 const AZ::IO::Path prefabPath(AZ::IO::Path(AZ::Utils::GetProjectPath()) / prefabPathRelative); bool fileExists = AZ::IO::FileIOBase::GetInstance()->Exists(prefabPath.c_str()); - if (CheckCyclicalDependency(prefabPathRelative)) { m_prefabMakerPage->setSuccess(false); return; diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp index b79253856..e3fa2d979 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp @@ -72,7 +72,7 @@ namespace ROS2 }; // Iterate over all visuals to get their materials - auto VisitAllModels = [&GetVisualsFromModel](const sdf::Model& model) -> Utils::VisitModelResponse + auto VisitAllModels = [&GetVisualsFromModel](const sdf::Model& model, const Utils::ModelStack&) -> Utils::VisitModelResponse { GetVisualsFromModel(model); // Continue to visit all models within the SDF document and query their tags @@ -84,101 +84,140 @@ namespace ROS2 } } - void URDFPrefabMaker::BuildAssetsForLink(const sdf::Link* link) + URDFPrefabMaker::CreatePrefabTemplateResult URDFPrefabMaker::CreatePrefabTemplateFromUrdfOrSdf() { - m_collidersMaker.BuildColliders(link); + { + AZStd::lock_guard lck(m_statusLock); + m_status.clear(); + } - auto GetAssetsForLinkInModel = [this, link](const sdf::Model& model) -> Utils::VisitModelResponse + if (!ContainsModel()) + { + return AZ::Failure(AZStd::string("URDF/SDF doesn't contain any models.")); + } + + // Visit any nested models in the SDF as well + constexpr bool visitNestedModels = true; + + // Maintains references to all joints and a mapping from the joints to their parent and child links + struct JointsMapper + { + struct JointToAttachedModel + { + AZStd::string m_fullyQualifiedName; + const sdf::Joint* m_joint; + const sdf::Model* m_attachedModel; + }; + // this is a unique ordered vector + AZStd::vector m_joints; + AZStd::unordered_map m_jointToParentLinks; + AZStd::unordered_map m_jointToChildLinks; + }; + JointsMapper jointsMapper; + auto GetAllJointsFromModel = [&jointsMapper](const sdf::Model& model, const Utils::ModelStack&) -> Utils::VisitModelResponse { - // Find the links which are children in a joint where this link is a parent - auto BuildAssetsFromJointChildLinks = [this, &model](const sdf::Joint& joint) + // As the VisitModels function visits nested models by default, gatherNestedModelJoints is set to false + constexpr bool gatherNestedModelJoints = false; + auto jointsForModel = Utils::GetAllJoints(model, gatherNestedModelJoints); + for (const auto& [fullyQualifiedName, joint] : jointsForModel) { - if (const sdf::Link* childLink = model.LinkByName(joint.ChildName()); childLink != nullptr) + JointsMapper::JointToAttachedModel jointToAttachedModel{ + AZStd::string(fullyQualifiedName.c_str(), fullyQualifiedName.size()), joint, &model + }; + jointsMapper.m_joints.push_back(AZStd::move(jointToAttachedModel)); + + // add mapping from joint to child link + std::string childName = joint->ChildName(); + if (const sdf::Link* link = model.LinkByName(childName); link != nullptr) { - BuildAssetsForLink(childLink); + // Add a mapping of joint to child link + jointsMapper.m_jointToChildLinks[joint] = link; } - return true; - }; - - // Make sure the link is a child of the model being visited - // Before visiting the joints of the model - if (const sdf::Link* searchLink = model.LinkByName(link->Name()); searchLink != nullptr) - { - // Don't visit nested models using the VisitJoints function as the outer call - // to VisitModels already visits nested models - constexpr bool visitNestedModelLinks = false; - Utils::VisitJoints(model, BuildAssetsFromJointChildLinks, visitNestedModelLinks); + // add mapping from joint to parent link + std::string parentName = joint->ParentName(); + if (const sdf::Link* link = model.LinkByName(parentName); link != nullptr) + { + jointsMapper.m_jointToParentLinks[joint] = link; + } } return Utils::VisitModelResponse::VisitNestedAndSiblings; }; + // Gather all Joints in SDF including in nested models + Utils::VisitModels(*m_root, GetAllJointsFromModel, visitNestedModels); - constexpr bool visitNestedModels = true; - Utils::VisitModels(*m_root, GetAssetsForLinkInModel, visitNestedModels); - } - - URDFPrefabMaker::CreatePrefabTemplateResult URDFPrefabMaker::CreatePrefabTemplateFromUrdfOrSdf() - { + // Maintains references to all Links in the SDF and a mapping of links to the model it is attached to + struct LinksMapper { - AZStd::lock_guard lck(m_statusLock); - m_status.clear(); - } - - if (!ContainsModel()) - { - return AZ::Failure(AZStd::string("URDF/SDF doesn't contain any models.")); - } + struct LinkToAttachedModel + { + AZStd::string m_fullyQualifiedName; + const sdf::Link* m_link; + const sdf::Model* m_attachedModel; + }; + // this is a unique ordered vector + AZStd::vector m_links; + }; + LinksMapper linksMapper; - // Build up a list of all entities created as a part of processing the file. - AZStd::vector createdEntities; - AZStd::unordered_map createdLinks; - AZStd::unordered_map links; - // Gather all links from all the models in the SDF - auto GetAllLinksFromModel = [&links](const sdf::Model& model) -> Utils::VisitModelResponse + auto GetAllLinksFromModel = [&linksMapper](const sdf::Model& model, const Utils::ModelStack&) -> Utils::VisitModelResponse { // As the VisitModels function visits nested models by default, gatherNestedModelLinks is set to false constexpr bool gatherNestedModelLinks = false; auto linksForModel = Utils::GetAllLinks(model, gatherNestedModelLinks); - links.insert(linksForModel.begin(), linksForModel.end()); - + for (const auto& [fullyQualifiedName, link] : linksForModel) + { + // Push back the mapping of link to attached model into the ordered vector + LinksMapper::LinkToAttachedModel linkToAttachedModel{ AZStd::string(fullyQualifiedName.c_str(), fullyQualifiedName.size()), + link, + &model }; + linksMapper.m_links.push_back(AZStd::move(linkToAttachedModel)); + } return Utils::VisitModelResponse::VisitNestedAndSiblings; }; - // Visit any nested models in the SDF as well - constexpr bool visitNestedModels = true; + // Gather all links from all the models in the SDF Utils::VisitModels(*m_root, GetAllLinksFromModel, visitNestedModels); - for (const auto& [name, linkPtr] : links) + // Build up a list of all entities created as a part of processing the file. + AZStd::vector createdEntities; + AZStd::unordered_map createdLinks; + AZStd::unordered_map links; + for ([[maybe_unused]] const auto& [fullLinkName, linkPtr, _] : linksMapper.m_links) { - createdLinks[name] = AddEntitiesForLink(linkPtr, AZ::EntityId{}, createdEntities); + createdLinks[linkPtr] = AddEntitiesForLink(linkPtr, AZ::EntityId{}, createdEntities); } - for (const auto& [name, result] : createdLinks) + for (const auto& [linkPtr, result] : createdLinks) { + std::string linkName = linkPtr->Name(); + AZStd::string azLinkName(linkName.c_str(), linkName.size()); AZ_Trace( "CreatePrefabFromUrdfOrSdf", "Link with name %s was created as: %s\n", - name.c_str(), + linkName.c_str(), result.IsSuccess() ? (result.GetValue().ToString().c_str()) : ("[Failed]")); AZStd::lock_guard lck(m_statusLock); if (result.IsSuccess()) { - m_status.emplace(name, AZStd::string::format("created as: %s", result.GetValue().ToString().c_str())); + m_status.emplace(azLinkName, AZStd::string::format("created as: %s", result.GetValue().ToString().c_str())); } else { - m_status.emplace(name, AZStd::string::format("failed : %s", result.GetError().c_str())); + m_status.emplace(azLinkName, AZStd::string::format("failed : %s", result.GetError().c_str())); } } // Set the transforms of links - for (const auto& [name, linkPtr] : links) + for ([[maybe_unused]] const auto& [fullLinkName, linkPtr, _] : linksMapper.m_links) { - if (const auto thisEntry = createdLinks.at(name); thisEntry.IsSuccess()) + if (const auto createLinkEntityResult = createdLinks.at(linkPtr); createLinkEntityResult.IsSuccess()) { + AZ::EntityId createdEntityId = createLinkEntityResult.GetValue(); + std::string linkName = linkPtr->Name(); AZ::Transform tf = Utils::GetWorldTransformURDF(linkPtr); - auto* entity = AzToolsFramework::GetEntityById(thisEntry.GetValue()); + auto* entity = AzToolsFramework::GetEntityById(createdEntityId); if (entity) { auto* transformInterface = entity->FindComponent(); @@ -187,8 +226,8 @@ namespace ROS2 AZ_Trace( "CreatePrefabFromUrdfOrSdf", "Setting transform %s %s to [%f %f %f] [%f %f %f %f]\n", - name.c_str(), - thisEntry.GetValue().ToString().c_str(), + linkName.c_str(), + createdEntityId.ToString().c_str(), tf.GetTranslation().GetX(), tf.GetTranslation().GetY(), tf.GetTranslation().GetZ(), @@ -201,7 +240,9 @@ namespace ROS2 else { AZ_Trace( - "CreatePrefabFromUrdfOrSdf", "Setting transform failed: %s does not have transform interface\n", name.c_str()); + "CreatePrefabFromUrdfOrSdf", + "Setting transform failed: %s does not have transform interface\n", + linkName.c_str()); } } } @@ -209,21 +250,20 @@ namespace ROS2 // Set the hierarchy AZStd::vector linkEntityIdsWithoutParent; - for (const auto& [linkName, linkPtr] : links) + for (const auto& [fullLinkName, linkPtr, attachedModel] : linksMapper.m_links) { - const auto linkPrefabResult = createdLinks.at(linkName); + std::string linkName = linkPtr->Name(); + AZStd::string azLinkName(linkName.c_str(), linkName.size()); + const auto linkPrefabResult = createdLinks.at(linkPtr); if (!linkPrefabResult.IsSuccess()) { - AZ_Trace("CreatePrefabFromUrdfOrSdf", "Link %s creation failed\n", linkName.c_str()); + AZ_Trace("CreatePrefabFromUrdfOrSdf", "Link %s creation failed\n", fullLinkName.c_str()); continue; } AZStd::vector jointsWhereLinkIsChild; - if (const sdf::Model* modelContainingLink = Utils::GetModelContainingLink(*m_root, *linkPtr); modelContainingLink != nullptr) - { - bool gatherNestedModelJoints = true; - jointsWhereLinkIsChild = Utils::GetJointsForChildLink(*modelContainingLink, linkName, gatherNestedModelJoints); - } + bool gatherNestedModelJoints = true; + jointsWhereLinkIsChild = Utils::GetJointsForChildLink(*attachedModel, azLinkName, gatherNestedModelJoints); if (jointsWhereLinkIsChild.empty()) { @@ -251,15 +291,22 @@ namespace ROS2 > defining the coordinate transformation from the parent link frame to the child link frame. */ - AZStd::string parentName( - jointsWhereLinkIsChild.front()->ParentName().c_str(), jointsWhereLinkIsChild.front()->ParentName().size()); - const auto parentEntry = createdLinks.find(parentName); - if (parentEntry == createdLinks.end()) + // Use the first joint where this link is a child to locate the parent link pointer. + const sdf::Joint* joint = jointsWhereLinkIsChild.front(); + std::string parentLinkName = joint->ParentName(); + AZStd::string parentName(parentLinkName.c_str(), parentLinkName.size()); + + // Lookup the entity created from the parent link using the JointMapper to locate the parent SDF link. + // followed by using SDF link address to lookup the O3DE created entity ID + auto parentLinkIter = jointsMapper.m_jointToParentLinks.find(joint); + auto parentEntityIter = + parentLinkIter != jointsMapper.m_jointToParentLinks.end() ? createdLinks.find(parentLinkIter->second) : createdLinks.end(); + if (parentEntityIter == createdLinks.end()) { AZ_Trace("CreatePrefabFromUrdfOrSdf", "Link %s has invalid parent name %s\n", linkName.c_str(), parentName.c_str()); continue; } - if (!parentEntry->second.IsSuccess()) + if (!parentEntityIter->second.IsSuccess()) { AZ_Trace( "CreatePrefabFromUrdfOrSdf", @@ -272,22 +319,25 @@ namespace ROS2 "CreatePrefabFromUrdfOrSdf", "Link %s setting parent to %s\n", linkPrefabResult.GetValue().ToString().c_str(), - parentEntry->second.GetValue().ToString().c_str()); + parentEntityIter->second.GetValue().ToString().c_str()); AZ_Trace("CreatePrefabFromUrdfOrSdf", "Link %s setting parent to %s\n", linkName.c_str(), parentName.c_str()); - PrefabMakerUtils::SetEntityParent(linkPrefabResult.GetValue(), parentEntry->second.GetValue()); + PrefabMakerUtils::SetEntityParent(linkPrefabResult.GetValue(), parentEntityIter->second.GetValue()); } - auto JointVisitor = [this, &createdLinks](const sdf::Joint& joint) + // Iterate over all the joints and locate the entity associated with the link + for ([[maybe_unused]] const auto& [fullJointName, jointPtr, _] : jointsMapper.m_joints) { - auto jointPtr = &joint; - - const std::string& jointName = jointPtr->Name(); + std::string jointName = jointPtr->Name(); AZStd::string azJointName(jointName.c_str(), jointName.size()); - AZStd::string parentLinkName(jointPtr->ParentName().c_str(), jointPtr->ParentName().size()); - AZStd::string childLinkName(jointPtr->ChildName().c_str(), jointPtr->ChildName().size()); - - auto parentLinkIter = createdLinks.find(parentLinkName); - if (parentLinkIter == createdLinks.end()) + std::string childLinkName = jointPtr->ChildName(); + std::string parentLinkName = jointPtr->ParentName(); + + // Look up the O3DE created entity by first locating the parent SDF link associated with the current joint + // and then using that SDF link to lookup the created entity + auto parentLinkIter = jointsMapper.m_jointToParentLinks.find(jointPtr); + auto parentEntityIter = + parentLinkIter != jointsMapper.m_jointToParentLinks.end() ? createdLinks.find(parentLinkIter->second) : createdLinks.end(); + if (parentEntityIter == createdLinks.end()) { AZ_Warning( "CreatePrefabFromUrdfOrSdf", @@ -297,10 +347,13 @@ namespace ROS2 parentLinkName.c_str()); return true; } - auto leadEntity = parentLinkIter->second; + auto leadEntity = parentEntityIter->second; - auto childLinkIter = createdLinks.find(childLinkName); - if (childLinkIter == createdLinks.end()) + // Use the joint to lookup the child SDF link which is used to look up the O3DE entity + auto childLinkIter = jointsMapper.m_jointToChildLinks.find(jointPtr); + auto childEntityIter = + childLinkIter != jointsMapper.m_jointToChildLinks.end() ? createdLinks.find(childLinkIter->second) : createdLinks.end(); + if (childEntityIter == createdLinks.end()) { AZ_Warning( "CreatePrefabFromUrdfOrSdf", @@ -310,7 +363,7 @@ namespace ROS2 childLinkName.c_str()); return true; } - auto childEntity = childLinkIter->second; + auto childEntity = childEntityIter->second; AZ_Trace( "CreatePrefabFromUrdfOrSdf", @@ -344,23 +397,7 @@ namespace ROS2 } } return true; - }; - - // Visit all joints that has at least a parent or child link in every model inside of the SDF document - auto VisitJointsInModel = [&JointVisitor](const sdf::Model& model) -> Utils::VisitModelResponse - { - // The JointVisitor is used instead of iterating over the sdf::Model::JointCount - // as it will skip joints that exist that doesn't have an attached parent link or child link - // such as the root link when its name is "world" - - // As the VisitModels function visits nested models by default, visitNestedJoints - // is set to false to prevent visiting joints twice - constexpr bool visitNestedJoints = false; - Utils::VisitJoints(model, JointVisitor, visitNestedJoints); - return Utils::VisitModelResponse::VisitNestedAndSiblings; - }; - - Utils::VisitModels(*m_root, VisitJointsInModel, visitNestedModels); + } // Use the first entity based on a link that is not parented to any other link if (!linkEntityIdsWithoutParent.empty() && linkEntityIdsWithoutParent.front().IsValid()) @@ -373,8 +410,6 @@ namespace ROS2 // Create prefab, save it to disk immediately // Remove prefab, if it was already created. - AZ::IO::FixedMaxPath prefabTemplateName{ AZ::IO::PathView(m_prefabPath).FixedMaxPathStringAsPosix() }; - // clear out any previously created prefab template for this path auto* prefabSystemComponentInterface = AZ::Interface::Get(); auto prefabLoaderInterface = AZ::Interface::Get(); @@ -566,7 +601,7 @@ namespace ROS2 bool URDFPrefabMaker::ContainsModel() const { const sdf::Model* sdfModel{}; - auto GetModelAndStopIteration = [&sdfModel](const sdf::Model& model) -> Utils::VisitModelResponse + auto GetModelAndStopIteration = [&sdfModel](const sdf::Model& model, const Utils::ModelStack&) -> Utils::VisitModelResponse { sdfModel = &model; // Return stop to prevent further visitation of additional models diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h index 6a048de65..2cbbb65a8 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h @@ -70,9 +70,7 @@ namespace ROS2 AZStd::string GetStatus(); private: - AzToolsFramework::Prefab::PrefabEntityResult AddEntitiesForLink( - const sdf::Link* link, AZ::EntityId parentEntityId, AZStd::vector& createdEntities); - void BuildAssetsForLink(const sdf::Link* link); + AzToolsFramework::Prefab::PrefabEntityResult AddEntitiesForLink(const sdf::Link* link, AZ::EntityId parentEntityId, AZStd::vector& createdEntities); void AddRobotControl(AZ::EntityId rootEntityId); static void MoveEntityToDefaultSpawnPoint(const AZ::EntityId& rootEntityId, AZStd::optional spawnPosition); diff --git a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp index 08f4d472e..16b3c87e2 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp @@ -113,17 +113,19 @@ namespace ROS2::Utils // Optionally it supports recursing nested models to visit their links as well struct VisitLinksForNestedModels_fn { - void operator()(const sdf::Model& model) + VisitModelResponse operator()(const sdf::Model& model) { - if (VisitLinksForModel(model) && m_recurseModels) + m_modelStack.push_back(model); + VisitModelResponse visitResponse = VisitLinksForModel(model); + if (m_recurseModels && visitResponse == VisitModelResponse::VisitNestedAndSiblings) { // Nested model link are only visited if the joint visitor returns true for (uint64_t modelIndex{}; modelIndex < model.ModelCount(); ++modelIndex) { - const sdf::Model* nestedModel = model.ModelByIndex(modelIndex); - if (nestedModel != nullptr) + if (const sdf::Model* nestedModel = model.ModelByIndex(modelIndex); nestedModel != nullptr) { - if (!VisitLinksForModel(*nestedModel)) + if (VisitModelResponse nestedVisitResponse = operator()(*nestedModel); + nestedVisitResponse >= VisitModelResponse::Stop) { // Sibling nested model links are only visited // if the joint visitor returns true @@ -132,31 +134,36 @@ namespace ROS2::Utils } } } + m_modelStack.pop_back(); + + return visitResponse; } private: //! Returns success by default //! But an invoked visitor can return false to halt further iteration - bool VisitLinksForModel(const sdf::Model& currentModel) + VisitModelResponse VisitLinksForModel(const sdf::Model& currentModel) { for (uint64_t linkIndex{}; linkIndex < currentModel.LinkCount(); ++linkIndex) { - const sdf::Link* link = currentModel.LinkByIndex(linkIndex); - if (link != nullptr) + if (const sdf::Link* link = currentModel.LinkByIndex(linkIndex); link != nullptr) { - if (!m_linkVisitorCB(*link)) + if (!m_linkVisitorCB(*link, m_modelStack)) { - return false; + return VisitModelResponse::Stop; } } } - return true; + return VisitModelResponse::VisitNestedAndSiblings; } public: LinkVisitorCallback m_linkVisitorCB; bool m_recurseModels{}; + private: + // Stack storing the current composition of models visited so far + ModelStack m_modelStack; }; VisitLinksForNestedModels_fn VisitLinksForNestedModels{}; @@ -171,17 +178,19 @@ namespace ROS2::Utils // Optionally it supports recursing nested models to visit their joints as well struct VisitJointsForNestedModels_fn { - void operator()(const sdf::Model& model) + VisitModelResponse operator()(const sdf::Model& model) { - if (VisitJointsForModel(model) && m_recurseModels) + m_modelStack.push_back(model); + VisitModelResponse visitResponse = VisitJointsForModel(model); + if (m_recurseModels && visitResponse == VisitModelResponse::VisitNestedAndSiblings) { // Nested model joints are only visited if the joint visitor returns true for (uint64_t modelIndex{}; modelIndex < model.ModelCount(); ++modelIndex) { - const sdf::Model* nestedModel = model.ModelByIndex(modelIndex); - if (nestedModel != nullptr) + if (const sdf::Model* nestedModel = model.ModelByIndex(modelIndex); nestedModel != nullptr) { - if (!VisitJointsForModel(*nestedModel)) + if (VisitModelResponse nestedVisitResponse = operator()(*nestedModel); + nestedVisitResponse >= VisitModelResponse::Stop) { // Sibling nested model joints are only visited // if the joint visitor returns true @@ -190,10 +199,13 @@ namespace ROS2::Utils } } } + m_modelStack.pop_back(); + + return visitResponse; } private: - bool VisitJointsForModel(const sdf::Model& currentModel) + VisitModelResponse VisitJointsForModel(const sdf::Model& currentModel) { for (uint64_t jointIndex{}; jointIndex < currentModel.JointCount(); ++jointIndex) { @@ -202,19 +214,22 @@ namespace ROS2::Utils // don't have an actual sdf::link in the parsed model if (joint != nullptr && currentModel.LinkNameExists(joint->ParentName()) && currentModel.LinkByName(joint->ChildName())) { - if (!m_jointVisitorCB(*joint)) + if (!m_jointVisitorCB(*joint, m_modelStack)) { - return false; + return VisitModelResponse::Stop; } } } - return true; + return VisitModelResponse::VisitNestedAndSiblings; } public: JointVisitorCallback m_jointVisitorCB; bool m_recurseModels{}; + private: + // Stack storing the current composition of models visited so far + ModelStack m_modelStack; }; VisitJointsForNestedModels_fn VisitJointsForNestedModels{}; @@ -227,10 +242,19 @@ namespace ROS2::Utils { using LinkMap = AZStd::unordered_map; LinkMap links; - auto GatherLinks = [&links](const sdf::Link& link) + auto GatherLinks = [&links](const sdf::Link& link, const ModelStack& modelStack) { - AZStd::string linkName(link.Name().c_str(), link.Name().size()); - links.insert_or_assign(AZStd::move(linkName), &link); + std::string fullyQualifiedLinkName; + // Prepend the Model names to the link name using the Name Scoping support in libsdformat + // http://sdformat.org/tutorials?tut=composition_proposal#1-3-name-scoping-and-cross-referencing + for (const sdf::Model& model : modelStack) + { + fullyQualifiedLinkName = sdf::JoinName(fullyQualifiedLinkName, model.Name()); + } + fullyQualifiedLinkName = sdf::JoinName(fullyQualifiedLinkName, link.Name()); + + AZStd::string azLinkName(fullyQualifiedLinkName.c_str(), fullyQualifiedLinkName.size()); + links.insert_or_assign(AZStd::move(azLinkName), &link); return true; }; @@ -242,10 +266,19 @@ namespace ROS2::Utils { using JointMap = AZStd::unordered_map; JointMap joints; - auto GatherJoints = [&joints](const sdf::Joint& joint) + auto GatherJoints = [&joints](const sdf::Joint& joint, const ModelStack& modelStack) { - AZStd::string jointName(joint.Name().c_str(), joint.Name().size()); - joints.insert_or_assign(AZStd::move(jointName), &joint); + std::string fullyQualifiedJointName; + // Prepend the Model names to the joint name using the Name Scoping support in libsdformat + // http://sdformat.org/tutorials?tut=composition_proposal#1-3-name-scoping-and-cross-referencing + for (const sdf::Model& model : modelStack) + { + fullyQualifiedJointName = sdf::JoinName(fullyQualifiedJointName, model.Name()); + } + fullyQualifiedJointName = sdf::JoinName(fullyQualifiedJointName, joint.Name()); + + AZStd::string azJointName(fullyQualifiedJointName.c_str(), fullyQualifiedJointName.size()); + joints.insert_or_assign(AZStd::move(azJointName), &joint); return true; }; @@ -258,7 +291,7 @@ namespace ROS2::Utils { using JointVector = AZStd::vector; JointVector joints; - auto GatherJointsWhereLinkIsChild = [&joints, linkName](const sdf::Joint& joint) + auto GatherJointsWhereLinkIsChild = [&joints, linkName](const sdf::Joint& joint, const ModelStack& modelStack) { if (AZStd::string_view jointChildName{ joint.ChildName().c_str(), joint.ChildName().size() }; jointChildName == linkName) { @@ -277,7 +310,7 @@ namespace ROS2::Utils { using JointVector = AZStd::vector; JointVector joints; - auto GatherJointsWhereLinkIsParent = [&joints, linkName](const sdf::Joint& joint) + auto GatherJointsWhereLinkIsParent = [&joints, linkName](const sdf::Joint& joint, const ModelStack& modelStack) { if (AZStd::string_view jointParentName{ joint.ParentName().c_str(), joint.ParentName().size() }; jointParentName == linkName) { @@ -305,11 +338,12 @@ namespace ROS2::Utils // The VisitModelResponse enum value is used to filter out // less callbacks the higher the value grows. // So any values above VisitNestedAndSiblings will not visit nested models - VisitModelResponse visitResponse = m_modelVisitorCB(model); + VisitModelResponse visitResponse = m_modelVisitorCB(model, m_modelStack); if (m_recurseModels && visitResponse == VisitModelResponse::VisitNestedAndSiblings) { // Nested models are only visited if the model visitor returns VisitNestedAndSiblings + m_modelStack.push_back(model); for (uint64_t modelIndex{}; modelIndex < model.ModelCount(); ++modelIndex) { if (const sdf::Model* nestedModel = model.ModelByIndex(modelIndex); nestedModel != nullptr) @@ -322,6 +356,7 @@ namespace ROS2::Utils } } } + m_modelStack.pop_back(); } return visitResponse; @@ -378,6 +413,9 @@ namespace ROS2::Utils public: ModelVisitorCallback m_modelVisitorCB; bool m_recurseModels{}; + private: + // Stack storing the current composition of models visited so far + ModelStack m_modelStack; }; VisitModelsForNestedModels_fn VisitModelsForNestedModels{}; @@ -389,7 +427,7 @@ namespace ROS2::Utils AZStd::unordered_set GetMeshesFilenames(const sdf::Root& root, bool visual, bool colliders) { AZStd::unordered_set filenames; - auto GetMeshesFromModel = [&filenames, visual, colliders](const sdf::Model& model) -> VisitModelResponse + auto GetMeshesFromModel = [&filenames, visual, colliders](const sdf::Model& model, const ModelStack&) -> VisitModelResponse { const auto addFilenameFromGeometry = [&filenames](const sdf::Geometry* geometry) { @@ -434,12 +472,13 @@ namespace ROS2::Utils return filenames; } - const sdf::Model* GetModelContainingLink(const sdf::Root& root, AZStd::string_view linkName) + const sdf::Model* GetModelContainingLink(const sdf::Root& root, AZStd::string_view fullyQualifiedLinkName) { const sdf::Model* resultModel{}; - auto IsLinkInModel = [&linkName, &resultModel](const sdf::Model& model) -> VisitModelResponse + auto IsLinkInModel = [&fullyQualifiedLinkName, + &resultModel](const sdf::Model& model, const ModelStack&) -> VisitModelResponse { - const std::string stdLinkName(linkName.data(), linkName.size()); + const std::string stdLinkName(fullyQualifiedLinkName.data(), fullyQualifiedLinkName.size()); if (const sdf::Link* searchLink = model.LinkByName(stdLinkName); searchLink != nullptr) { resultModel = &model; @@ -456,9 +495,9 @@ namespace ROS2::Utils const sdf::Model* GetModelContainingLink(const sdf::Root& root, const sdf::Link& link) { const sdf::Model* resultModel{}; - auto IsLinkInModel = [&link, &resultModel](const sdf::Model& model) -> VisitModelResponse + auto IsLinkInModel = [&link, &resultModel](const sdf::Model& model, const ModelStack&) -> VisitModelResponse { - if (const sdf::Link* searchLink = model.LinkByName(link.Name()); searchLink != nullptr) + if (const sdf::Link* searchLink = model.LinkByName(link.Name()); searchLink != &link) { resultModel = &model; return VisitModelResponse::Stop; @@ -471,12 +510,12 @@ namespace ROS2::Utils return resultModel; } - const sdf::Model* GetModelContainingJoint(const sdf::Root& root, AZStd::string_view jointName) + const sdf::Model* GetModelContainingJoint(const sdf::Root& root, AZStd::string_view fullyQualifiedJointName) { const sdf::Model* resultModel{}; - auto IsJointInModel = [&jointName, &resultModel](const sdf::Model& model) -> VisitModelResponse + auto IsJointInModel = [&fullyQualifiedJointName, &resultModel](const sdf::Model& model, const ModelStack&) -> VisitModelResponse { - const std::string stdJointName(jointName.data(), jointName.size()); + const std::string stdJointName(fullyQualifiedJointName.data(), fullyQualifiedJointName.size()); if (const sdf::Joint* searchJoint = model.JointByName(stdJointName); searchJoint != nullptr) { resultModel = &model; @@ -493,9 +532,9 @@ namespace ROS2::Utils const sdf::Model* GetModelContainingJoint(const sdf::Root& root, const sdf::Joint& joint) { const sdf::Model* resultModel{}; - auto IsJointInModel = [&joint, &resultModel](const sdf::Model& model) -> VisitModelResponse + auto IsJointInModel = [&joint, &resultModel](const sdf::Model& model, const ModelStack&) -> VisitModelResponse { - if (const sdf::Joint* searchJoint = model.JointByName(joint.Name()); searchJoint != nullptr) + if (const sdf::Joint* searchJoint = model.JointByName(joint.Name()); searchJoint != &joint) { resultModel = &model; return VisitModelResponse::Stop; @@ -562,12 +601,12 @@ namespace ROS2::Utils const AZ::IO::Path packageManifestPath = amentSharePath / packageName / "package.xml"; // Given a path like 'ambulance/meshes/model.stl', it will be considered a match if - // /share/ambulance/package.xml exists and + // /share/ambulance/package.xml exists and // /share/ambulance/meshes/model.stl exists. 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_Trace("ResolveAssetPath", R"(Resolved using AMENT_PREFIX_PATH: "%.*s" -> "%.*s")" "\n", AZ_PATH_ARG(unresolvedPath), AZ_PATH_ARG(candidateResolvedPath)); return candidateResolvedPath; } @@ -652,7 +691,7 @@ namespace ROS2::Utils { if (fileExistsCB(replacedUriPath)) { - AZ_Trace("ResolveAssetPath", R"(Resolved Absolute Path: "%.*s" -> "%.*s")" "\n", + AZ_Trace("ResolveAssetPath", R"(Resolved Absolute Path: "%.*s" -> "%.*s")" "\n", AZ_PATH_ARG(unresolvedPath), AZ_PATH_ARG(replacedUriPath)); return replacedUriPath; } @@ -669,7 +708,7 @@ namespace ROS2::Utils if (const AZ::IO::Path candidateResolvedPath = ancestorPath / replacedUriPath; fileExistsCB(candidateResolvedPath)) { - AZ_Trace("ResolveAssetPath", R"(Resolved using ancestor paths: "%.*s" -> "%.*s")" "\n", + AZ_Trace("ResolveAssetPath", R"(Resolved using ancestor paths: "%.*s" -> "%.*s")" "\n", AZ_PATH_ARG(unresolvedPath), AZ_PATH_ARG(candidateResolvedPath)); return candidateResolvedPath; } @@ -684,13 +723,13 @@ namespace ROS2::Utils { 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 {}; } @@ -702,12 +741,12 @@ namespace ROS2::Utils if (fileExistsCB(relativePath)) { - AZ_Trace("ResolveAssetPath", R"(Resolved Relative Path: "%.*s" -> "%.*s")" "\n", + 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_Trace("ResolveAssetPath", R"(Failed to resolve Relative Path: "%.*s" -> "%.*s")" "\n", AZ_PATH_ARG(unresolvedPath), AZ_PATH_ARG(relativePath)); return {}; } diff --git a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h index bf6a8a871..a3ca63a9d 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h +++ b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h @@ -43,9 +43,15 @@ namespace ROS2::Utils //! @returns root to entity transform AZ::Transform GetWorldTransformURDF(const sdf::Link* link, AZ::Transform t = AZ::Transform::Identity()); + //! Type Alias representing a "stack" of Model object that were visited on the way to the current Link/Joint Visitor Callback + using ModelStack = AZStd::deque>; + //! Callback which is invoke for each link within a model + //! @param link reference to link being visited + //! @param modelStack stack of references to the models and nested models that were visited to get to the current link. The stack will + //! always contain at least one element. The back() element of the stack returns the direct model the link is attached to //! @return Return true to continue visiting links or false to halt - using LinkVisitorCallback = AZStd::function; + using LinkVisitorCallback = AZStd::function; //! Visit links from URDF/SDF //! @param sdfModel Model object of SDF document corresponding to the tag. It used to query link //! @param visitNestedModelLinks When true recurses to any nested tags of the Model object and invoke visitor on their links as @@ -53,35 +59,36 @@ namespace ROS2::Utils //! @returns void void VisitLinks(const sdf::Model& sdfModel, const LinkVisitorCallback& linkVisitorCB, bool visitNestedModelLinks = false); - //! Retrieve all links in URDF/SDF as a map, where a key is link's name and a value is a pointer to link. + //! Retrieve all links in URDF/SDF as a map, where a key is link's fully qualified name and a value is a pointer to link. //! Allows to retrieve a pointer to a link given it name. //! @param sdfModel object of SDF document corresponding to the tag. It used to query links //! @param gatherNestedModelLinks When true recurses to any nested tags of the Model object and also gathers their links as well - //! @returns mapping from link name to link pointer + //! @returns mapping from fully qualified link name(such as "model_name::link_name") to link pointer AZStd::unordered_map GetAllLinks(const sdf::Model& sdfModel, bool gatherNestedModelLinks = false); //! Callback which is invoke for each valid joint for a given model + //! @param joint reference to joint being visited + //! @param modelStack stack of references to the models and nested models that were visited to get to the current joint. The stack will + //! always contain at least one element. The back() element of the stack returns the direct model the joint is attached to //! @return Return true to continue visiting joint or false to halt - using JointVisitorCallback = AZStd::function; + using JointVisitorCallback = AZStd::function; //! Visit joints from URDF/SDF //! @param sdfModel Model object of SDF document corresponding to the tag. It used to query joints - //! @param visitNestedModelJoints When true recurses to any nested tags of the Model object and invoke visitor on their joints - //! as well + //! @param visitNestedModelJoints When true recurses to any nested tags of the Model object and invoke visitor on their joint as well //! @returns void void VisitJoints(const sdf::Model& sdfModel, const JointVisitorCallback& jointVisitorCB, bool visitNestedModelJoints = false); - //! Retrieve all joints in URDF/SDF. + //! Retrieve all joints in URDF/SDF where the key is the full composed path following the name scoping proposal in SDF 1.8 + //! http://sdformat.org/tutorials?tut=composition_proposal#1-nesting-and-encapsulation //! @param sdfModel Model object of SDF document corresponding to the tag. It used to query joints - //! @param gatherNestedModelJoints When true recurses to any nested tags of the Model object and also gathers their joints as - //! well - //! @returns mapping from joint name to joint pointer + //! @param gatherNestedModelJoints When true recurses to any nested tags of the Model object and also gathers their joints as well + //! @returns mapping from fully qualified joint name(such as "model_name::joint_name") to joint pointer AZStd::unordered_map GetAllJoints(const sdf::Model& sdfModel, bool gatherNestedModelJoints = false); //! Retrieve all joints from URDF/SDF in which the specified link is a child in a sdf::Joint. //! @param sdfModel Model object of SDF document corresponding to the tag. It used to query joints //! @param linkName Name of link which to query in joint objects ChildName() - //! @param gatherNestedModelJoints When true recurses to any nested tags of the Model object and also gathers their joints as - //! well + //! @param gatherNestedModelJoints When true recurses to any nested tags of the Model object and also gathers their joints as well //! @returns vector of joints where link is a child AZStd::vector GetJointsForChildLink( const sdf::Model& sdfModel, AZStd::string_view linkName, bool gatherNestedModelJoints = false); @@ -89,8 +96,7 @@ namespace ROS2::Utils //! Retrieve all joints from URDF/SDF in which the specified link is a parent in a sdf::Joint. //! @param sdfModel Model object of SDF document corresponding to the tag. It used to query joints //! @param linkName Name of link which to query in joint objects ParentName() - //! @param gatherNestedModelJoints When true recurses to any nested tags of the Model object and also gathers their joints as - //! well + //! @param gatherNestedModelJoints When true recurses to any nested tags of the Model object and also gathers their joints as well //! @returns vector of joints where link is a parent AZStd::vector GetJointsForParentLink( const sdf::Model& sdfModel, AZStd::string_view linkName, bool gatherNestedModelJoints = false); @@ -111,8 +117,11 @@ namespace ROS2::Utils //! Callback which is invoke for each model in the SDF //! This function visits any tags in the root of the SDF XML content //! as well as any tags in any tags that are in the root of the SDF XML content + //! @param model reference to SDF model object being visited + //! @param modelStack stack of references to the models were visited to get to the current model + //! NOTE: Unlike the Link/Joint visitor the modelStack can be empty (i.e for the root model or a model that is a child of a ) //! @return Return true to continue visiting models or false to halt - using ModelVisitorCallback = AZStd::function; + using ModelVisitorCallback = AZStd::function; //! Visit Models from URDF/SDF //! @param sdfRoot Root object of SDF document //! @param visitNestedModels When true recurses to any nested tags of the Model objects and invoke the visitor on them @@ -121,26 +130,32 @@ namespace ROS2::Utils //! Retrieve all meshes referenced in URDF as unresolved URDF patches. //! Note that returned filenames are unresolved URDF patches. - //! @param root - reference to SDF Root object representing the root of the parsed SDF xml document - //! @param visual - search for visual meshes. - //! @param colliders - search for collider meshes. + //! @param root reference to SDF Root object representing the root of the parsed SDF xml document + //! @param visual search for visual meshes. + //! @param colliders search for collider meshes. //! @returns set of meshes' filenames. AZStd::unordered_set GetMeshesFilenames(const sdf::Root& root, bool visual, bool colliders); //! Returns the SDF model object which contains the specified link - //! @param root - reference to SDF Root object representing the root of the parsed SDF xml document - //! @param linkName - Name of SDF link to lookup in the SDF document + //! @param root reference to SDF Root object representing the root of the parsed SDF xml document + //! @param linkName Fully qualified name of SDF link to lookup in the SDF document + //! A fully qualified name has the form "modelname1::nested_modelname1::linkname" + //! This is detailed in the SDF format composition proposal: http://sdformat.org/tutorials?tut=composition_proposal#motivation const sdf::Model* GetModelContainingLink(const sdf::Root& root, AZStd::string_view linkName); - //! @param root - reference to SDF Root object representing the root of the parsed SDF xml document - //! @param link - SDF link object to lookup in the SDF document + + //! @param root reference to SDF Root object representing the root of the parsed SDF xml document + //! @param link SDF link object to lookup in the SDF document const sdf::Model* GetModelContainingLink(const sdf::Root& root, const sdf::Link& link); //! Returns the SDF model object which contains the specified joint - //! @param root - reference to SDF Root object representing the root of the parsed SDF xml document - //! @param jointName - Name of SDF joint to lookup in the SDF document + //! @param root reference to SDF Root object representing the root of the parsed SDF xml document + //! @param jointName Name of SDF joint to lookup in the SDF document + //! A fully qualified name has the form "modelname1::nested_modelname1::jointname" + //! This is detailed in the SDF format composition proposal: http://sdformat.org/tutorials?tut=composition_proposal#motivation const sdf::Model* GetModelContainingJoint(const sdf::Root& root, AZStd::string_view jointName); - //! @param root - reference to SDF Root object representing the root of the parsed SDF xml document - //! @param jointName - Name of SDF joint to lookup in the SDF document + + //! @param root reference to SDF Root object representing the root of the parsed SDF xml document + //! @param joint SDF joint pointer to lookup in the SDF document const sdf::Model* GetModelContainingJoint(const sdf::Root& root, const sdf::Joint& joint); //! Callback used to check for file exist of a path referenced within a URDF/SDF file @@ -149,11 +164,11 @@ namespace ROS2::Utils using FileExistsCB = AZStd::function; //! Resolves path for an asset referenced in a URDF/SDF file. - //! @param unresolvedPath - unresolved URDF/SDF path, example : `model://meshes/foo.dae`. - //! @param baseFilePath - the absolute path of URDF/SDF file which contains the path that is to be resolved. - //! @param amentPrefixPath - the string that contains available packages' path, separated by ':' signs. - //! @param settings - the asset path resolution settings to use for attempting to locate the correct files - //! @param fileExists - functor to check if the given file exists. Exposed for unit test, default one should be used. + //! @param unresolvedPath unresolved URDF/SDF path, example : `model://meshes/foo.dae`. + //! @param baseFilePath the absolute path of URDF/SDF file which contains the path that is to be resolved. + //! @param amentPrefixPath the string that contains available packages' path, separated by ':' signs. + //! @param settings the asset path resolution settings to use for attempting to locate the correct files + //! @param fileExists functor to check if the given file exists. Exposed for unit test, default one should be used. //! @returns resolved path to the referenced file within the URDF/SDF, or the passed-in path if no resolution was possible. AZ::IO::Path ResolveAssetPath( AZ::IO::Path unresolvedPath, diff --git a/Gems/ROS2/Code/Tests/SdfParserTest.cpp b/Gems/ROS2/Code/Tests/SdfParserTest.cpp index de25cc98e..69703bda2 100644 --- a/Gems/ROS2/Code/Tests/SdfParserTest.cpp +++ b/Gems/ROS2/Code/Tests/SdfParserTest.cpp @@ -178,6 +178,21 @@ namespace UnitTest )"; } + + static std::string GetSdfWithMultipleModelsThatHaveLinksWithTheSameName() + { + return R"( + + + + + + + + + + )"; + } }; TEST_F(SdfParserTest, SdfWithDuplicateModelNames_ResultsInError) @@ -231,10 +246,11 @@ namespace UnitTest AZStd::vector models; // Test visitation return results. All model siblings and nested models are visited - auto StoreModelAndVisitNestedModelsAndSiblings = [&models](const sdf::Model& model) -> ROS2::Utils::VisitModelResponse + auto StoreModelAndVisitNestedModelsAndSiblings = + [&models](const sdf::Model& model, const ROS2::Utils::ModelStack&) -> ROS2::Utils::VisitModelResponse { - models.push_back(&model); - return ROS2::Utils::VisitModelResponse::VisitNestedAndSiblings; + models.push_back(&model); + return ROS2::Utils::VisitModelResponse::VisitNestedAndSiblings; }; ROS2::Utils::VisitModels(sdfRoot, StoreModelAndVisitNestedModelsAndSiblings); @@ -248,10 +264,11 @@ namespace UnitTest // In this case only models directly on the SDF root object // or directory child of the sdf world has there models visited models.clear(); - auto StoreModelAndVisitSiblings = [&models](const sdf::Model& model) -> ROS2::Utils::VisitModelResponse + auto StoreModelAndVisitSiblings = + [&models](const sdf::Model& model, const ROS2::Utils::ModelStack&) -> ROS2::Utils::VisitModelResponse { - models.push_back(&model); - return ROS2::Utils::VisitModelResponse::VisitSiblings; + models.push_back(&model); + return ROS2::Utils::VisitModelResponse::VisitSiblings; }; ROS2::Utils::VisitModels(sdfRoot, StoreModelAndVisitSiblings); @@ -263,10 +280,10 @@ namespace UnitTest // Visit only the first model and stop any futher visitation models.clear(); - auto StoreModelAndStop = [&models](const sdf::Model& model) -> ROS2::Utils::VisitModelResponse + auto StoreModelAndStop = [&models](const sdf::Model& model, const ROS2::Utils::ModelStack&) -> ROS2::Utils::VisitModelResponse { - models.push_back(&model); - return ROS2::Utils::VisitModelResponse::Stop; + models.push_back(&model); + return ROS2::Utils::VisitModelResponse::Stop; }; ROS2::Utils::VisitModels(sdfRoot, StoreModelAndStop); @@ -274,6 +291,38 @@ namespace UnitTest EXPECT_EQ("root_model", models[0]->Name()); } + TEST_F(SdfParserTest, VisitingSdfWithMultipleModelsWithSameLinkName_VisitsAllLinks) + { + const auto xmlStr = GetSdfWithMultipleModelsThatHaveLinksWithTheSameName(); + const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, {}); + ASSERT_TRUE(sdfRootOutcome); + const auto& sdfRoot = sdfRootOutcome.GetRoot(); + // The SDF should also have a single world + ASSERT_EQ(1, sdfRoot.WorldCount()); + const auto* sdfWorld = sdfRoot.WorldByIndex(0); + ASSERT_NE(nullptr, sdfWorld); + + // There should be two models of "my_model" and "your_model" + EXPECT_EQ(2, sdfWorld->ModelCount()); + + const auto* myModel = sdfWorld->ModelByName("my_model"); + ASSERT_NE(nullptr, myModel); + EXPECT_TRUE(myModel->LinkNameExists("same_link_name")); + + const auto* yourModel = sdfWorld->ModelByName("your_model"); + ASSERT_NE(nullptr, yourModel); + EXPECT_TRUE(yourModel->LinkNameExists("same_link_name")); + + // Make sure that all links are gathered + AZStd::unordered_map links = ROS2::Utils::GetAllLinks(*myModel, true); + auto otherLinks = ROS2::Utils::GetAllLinks(*yourModel, true); + links.insert(AZStd::move(otherLinks.begin()), AZStd::move(otherLinks.end())); + + ASSERT_EQ(2, links.size()); + EXPECT_TRUE(links.contains("my_model::same_link_name")); + EXPECT_TRUE(links.contains("your_model::same_link_name")); + } + TEST_F(SdfParserTest, CheckModelCorrectness) { { diff --git a/Gems/ROS2/Code/Tests/UrdfParserTest.cpp b/Gems/ROS2/Code/Tests/UrdfParserTest.cpp index dbbfb558f..65136e524 100644 --- a/Gems/ROS2/Code/Tests/UrdfParserTest.cpp +++ b/Gems/ROS2/Code/Tests/UrdfParserTest.cpp @@ -539,8 +539,7 @@ namespace UnitTest TEST_F(UrdfParserTest, ParseUrdf_WithRootLink_WithName_world_DoesNotContain_world_Link) { - // The libsdformat URDF parser skips converting the root link if its - // name is "world" + // The libsdformat URDF parser skips converting the root link if its name is "world" // https://github.com/gazebosim/sdformat/blob/a1027c3ed96f2f663760df10f13b06f47f922c55/src/parser_urdf.cc#L3385-L3399 // Therefore it will not be part of joint reduction constexpr const char* RootLinkName = "world"; @@ -576,7 +575,7 @@ namespace UnitTest // Check the ROS2 visitor logic to make sure the joint with "world" parent link isn't visited auto joints = ROS2::Utils::GetAllJoints(*model); EXPECT_EQ(1, joints.size()); - EXPECT_THAT(joints, ::testing::UnorderedPointwise(UnorderedMapKeyMatcher(), { "base_inertia_child_joint" })); + EXPECT_THAT(joints, ::testing::UnorderedPointwise(UnorderedMapKeyMatcher(), { "FooRobot::base_inertia_child_joint" })); // The libsdformat sdf::Model::JointCount function however returns 2 // as the "world_base_joint" is skipped over by joint reduction @@ -601,10 +600,9 @@ namespace UnitTest EXPECT_EQ("FooRobot", model->Name()); ASSERT_NE(nullptr, model); - // Due to joint reduction there should be 2 links and 43 frames + // Due to joint reduction there should be 2 links and 4 frames // This is different from the previous test, as the root link - // can now partipate in joint reduction because it has a name - // that isn't "world" + // can now participate in joint reduction because its name isn't "world" ASSERT_EQ(2, model->LinkCount()); ASSERT_EQ(4, model->FrameCount()); @@ -625,7 +623,7 @@ namespace UnitTest // there should only be a single one of the revolute joint auto joints = ROS2::Utils::GetAllJoints(*model); EXPECT_EQ(1, joints.size()); - EXPECT_THAT(joints, ::testing::UnorderedPointwise(UnorderedMapKeyMatcher(), { "base_inertia_child_joint" })); + EXPECT_THAT(joints, ::testing::UnorderedPointwise(UnorderedMapKeyMatcher(), { "FooRobot::base_inertia_child_joint" })); // The libsdformat sdf::Model::JointCount function should match // the ROS2 Visitor logic as the root link of "not_world" is part of joint reduction @@ -741,12 +739,12 @@ namespace UnitTest // into the base_link of the SDF // However there are Frames for the combined links and joints EXPECT_EQ(links.size(), 3); - ASSERT_TRUE(links.contains("base_link")); - ASSERT_TRUE(links.contains("link2")); - ASSERT_TRUE(links.contains("link3")); - EXPECT_EQ("base_link", links.at("base_link")->Name()); - EXPECT_EQ("link2", links.at("link2")->Name()); - EXPECT_EQ("link3", links.at("link3")->Name()); + ASSERT_TRUE(links.contains("complicated::base_link")); + ASSERT_TRUE(links.contains("complicated::link2")); + ASSERT_TRUE(links.contains("complicated::link3")); + EXPECT_EQ("base_link", links.at("complicated::base_link")->Name()); + EXPECT_EQ("link2", links.at("complicated::link2")->Name()); + EXPECT_EQ("link3", links.at("complicated::link3")->Name()); // Check that the frame names exist on the model EXPECT_TRUE(model->FrameNameExists("joint_bs")); @@ -764,8 +762,8 @@ namespace UnitTest ASSERT_NE(nullptr, model); auto joints = ROS2::Utils::GetAllJoints(*model); EXPECT_EQ(2, joints.size()); - ASSERT_TRUE(joints.contains("joint0")); - ASSERT_TRUE(joints.contains("joint1")); + ASSERT_TRUE(joints.contains("complicated::joint0")); + ASSERT_TRUE(joints.contains("complicated::joint1")); } TEST_F(UrdfParserTest, TestTransforms) @@ -781,12 +779,12 @@ namespace UnitTest // The "link1" is combined with the base_link through // joint reduction in the URDF->SDF parser logic // https://github.com/gazebosim/sdformat/issues/1110 - ASSERT_TRUE(links.contains("base_link")); - ASSERT_TRUE(links.contains("link2")); - ASSERT_TRUE(links.contains("link3")); - const auto base_link_ptr = links.at("base_link"); - const auto link2_ptr = links.at("link2"); - const auto link3_ptr = links.at("link3"); + ASSERT_TRUE(links.contains("complicated::base_link")); + ASSERT_TRUE(links.contains("complicated::link2")); + ASSERT_TRUE(links.contains("complicated::link3")); + const auto base_link_ptr = links.at("complicated::base_link"); + const auto link2_ptr = links.at("complicated::link2"); + const auto link3_ptr = links.at("complicated::link3"); // values exported from Blender const AZ::Vector3 expected_translation_link1{ 0.0, 0.0, 0.0 }; @@ -861,7 +859,7 @@ namespace UnitTest TEST_F(UrdfParserTest, TestPathResolve_ValidAbsolutePath_ResolvesCorrectly) { - // Verify that an absolute path that wouldn't be resolved by prefixes or ancestor paths + // Verify that an absolute path that wouldn't be resolved by prefixes or ancestor paths // or the AMENT_PREFIX_PATH will still resolve correctly as long as the absolute path exists // (as determined by the mocked-out FileExistsCallback below). constexpr AZ::IO::PathView dae = "file:///usr/ros/humble/meshes/bar.dae"; @@ -944,7 +942,7 @@ namespace UnitTest [](const AZ::IO::PathView& p) -> bool { // For an AMENT_PREFIX_PATH to be a valid match, the share//package.xml and share/ - // both need to exist. We'll return that both exist, but since the dae file entry doesn't start with + // both need to exist. We'll return that both exist, but since the dae file entry doesn't start with // "package://" or "model://", it shouldn't get resolved. return (p == AZ::IO::PathView("/ament/path2/share/robot/package.xml")) || (p == "/ament/path2/share/robot/meshes/bar.dae"); }); @@ -991,7 +989,7 @@ namespace UnitTest constexpr AZ::IO::PathView dae = "package://meshes/bar.dae"; constexpr AZ::IO::PathView urdf = "/home/foo/ros_ws/install/foo_robot/description/foo_robot.urdf"; constexpr AZ::IO::PathView resolvedDae = "/home/foo/ros_ws/install/foo_robot/meshes/bar.dae"; - + auto settings = GetTestSettings(); settings.m_resolverSettings.m_useAncestorPaths = false; From 2940787c3cbd9efde062164e906f3aa35b0d46fb Mon Sep 17 00:00:00 2001 From: lumberyard-employee-dm <56135373+lumberyard-employee-dm@users.noreply.github.com> Date: Thu, 28 Sep 2023 15:43:01 -0500 Subject: [PATCH 2/8] Updating ROS2 Gem to use libsdformat-13.5.0-rev2 (#550) Updating ROS2 Gem to use libsdformat-13.5.0-rev2 The new revision of libsdformat comes with debug symbols, that can be used to get symbol information when debugging the RobotImporter using lldb/gdb Signed-off-by: lumberyard-employee-dm <56135373+lumberyard-employee-dm@users.noreply.github.com> --- Gems/ROS2/Code/Platform/Linux/PAL_linux.cmake | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Gems/ROS2/Code/Platform/Linux/PAL_linux.cmake b/Gems/ROS2/Code/Platform/Linux/PAL_linux.cmake index 63d6c4acd..1bf09bc01 100644 --- a/Gems/ROS2/Code/Platform/Linux/PAL_linux.cmake +++ b/Gems/ROS2/Code/Platform/Linux/PAL_linux.cmake @@ -9,11 +9,11 @@ set(PAL_TRAIT_BUILD_ROS2_GEM_SUPPORTED TRUE) if(${CMAKE_SYSTEM_PROCESSOR} STREQUAL "x86_64") - ly_associate_package(PACKAGE_NAME sdformat-13.5.0-rev1-linux + ly_associate_package(PACKAGE_NAME sdformat-13.5.0-rev2-linux TARGETS sdformat - PACKAGE_HASH 71a86e255cfc7a176f6087e069fb26c9837c98e277becf87f3a4b0e25fe90bd3) + PACKAGE_HASH b8e988954c07f41b99ba0950da3f4d3e8489ffabaaff157f79ab0c716e2142e0) elseif(${CMAKE_SYSTEM_PROCESSOR} STREQUAL "aarch64") - ly_associate_package(PACKAGE_NAME sdformat-13.5.0-rev1-linux-aarch64 + ly_associate_package(PACKAGE_NAME sdformat-13.5.0-rev2-linux-aarch64 TARGETS sdformat - PACKAGE_HASH c8ca2ffad604162cbc8ed852c8ea8695851c4c69c5f39f2a9d8e423511ce78f9) + PACKAGE_HASH 7e51cc60c61a058c1f8aba7277574946ab974af3ff4601884e72380e8585c0ea) endif() From 376eb8e6d49f6adb560ef47281d4a8648e5bd78e Mon Sep 17 00:00:00 2001 From: lumberyard-employee-dm <56135373+lumberyard-employee-dm@users.noreply.github.com> Date: Wed, 27 Sep 2023 17:25:33 -0500 Subject: [PATCH 3/8] Adding back inadvertent code deletion from the RobotImporterWidget.cpp (#547) Adding back inadvertent code deletion from the RobotImporterWidget.cpp The `RobotImporterWidget::CreatePrefab` function had it's call to `CheckCyclicalDependency` removed as part of a conflict when resolving a rebase. Signed-off-by: lumberyard-employee-dm <56135373+lumberyard-employee-dm@users.noreply.github.com> --- Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp index 87e5178fb..8d808c2f0 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp @@ -439,6 +439,7 @@ namespace ROS2 const AZ::IO::Path prefabPath(AZ::IO::Path(AZ::Utils::GetProjectPath()) / prefabPathRelative); bool fileExists = AZ::IO::FileIOBase::GetInstance()->Exists(prefabPath.c_str()); + if (CheckCyclicalDependency(prefabPathRelative)) { m_prefabMakerPage->setSuccess(false); return; From d54993918efd85cdf93f67a6b26cfbb0ce742178 Mon Sep 17 00:00:00 2001 From: lumberyard-employee-dm <56135373+lumberyard-employee-dm@users.noreply.github.com> Date: Mon, 2 Oct 2023 11:17:18 -0500 Subject: [PATCH 4/8] SDF Nested Model Support Changes (#561) Updated the Check Asset Page to not add Assets with empty source paths or null Asset IDs to the list of resolved assets Signed-off-by: lumberyard-employee-dm <56135373+lumberyard-employee-dm@users.noreply.github.com> Added a `GetModelContainingModel` function that can return the parent model of a nested model Signed-off-by: lumberyard-employee-dm <56135373+lumberyard-employee-dm@users.noreply.github.com> Updated the URDFPrefabMaker to create an Entity for which represents the URDF/SDF model itself. The AddEntitiesForLink function has been updated to to pass in the model that the link is attached to and in order to allow the CollidersMaker, ArticulationsMaker and SensorsMaker have access to the Model Signed-off-by: lumberyard-employee-dm <56135373+lumberyard-employee-dm@users.noreply.github.com> Addressed sentence typos in comments. Signed-off-by: lumberyard-employee-dm <56135373+lumberyard-employee-dm@users.noreply.github.com> Fixed the joint iteration for loop in `CreatePrefabTemplateFromUrdfOrSdf` The code in hee for loop was converted from a lambda to a for loop, but the return statements weren't converted. Signed-off-by: lumberyard-employee-dm <56135373+lumberyard-employee-dm@users.noreply.github.com> --- .../RobotImporter/Pages/CheckAssetPage.cpp | 13 +- .../RobotImporter/RobotImporterWidget.cpp | 13 +- .../RobotImporter/URDF/URDFPrefabMaker.cpp | 152 +++++++++++++++--- .../RobotImporter/URDF/URDFPrefabMaker.h | 3 +- .../Utils/RobotImporterUtils.cpp | 24 ++- .../RobotImporter/Utils/RobotImporterUtils.h | 10 +- 6 files changed, 176 insertions(+), 39 deletions(-) diff --git a/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckAssetPage.cpp b/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckAssetPage.cpp index d555e9a1f..4754f93af 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckAssetPage.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckAssetPage.cpp @@ -105,7 +105,9 @@ namespace ROS2 int i = m_table->rowCount(); m_table->setRowCount(i + 1); - bool isOk = assetSourcePath.has_value() && resolvedSdfPath.has_value(); + // The Asset ID GUID must not be null(all zeros) and the asset source path must not be empty + bool isOk = (assetSourcePath.has_value() && !assetSourcePath->empty() && assetSourcePath != "not found") + && (resolvedSdfPath.has_value() && !resolvedSdfPath->empty()) && !assetUuid.IsNull(); if (!isOk) { m_missingCount++; @@ -154,8 +156,13 @@ namespace ROS2 m_table->item(i, Columns::ResolvedMeshPath)->setIcon(m_failureIcon); m_table->setItem(i, Columns::ProductAsset, createCell(false, QString())); } - m_assetsPaths.push_back(assetSourcePath ? *assetSourcePath : AZStd::string()); - m_assetsUuids.push_back(assetUuid); + + // Don't add an empty asset path to the list of resolved assets + if (isOk) + { + m_assetsPaths.push_back(AZStd::move(*assetSourcePath)); + m_assetsUuids.push_back(assetUuid); + } } void CheckAssetPage::StartWatchAsset() diff --git a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp index 8d808c2f0..87835a3e3 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp @@ -330,14 +330,11 @@ namespace ROS2 type = tr("Collider"); } - if (m_urdfAssetsMapping->contains(meshPath)) - { - const auto& asset = m_urdfAssetsMapping->at(meshPath); - sourceAssetUuid = asset.m_availableAssetInfo.m_sourceGuid; - sourcePath = asset.m_availableAssetInfo.m_sourceAssetRelativePath.String(); - resolvedPath = asset.m_resolvedUrdfPath.String(); - crc = asset.m_urdfFileCRC; - } + const auto& asset = m_urdfAssetsMapping->at(meshPath); + sourceAssetUuid = asset.m_availableAssetInfo.m_sourceGuid; + sourcePath = asset.m_availableAssetInfo.m_sourceAssetRelativePath.String(); + resolvedPath = asset.m_resolvedUrdfPath.String(); + crc = asset.m_urdfFileCRC; } m_assetPage->ReportAsset(sourceAssetUuid, meshPath, type, sourcePath, crc, resolvedPath); } diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp index e3fa2d979..41042891f 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp @@ -25,8 +25,9 @@ #include #include #include +#include #include -#include +#include namespace ROS2 { @@ -183,10 +184,43 @@ namespace ROS2 // Build up a list of all entities created as a part of processing the file. AZStd::vector createdEntities; AZStd::unordered_map createdLinks; + AZStd::unordered_map createdModels; AZStd::unordered_map links; - for ([[maybe_unused]] const auto& [fullLinkName, linkPtr, _] : linksMapper.m_links) + for ([[maybe_unused]] const auto& [fullLinkName, linkPtr, attachedModel] : linksMapper.m_links) { - createdLinks[linkPtr] = AddEntitiesForLink(linkPtr, AZ::EntityId{}, createdEntities); + // Create entities for the model containing the link + AZ::EntityId modelEntityId; + if (attachedModel != nullptr) + { + if (auto modelIt = createdModels.find(attachedModel); modelIt != createdModels.end()) + { + if (modelIt->second.IsSuccess()) + { + modelEntityId = modelIt->second.GetValue(); + } + } + else + { + if (AzToolsFramework::Prefab::PrefabEntityResult createModelEntityResult = CreateEntityForModel(*attachedModel); + createModelEntityResult) + { + modelEntityId = createModelEntityResult.GetValue(); + // Add the model entity to the created entity list + // so that it gets added to the prefab + createdEntities.emplace_back(modelEntityId); + + std::string modelName = attachedModel->Name(); + AZStd::string azModelName(modelName.c_str(), modelName.size()); + AZStd::lock_guard lck(m_statusLock); + m_status.emplace(azModelName, AZStd::string::format("created as: %s", modelEntityId.ToString().c_str())); + createdModels.emplace(attachedModel, createModelEntityResult); + } + } + + } + + // Add all link as children of their attached model entity by default + createdLinks[linkPtr] = AddEntitiesForLink(*linkPtr, attachedModel, modelEntityId, createdEntities); } for (const auto& [linkPtr, result] : createdLinks) @@ -345,7 +379,7 @@ namespace ROS2 "Joint %s has no parent link %s. Cannot create", azJointName.c_str(), parentLinkName.c_str()); - return true; + continue; } auto leadEntity = parentEntityIter->second; @@ -361,7 +395,7 @@ namespace ROS2 "Joint %s has no child link %s. Cannot create", azJointName.c_str(), childLinkName.c_str()); - return true; + continue; } auto childEntity = childEntityIter->second; @@ -389,14 +423,13 @@ namespace ROS2 AZStd::lock_guard lck(m_statusLock); auto result = m_jointsMaker.AddJointComponent(jointPtr, childEntity.GetValue(), leadEntity.GetValue()); m_status.emplace( - azJointName, AZStd::string::format(" %s %llu", result.IsSuccess() ? "created as" : "Failed", result.GetValue())); + azJointName, AZStd::string::format(" %s: %llu", result.IsSuccess() ? "created as" : "failed", result.GetValue())); } else { AZ_Warning("CreatePrefabFromUrdfOrSdf", false, "cannot create joint %s", azJointName.c_str()); } } - return true; } // Use the first entity based on a link that is not parented to any other link @@ -493,15 +526,91 @@ namespace ROS2 return result; } - AzToolsFramework::Prefab::PrefabEntityResult URDFPrefabMaker::AddEntitiesForLink( - const sdf::Link* link, AZ::EntityId parentEntityId, AZStd::vector& createdEntities) + AzToolsFramework::Prefab::PrefabEntityResult URDFPrefabMaker::CreateEntityForModel(const sdf::Model& model) { - if (!link) + auto createEntityResult = PrefabMakerUtils::CreateEntity(AZ::EntityId{}, model.Name().c_str()); + if (!createEntityResult.IsSuccess()) { - AZ::Failure(AZStd::string("Failed to create prefab entity - link is null")); + return createEntityResult; + } + + // Get the model entity and update its transform component with the pose information + AZ::EntityId entityId = createEntityResult.GetValue(); + AZStd::unique_ptr entity(AzToolsFramework::GetEntityById(entityId)); + + if (auto* transformComponent = entity->FindComponent(); + transformComponent != nullptr) + { + gz::math::Pose3d modelPose; + if (sdf::Errors poseResolveErrors = model.SemanticPose().Resolve(modelPose); !poseResolveErrors.empty()) + { + AZStd::string poseErrorMessages = Utils::JoinSdfErrorsToString(poseResolveErrors); + + auto poseErrorsForModel = AZStd::string::format( + R"(Unable to resolve semantic pose for model %s. Creation of Model entity has failed. Errors: "%s")", + model.Name().c_str(), + poseErrorMessages.c_str()); + + return AZ::Failure(poseErrorsForModel); + } + + AZ::Transform modelTransform = URDF::TypeConversions::ConvertPose(modelPose); + + // If the model is nested below another model, check if it has a placement frame + if (const sdf::Model* parentModel = Utils::GetModelContainingModel(*m_root, model); parentModel != nullptr) + { + if (const sdf::Frame* modelPlacementFrame = parentModel->FrameByName(model.PlacementFrameName()); modelPlacementFrame != nullptr) + { + gz::math::Pose3d placementFramePose; + if (sdf::Errors poseResolveErrors = modelPlacementFrame->SemanticPose().Resolve(placementFramePose); + !poseResolveErrors.empty()) + { + AZStd::string poseErrorMessages = Utils::JoinSdfErrorsToString(poseResolveErrors); + + auto poseErrorsForModel = AZStd::string::format( + R"(Unable to resolve semantic pose for model %s parent model %s. Creation of Model entity has failed. Errors: "%s")", + model.Name().c_str(), + parentModel->Name().c_str(), + poseErrorMessages.c_str()); + + return AZ::Failure(poseErrorsForModel); + } + + modelTransform = modelTransform * URDF::TypeConversions::ConvertPose(placementFramePose); + } + } + + if (const sdf::Frame* implicitFrame = model.FrameByName("__model__"); implicitFrame != nullptr) + { + gz::math::Pose3d implicitFramePose; + if (sdf::Errors poseResolveErrors = implicitFrame->SemanticPose().Resolve(implicitFramePose); !poseResolveErrors.empty()) + { + AZStd::string poseErrorMessages = Utils::JoinSdfErrorsToString(poseResolveErrors); + + auto poseErrorsForModel = AZStd::string::format( + R"(Unable to resolve semantic pose for model's implicit frame %s. Creation of Model entity has failed. Errors: "%s")", + implicitFrame->Name().c_str(), + poseErrorMessages.c_str()); + + return AZ::Failure(poseErrorsForModel); + } + + modelTransform = modelTransform * URDF::TypeConversions::ConvertPose(implicitFramePose); + } + + transformComponent->SetWorldTM(AZStd::move(modelTransform)); } - auto createEntityResult = PrefabMakerUtils::CreateEntity(parentEntityId, link->Name().c_str()); + // Allow the created model entity to persist if there are no errors at ths point + entity.release(); + + return entityId; + } + + AzToolsFramework::Prefab::PrefabEntityResult URDFPrefabMaker::AddEntitiesForLink( + const sdf::Link& link, const sdf::Model* attachedModel, AZ::EntityId parentEntityId, AZStd::vector& createdEntities) + { + auto createEntityResult = PrefabMakerUtils::CreateEntity(parentEntityId, link.Name().c_str()); if (!createEntityResult.IsSuccess()) { return createEntityResult; @@ -516,30 +625,27 @@ namespace ROS2 { auto* component = Utils::GetGameOrEditorComponent(entity); AZ_Assert(component, "ROS2 Frame Component does not exist for %s", entityId.ToString().c_str()); - component->SetFrameID(AZStd::string(link->Name().c_str(), link->Name().size())); + component->SetFrameID(AZStd::string(link.Name().c_str(), link.Name().size())); } - auto createdVisualEntities = m_visualsMaker.AddVisuals(link, entityId); + auto createdVisualEntities = m_visualsMaker.AddVisuals(&link, entityId); createdEntities.insert(createdEntities.end(), createdVisualEntities.begin(), createdVisualEntities.end()); - // Get the SDF model where this link is a child - const sdf::Model* modelContainingLink = Utils::GetModelContainingLink(*m_root, *link); - if (!m_useArticulations) { - m_inertialsMaker.AddInertial(link->Inertial(), entityId); + m_inertialsMaker.AddInertial(link.Inertial(), entityId); } else { - if (modelContainingLink != nullptr) + if (attachedModel != nullptr) { - m_articulationsMaker.AddArticulationLink(*modelContainingLink, link, entityId); + m_articulationsMaker.AddArticulationLink(*attachedModel, &link, entityId); } } - if (modelContainingLink != nullptr) + if (attachedModel != nullptr) { - m_collidersMaker.AddColliders(*modelContainingLink, link, entityId); - m_sensorsMaker.AddSensors(*modelContainingLink, link, entityId); + m_collidersMaker.AddColliders(*attachedModel, &link, entityId); + m_sensorsMaker.AddSensors(*attachedModel, &link, entityId); } return AZ::Success(entityId); } diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h index 2cbbb65a8..78bb7f196 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h @@ -70,7 +70,8 @@ namespace ROS2 AZStd::string GetStatus(); private: - AzToolsFramework::Prefab::PrefabEntityResult AddEntitiesForLink(const sdf::Link* link, AZ::EntityId parentEntityId, AZStd::vector& createdEntities); + AzToolsFramework::Prefab::PrefabEntityResult CreateEntityForModel(const sdf::Model& model); + AzToolsFramework::Prefab::PrefabEntityResult AddEntitiesForLink(const sdf::Link& link, const sdf::Model* attachedModel, AZ::EntityId parentEntityId, AZStd::vector& createdEntities); void AddRobotControl(AZ::EntityId rootEntityId); static void MoveEntityToDefaultSpawnPoint(const AZ::EntityId& rootEntityId, AZStd::optional spawnPosition); diff --git a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp index 16b3c87e2..de7aa9e9a 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp @@ -434,9 +434,10 @@ namespace ROS2::Utils if (geometry->Type() == sdf::GeometryType::MESH) { auto pMesh = geometry->MeshShape(); - if (pMesh) + std::string meshUri = pMesh->Uri(); + if (pMesh && !meshUri.empty()) { - filenames.insert(AZStd::string(pMesh->Uri().c_str(), pMesh->Uri().size())); + filenames.emplace(meshUri.c_str(), meshUri.size()); } } }; @@ -547,6 +548,25 @@ namespace ROS2::Utils return resultModel; } + const sdf::Model* GetModelContainingModel(const sdf::Root& root, const sdf::Model& model) + { + const sdf::Model* resultModel{}; + auto IsModelInModel = [&model, &resultModel](const sdf::Model& outerModel, const ModelStack&) -> VisitModelResponse + { + // Validate the memory address of the model matches the outer model found searching the visited model "child models" + if (const sdf::Model* searchModel = outerModel.ModelByName(model.Name()); searchModel != &model) + { + resultModel = &outerModel; + return VisitModelResponse::Stop; + } + + return VisitModelResponse::VisitNestedAndSiblings; + }; + VisitModels(root, IsModelInModel); + + return resultModel; + } + AZ::IO::Path ResolveAmentPrefixPath( AZ::IO::Path unresolvedPath, AZStd::string_view amentPrefixPath, diff --git a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h index a3ca63a9d..30f05f49b 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h +++ b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h @@ -144,7 +144,7 @@ namespace ROS2::Utils const sdf::Model* GetModelContainingLink(const sdf::Root& root, AZStd::string_view linkName); //! @param root reference to SDF Root object representing the root of the parsed SDF xml document - //! @param link SDF link object to lookup in the SDF document + //! @param link SDF link reference to lookup in the SDF document const sdf::Model* GetModelContainingLink(const sdf::Root& root, const sdf::Link& link); //! Returns the SDF model object which contains the specified joint @@ -155,9 +155,15 @@ namespace ROS2::Utils const sdf::Model* GetModelContainingJoint(const sdf::Root& root, AZStd::string_view jointName); //! @param root reference to SDF Root object representing the root of the parsed SDF xml document - //! @param joint SDF joint pointer to lookup in the SDF document + //! @param joint SDF joint reference to lookup in the SDF document const sdf::Model* GetModelContainingJoint(const sdf::Root& root, const sdf::Joint& joint); + //! Returns the SDF model object which contains the specified model + //! @param root reference to SDF Root object representing the root of the parsed SDF xml document + //! @param model SDF model reference to lookup in the SDF document + //! @return pointer to parent model containing this model if the model is nested, otherwise nullptr + const sdf::Model* GetModelContainingModel(const sdf::Root& root, const sdf::Model& model); + //! Callback used to check for file exist of a path referenced within a URDF/SDF file //! @param path Candidate local filesystem path to check for existence //! @return true should be returned if the file exist otherwise false From c7251a452fbd4b496bf706b0bcce581444b65df6 Mon Sep 17 00:00:00 2001 From: Mike Balfour <82224783+mbalfour-amzn@users.noreply.github.com> Date: Wed, 4 Oct 2023 15:18:28 -0500 Subject: [PATCH 5/8] Convert tags in SDF/URDF files (#555) * First version of material conversion. This currently skips scripts and pbr materials. Signed-off-by: Mike Balfour <82224783+mbalfour-amzn@users.noreply.github.com> * Modified comment so it's not a direct TODO. Signed-off-by: Mike Balfour <82224783+mbalfour-amzn@users.noreply.github.com> * PR feedback. Cleaned up VisualMakers construction Added error message if model fails to load. Signed-off-by: Mike Balfour <82224783+mbalfour-amzn@users.noreply.github.com> * Added missing includes and fixed missing asset warning. Signed-off-by: Mike Balfour <82224783+mbalfour-amzn@users.noreply.github.com> * Fixed typo to match fixed typo in development branch. Signed-off-by: Mike Balfour <82224783+mbalfour-amzn@users.noreply.github.com> --------- Signed-off-by: Mike Balfour <82224783+mbalfour-amzn@users.noreply.github.com> --- .../RobotImporter/URDF/URDFPrefabMaker.cpp | 39 +-- .../RobotImporter/URDF/VisualsMaker.cpp | 302 ++++++++++++++++-- .../Source/RobotImporter/URDF/VisualsMaker.h | 9 +- 3 files changed, 274 insertions(+), 76 deletions(-) diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp index 41042891f..bdcebf5db 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp @@ -10,10 +10,12 @@ #include "CollidersMaker.h" #include "PrefabMakerUtils.h" #include +#include #include #include #include #include +#include #include #include #include @@ -39,7 +41,7 @@ namespace ROS2 bool useArticulations, const AZStd::optional spawnPosition) : m_root(root) - , m_visualsMaker{} + , m_visualsMaker(urdfAssetsMapping) , m_collidersMaker(urdfAssetsMapping) , m_prefabPath(std::move(prefabPath)) , m_urdfAssetsMapping(urdfAssetsMapping) @@ -48,41 +50,6 @@ namespace ROS2 { AZ_Assert(!m_prefabPath.empty(), "Prefab path is empty"); AZ_Assert(m_root, "SDF Root is nullptr"); - if (m_root != nullptr) - { - VisualsMaker::MaterialNameMap materialMap; - auto GetVisualsFromModel = [&materialMap](const sdf::Model& model) - { - for (uint64_t linkIndex{}; linkIndex < model.LinkCount(); ++linkIndex) - { - if (const sdf::Link* link = model.LinkByIndex(linkIndex); link != nullptr) - { - for (uint64_t visualIndex{}; visualIndex < link->VisualCount(); ++visualIndex) - { - if (const sdf::Visual* visual = link->VisualByIndex(visualIndex); visual != nullptr) - { - if (const sdf::Material* material = visual->Material(); material != nullptr) - { - const std::string visualName = visual->Name(); - materialMap.emplace(AZStd::string{ visualName.c_str(), visualName.size() }, material); - } - } - } - } - } - }; - - // Iterate over all visuals to get their materials - auto VisitAllModels = [&GetVisualsFromModel](const sdf::Model& model, const Utils::ModelStack&) -> Utils::VisitModelResponse - { - GetVisualsFromModel(model); - // Continue to visit all models within the SDF document and query their tags - return Utils::VisitModelResponse::VisitNestedAndSiblings; - }; - Utils::VisitModels(*m_root, VisitAllModels); - - m_visualsMaker = VisualsMaker(AZStd::move(materialMap), urdfAssetsMapping); - } } URDFPrefabMaker::CreatePrefabTemplateResult URDFPrefabMaker::CreatePrefabTemplateFromUrdfOrSdf() diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp index b84f8e315..6a5ff760f 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp @@ -11,6 +11,7 @@ #include "RobotImporter/Utils/TypeConversions.h" #include +#include #include #include #include @@ -19,12 +20,14 @@ #include #include +#include +#include + namespace ROS2 { VisualsMaker::VisualsMaker() = default; - VisualsMaker::VisualsMaker(MaterialNameMap materials, const AZStd::shared_ptr& urdfAssetsMapping) - : m_materials(AZStd::move(materials)) - , m_urdfAssetsMapping(urdfAssetsMapping) + VisualsMaker::VisualsMaker(const AZStd::shared_ptr& urdfAssetsMapping) + : m_urdfAssetsMapping(urdfAssetsMapping) { } @@ -88,13 +91,16 @@ namespace ROS2 return AZ::EntityId(); } auto visualEntityId = createEntityResult.GetValue(); - AddVisualToEntity(visual, visualEntityId); - AddMaterialForVisual(visual, visualEntityId); + auto visualAssetId = AddVisualToEntity(visual, visualEntityId); + AddMaterialForVisual(visual, visualEntityId, visualAssetId); return visualEntityId; } - void VisualsMaker::AddVisualToEntity(const sdf::Visual* visual, AZ::EntityId entityId) const + AZ::Data::AssetId VisualsMaker::AddVisualToEntity(const sdf::Visual* visual, AZ::EntityId entityId) const { + // Asset ID for the asset added to the visual entity, if any. + AZ::Data::AssetId assetId; + // Apply transform as per origin PrefabMakerUtils::SetEntityTransformLocal(visual->RawPose(), entityId); @@ -109,7 +115,6 @@ namespace ROS2 const AZ::Vector3 sphereDimensions(sphereGeometry->Radius() * 2.0f); // The `_sphere_1x1.fbx.azmodel` is created by Asset Processor based on O3DE `PrimitiveAssets` Gem source. - AZ::Data::AssetId assetId; const char* sphereAssetRelPath = "objects/_primitives/_sphere_1x1.fbx.azmodel"; // relative path to cache folder. AZ::Data::AssetCatalogRequestBus::BroadcastResult( assetId, @@ -131,7 +136,6 @@ namespace ROS2 cylinderGeometry->Radius() * 2.0f, cylinderGeometry->Radius() * 2.0f, cylinderGeometry->Length()); // The `_cylinder_1x1.fbx.azmodel` is created by Asset Processor based on O3DE `PrimitiveAssets` Gem source. - AZ::Data::AssetId assetId; const char* cylinderAssetRelPath = "objects/_primitives/_cylinder_1x1.fbx.azmodel"; // relative path to cache folder. AZ::Data::AssetCatalogRequestBus::BroadcastResult( assetId, @@ -151,7 +155,6 @@ namespace ROS2 const AZ::Vector3 boxDimensions = URDF::TypeConversions::ConvertVector3(boxGeometry->Size()); // The `_box_1x1.fbx.azmodel` is created by Asset Processor based on O3DE `PrimitiveAssets` Gem source. - AZ::Data::AssetId assetId; const char* boxAssetRelPath = "objects/_primitives/_box_1x1.fbx.azmodel"; // relative path to cache folder. AZ::Data::AssetCatalogRequestBus::BroadcastResult( assetId, @@ -171,7 +174,8 @@ namespace ROS2 const AZ::Vector3 scaleVector = URDF::TypeConversions::ConvertVector3(meshGeometry->Scale()); const auto asset = PrefabMakerUtils::GetAssetFromPath(*m_urdfAssetsMapping, AZStd::string(meshGeometry->Uri().c_str())); - AZ::Data::AssetId assetId; + AZ_Warning("AddVisual", asset, "There is no source asset for %s.", meshGeometry->Uri().c_str()); + if (asset) { assetId = Utils::GetModelProductAssetId(asset->m_sourceGuid); @@ -183,8 +187,10 @@ namespace ROS2 break; default: AZ_Warning("AddVisual", false, "Unsupported visual geometry type, %d", (int)geometry->Type()); - return; + break; } + + return assetId; } void VisualsMaker::AddVisualAssetToEntity(AZ::EntityId entityId, const AZ::Data::AssetId& assetId, const AZ::Vector3& scale) const @@ -225,36 +231,264 @@ namespace ROS2 entity->Deactivate(); } - void VisualsMaker::AddMaterialForVisual(const sdf::Visual* visual, AZ::EntityId entityId) const + static void OverrideMaterialPbrSettings(const sdf::Material* material, [[maybe_unused]] AZ::Render::MaterialAssignmentMap& overrides) + { + if (auto pbr = material->PbrMaterial(); pbr) + { + // Start by trying to get the Metal workflow, since this is the workflow that O3DE uses. + auto pbrWorkflow = pbr->Workflow(sdf::PbrWorkflowType::METAL); + + if (!pbrWorkflow) + { + // If the Metal workflow doesn't exist, try to get the Specular workflow. + // Even though O3DE uses a Metal workflow, the vast majority of the Specular workflow data can still be used. + // It's only the specular/glossiness maps that won't be converted. + pbrWorkflow = pbr->Workflow(sdf::PbrWorkflowType::SPECULAR); + if (!pbrWorkflow) + { + AZ_Error("AddMaterial", false, "Material has a PBR definition, but it is neither a Metal nor a Specular workflow. Cannot convert."); + return; + } + } + + for (auto& [id, materialAssignment] : overrides) + { + // To truly add PBR conversion, all of the texture map references need to get detected and handled. + // The code in the following commented-out blocks demonstrate how to detect the texture maps + // and shows the correct property overrides to connect them to. However, the property overrides + // currently require an ImageAsset reference, not a string, so the code won't quite work as-is. + // Either the Material overrides need to be changed to work with strings, or this code needs to be changed + // to provide ImageAsset references, which would mean that the texture references would already need to be resolved + // and processed by the Asset Processor. + /* + if (auto texture = pbrWorkflow->AlbedoMap(); !texture.empty()) + { + materialAssignment.m_propertyOverrides.emplace(AZ::Name("baseColor.textureMap"), AZStd::any(AZStd::string(texture.data(), texture.size()))); + } + + if (auto texture = pbrWorkflow->NormalMap(); !texture.empty()) + { + materialAssignment.m_propertyOverrides.emplace(AZ::Name("normal.textureMap"), AZStd::any(AZStd::string(texture.data(), texture.size()))); + } + + if (auto texture = pbrWorkflow->AmbientOcclusionMap(); !texture.empty()) + { + materialAssignment.m_propertyOverrides.emplace(AZ::Name("occlusion.diffuseTextureMap"), AZStd::any(AZStd::string(texture.data(), texture.size()))); + } + + if (auto texture = pbrWorkflow->EmissiveMap(); !texture.empty()) + { + materialAssignment.m_propertyOverrides.emplace(AZ::Name("emissive.enable"), AZStd::any(true)); + materialAssignment.m_propertyOverrides.emplace(AZ::Name("emissive.textureMap"), AZStd::any(AZStd::string(texture.data(), texture.size()))); + } + */ + + if (pbrWorkflow->Type() == sdf::PbrWorkflowType::METAL) + { + /* + if (auto texture = pbrWorkflow->RoughnessMap(); !texture.empty()) + { + materialAssignment.m_propertyOverrides.emplace(AZ::Name("roughness.textureMap"), AZStd::any(AZStd::string(texture.data(), texture.size()))); + } + + if (auto texture = pbrWorkflow->MetalnessMap(); !texture.empty()) + { + materialAssignment.m_propertyOverrides.emplace(AZ::Name("metallic.textureMap"), AZStd::any(AZStd::string(texture.data(), texture.size()))); + } + */ + + if (pbrWorkflow->Element()->HasElement("roughness")) + { + materialAssignment.m_propertyOverrides.emplace(AZ::Name("roughness.factor"), AZStd::any(static_cast(pbrWorkflow->Roughness()))); + } + + if (pbrWorkflow->Element()->HasElement("metalness")) + { + materialAssignment.m_propertyOverrides.emplace(AZ::Name("metallic.factor"), AZStd::any(static_cast(pbrWorkflow->Metalness()))); + } + } + else + { + AZ_Warning("AddMaterial", pbrWorkflow->GlossinessMap().empty(), + "PBR material has a Glossiness map (%s), which is a Specular PBR workflow, not a Metal PBR workflow. It will not be converted.", pbrWorkflow->GlossinessMap().c_str()); + AZ_Warning("AddMaterial", pbrWorkflow->SpecularMap().empty(), + "PBR material has a Specular map (%s), which is a Specular PBR workflow, not a Metal PBR workflow. It will not be converted.", pbrWorkflow->SpecularMap().c_str()); + } + } + } + + } + + static void OverrideMaterialBaseColor(const sdf::Material* material, AZ::Render::MaterialAssignmentMap& overrides) + { + // Base Color: Try to use the diffuse color if the material has one, or fall back to using the ambient color as a backup option. + if (material->Element()->HasElement("diffuse") || material->Element()->HasElement("ambient")) + { + // Get the material's diffuse color as the preferred option for the PBR material base color. + // If a diffuse color didn't exist but ambient does, try to use that instead as the base color. + // It will likely be too dark, but that's still probably better than not setting the color at all. + // Convert from gamma to linear to try and account for the different color spaces between phong and PBR rendering. + const auto materialColor = material->Element()->HasElement("diffuse") ? material->Diffuse() : material->Ambient(); + const AZ::Color baseColor = URDF::TypeConversions::ConvertColor(materialColor).GammaToLinear(); + + for (auto& [id, materialAssignment] : overrides) + { + materialAssignment.m_propertyOverrides.emplace(AZ::Name("baseColor.color"), AZStd::any(baseColor)); + } + } + } + + static void OverrideMaterialTransparency(const sdf::Visual* visual, AZ::Render::MaterialAssignmentMap& overrides) + { + // Opacity: Use visual->transparency to set the material's opacity. + if (visual->Element()->HasElement("transparency")) + { + const auto transparency = visual->Transparency(); + if (transparency > 0.0f) + { + // Override the material properties for every material used by the model. + for (auto& [id, materialAssignment] : overrides) + { + materialAssignment.m_propertyOverrides.emplace(AZ::Name("opacity.mode"), AZStd::any(AZStd::string("Blended"))); + materialAssignment.m_propertyOverrides.emplace(AZ::Name("opacity.alphaSource"), AZStd::any(AZStd::string("None"))); + materialAssignment.m_propertyOverrides.emplace(AZ::Name("opacity.factor"), AZStd::any(1.0f - transparency)); + } + } + } + } + + static void OverrideMaterialEmissiveSettings(const sdf::Material* material, AZ::Render::MaterialAssignmentMap& overrides) + { + // Emissive: If an emissive color has been specified, enable emissive on the material and set the emissive color to the provided one. + if (material->Element()->HasElement("emissive")) + { + // Get the color and convert from gamma to linear to try and account for the different color spaces between phong and PBR rendering. + const auto materialColor = material->Emissive(); + const AZ::Color emissiveColor = URDF::TypeConversions::ConvertColor(materialColor).GammaToLinear(); + + // It seems to be fairly common to have an emissive entry of black, which isn't emissive at all. + // Only enable the emissive color if it's a non-black value. + if ((emissiveColor.GetR() > 0.0f) || (emissiveColor.GetG() > 0.0f) || (emissiveColor.GetB() > 0.0f)) + { + for (auto& [id, materialAssignment] : overrides) + { + materialAssignment.m_propertyOverrides.emplace(AZ::Name("emissive.enable"), AZStd::any(true)); + materialAssignment.m_propertyOverrides.emplace(AZ::Name("emissive.color"), AZStd::any(emissiveColor)); + + // The URDF/SDF file doesn't specify an emissive intensity, just a color. + // We're arbitrarily using a value slightly higher than the default emissive intensity. + // This value was picked based on observations of emissive color behaviors in Gazebo. + // This intensity mostly preserves the color (though it lightens it a little) and + // potentially adds a little bit of lighting to the scene if Bloom or Diffuse Probe Grid also exist in the world. + materialAssignment.m_propertyOverrides.emplace(AZ::Name("emissive.intensity"), AZStd::any(5.5f)); + } + } + } + } + + static void OverrideMaterialRoughness(const sdf::Material* material, AZ::Render::MaterialAssignmentMap& overrides) + { + // Metallic/Roughness: Try to use the shininess value for roughness if we have one, otherwise fall back to using the specular brightness. + if (material->Element()->HasElement("shininess") || material->Element()->HasElement("specular")) + { + float shininess = 0.0f; + float roughness = 0.0f; + + if (material->Element()->HasElement("shininess")) + { + // If we have a shininess value, we'll use it to set both metallic and roughness. + // The shinier it is, the more metallic and less rough we'll make the result. + shininess = material->Shininess(); + roughness = 1.0f - shininess; + } + else + { + // We don't have shininess, so use the specular color to estimate metallic/roughness values. + + // Convert the specular color into a brightness value. To get the brightness, we'll use the average of the three + // color values. This isn't the most perceptually accurate choice, but specular brightness isn't the same as roughness + // anyways, so it's hard to say what perceptual brightness model would cause any better or worse results here. + // Another possibility to consider would be taking the max of the RGB values. + const auto materialColor = material->Specular(); + const AZ::Color specularColor = URDF::TypeConversions::ConvertColor(materialColor); + const float specularBrightness = (specularColor.GetR() + specularColor.GetG() + specularColor.GetB()) / 3.0f; + roughness = 1.0f - specularBrightness; + + // Since specular color doesn't really speak to shininess, we'll arbitrarily scale down the specular brightness to + // 1/4 of the total brightness to modulate the metallic reflectiveness a little, but not too much. Without this scaling, + // a white specular color would always become fully metallic, perfectly smooth, and therefore fully reflective. + // With the scaling, a white specular color will be perfectly smooth but only 25% metallic, so it will have some reflectivity + // but not a lot. + shininess = specularBrightness * 0.25f; + + } + + for (auto& [id, materialAssignment] : overrides) + { + materialAssignment.m_propertyOverrides.emplace(AZ::Name("metallic.factor"), AZStd::any(shininess)); + materialAssignment.m_propertyOverrides.emplace(AZ::Name("roughness.factor"), AZStd::any(roughness)); + } + } + } + + static void OverrideMaterialDoubleSided(const sdf::Material* material, AZ::Render::MaterialAssignmentMap& overrides) { - // URDF does not include information from tags with specific materials, diffuse, specular and emissive params - if (!visual->Material()) + // DoubleSided: The double_sided element converts directly over to the O3DE doubleSided material attribute. + if (material->Element()->HasElement("double_sided")) { - // Material is optional, and it requires geometry + // The default material property value is one-sided, so only override the value if it should be double-sided. + if (material->DoubleSided()) + { + for (auto& [id, materialAssignment] : overrides) + { + materialAssignment.m_propertyOverrides.emplace(AZ::Name("general.doubleSided"), AZStd::any(true)); + } + } + } + } + + void VisualsMaker::AddMaterialForVisual(const sdf::Visual* visual, AZ::EntityId entityId, const AZ::Data::AssetId& assetId) const + { + auto material = visual->Material(); + + if (!material) + { + // Material is optional, it might not appear on all visuals. return; } - AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId); + AZ_Assert(material->Element(), "Material has data but no Element pointer. Something unexpected has happened with the SDF parsing."); - // As a Material doesn't have a name and there can only be 1 material per tag, - // the Visual Name is used for the material - const std::string materialName{ visual->Name() }; - const AZStd::string azMaterialName{ materialName.c_str(), materialName.size() }; + // Conversions from in the file to O3DE are extremely imprecise because the data is going from a Phong model to PBR, + // and there are no direct translations from one type of lighting model to the other. All of the conversions will create some + // rough approximations of the source lighting data, but should hopefully provide a reasonable starting point for tuning the look. - // If present in map, take map color definition as priority, otherwise apply local node definition - const auto materialColorUrdf = - m_materials.contains(azMaterialName) ? m_materials.at(azMaterialName)->Diffuse() : visual->Material()->Diffuse(); - const AZ::Color materialColor = URDF::TypeConversions::ConvertColor(materialColorUrdf); + // Also, URDF/SDF files don't have a concept of overriding specific materials, so every material override generated + // below will get applied to *all* materials for a mesh file. - entity->CreateComponent(AZ::Render::EditorMaterialComponentTypeId); - AZ_Trace("AddVisual", "Setting color for material %s\n", azMaterialName.c_str()); - entity->Activate(); - AZ::Render::MaterialComponentRequestBus::Event( - entityId, - &AZ::Render::MaterialComponentRequestBus::Events::SetPropertyValue, - AZ::Render::DefaultMaterialAssignmentId, - "settings.color", - AZStd::any(materialColor)); - entity->Deactivate(); + // First, force the model asset to get loaded into memory before adding the material component. + // This is required so that we can get the default material map that will be used to override properties for each material. + auto modelAsset = AZ::Data::AssetManager::Instance().GetAsset(assetId, AZ::Data::AssetLoadBehavior::Default); + modelAsset.BlockUntilLoadComplete(); + + AZ_Error("AddMaterial", modelAsset.IsReady(), "Trying to create materials for a model that couldn't load. The generated material overrides may not work correctly."); + + // Initialize the material component configuration to contain all of the material mappings from the model. + AZ::Render::MaterialComponentConfig config; + config.m_materials = AZ::Render::GetDefaultMaterialMapFromModelAsset(modelAsset); + + // Try to override all of the various material settings based on what's contained in the and elements in the source file. + OverrideMaterialPbrSettings(material, config.m_materials); + OverrideMaterialBaseColor(material, config.m_materials); + OverrideMaterialTransparency(visual, config.m_materials); + OverrideMaterialEmissiveSettings(material, config.m_materials); + OverrideMaterialRoughness(material, config.m_materials); + OverrideMaterialDoubleSided(material, config.m_materials); + + // All the material overrides are in place, so get the entity, add the material component, and set its configuration to use the material overrides. + AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId); + AZ_Assert(entity, "Entity ID for visual %s couldn't be found.", visual->Name().c_str()); + auto component = entity->CreateComponent(AZ::Render::EditorMaterialComponentTypeId); + component->SetConfiguration(config); } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.h index 8d4ea8900..1d619387e 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.h @@ -22,10 +22,8 @@ namespace ROS2 class VisualsMaker { public: - using MaterialNameMap = AZStd::unordered_map; - VisualsMaker(); - VisualsMaker(MaterialNameMap materials, const AZStd::shared_ptr& urdfAssetsMapping); + VisualsMaker(const AZStd::shared_ptr& urdfAssetsMapping); //! Add zero, one or many visual elements to a given entity (depending on link content). //! Note that a sub-entity will be added to hold each visual (since they can have different transforms). @@ -36,11 +34,10 @@ namespace ROS2 private: AZ::EntityId AddVisual(const sdf::Visual* visual, AZ::EntityId entityId, const AZStd::string& generatedName) const; - void AddVisualToEntity(const sdf::Visual* visual, AZ::EntityId entityId) const; + AZ::Data::AssetId AddVisualToEntity(const sdf::Visual* visual, AZ::EntityId entityId) const; void AddVisualAssetToEntity(AZ::EntityId entityId, const AZ::Data::AssetId& assetId, const AZ::Vector3& scale) const; - void AddMaterialForVisual(const sdf::Visual* visual, AZ::EntityId entityId) const; + void AddMaterialForVisual(const sdf::Visual* visual, AZ::EntityId entityId, const AZ::Data::AssetId& assetId) const; - MaterialNameMap m_materials; AZStd::shared_ptr m_urdfAssetsMapping; }; } // namespace ROS2 From 08fe340c5b95b2149b31cc42eab3be056bbf2a74 Mon Sep 17 00:00:00 2001 From: Mike Balfour <82224783+mbalfour-amzn@users.noreply.github.com> Date: Thu, 5 Oct 2023 05:53:48 -0500 Subject: [PATCH 6/8] Fully working SDF material conversions (#566) * Fully working material conversions. * Fixed mipmap bug by using AssetId instead of Asset. * Rename GetMeshesFromModel to GetAssetsFromModel since it gets textures now too. --------- Signed-off-by: Mike Balfour <82224783+mbalfour-amzn@users.noreply.github.com> Co-authored-by: lumberyard-employee-dm <56135373+lumberyard-employee-dm@users.noreply.github.com> --- Gems/ROS2/Assets/Gazebo/Black.material | 20 +++ .../Assets/Gazebo/BlackTransparent.material | 23 +++ Gems/ROS2/Assets/Gazebo/Blue.material | 20 +++ Gems/ROS2/Assets/Gazebo/BlueGlow.material | 28 ++++ Gems/ROS2/Assets/Gazebo/BlueLaser.material | 23 +++ .../Assets/Gazebo/BlueTransparent.material | 23 +++ Gems/ROS2/Assets/Gazebo/Bricks.material | 20 +++ Gems/ROS2/Assets/Gazebo/CeilingTiled.material | 20 +++ Gems/ROS2/Assets/Gazebo/CloudySky.material | 20 +++ Gems/ROS2/Assets/Gazebo/DarkGray.material | 20 +++ Gems/ROS2/Assets/Gazebo/DarkGrey.material | 20 +++ .../Gazebo/DarkMagentaTransparent.material | 23 +++ Gems/ROS2/Assets/Gazebo/DarkYellow.material | 20 +++ Gems/ROS2/Assets/Gazebo/FlatBlack.material | 20 +++ Gems/ROS2/Assets/Gazebo/Footway.material | 20 +++ Gems/ROS2/Assets/Gazebo/Gold.material | 20 +++ Gems/ROS2/Assets/Gazebo/Grass.material | 20 +++ Gems/ROS2/Assets/Gazebo/Gray.material | 20 +++ Gems/ROS2/Assets/Gazebo/Green.material | 20 +++ Gems/ROS2/Assets/Gazebo/GreenGlow.material | 28 ++++ .../Assets/Gazebo/GreenTransparent.material | 23 +++ Gems/ROS2/Assets/Gazebo/Grey.material | 20 +++ .../Assets/Gazebo/GreyGradientSky.material | 20 +++ .../Assets/Gazebo/GreyTransparent.material | 23 +++ Gems/ROS2/Assets/Gazebo/Indigo.material | 20 +++ .../Assets/Gazebo/LightBlueLaser.material | 23 +++ Gems/ROS2/Assets/Gazebo/LightOff.material | 28 ++++ Gems/ROS2/Assets/Gazebo/LightOn.material | 28 ++++ Gems/ROS2/Assets/Gazebo/Orange.material | 20 +++ .../Assets/Gazebo/OrangeTransparent.material | 23 +++ Gems/ROS2/Assets/Gazebo/PaintedWall.material | 20 +++ Gems/ROS2/Assets/Gazebo/Pedestrian.material | 20 +++ Gems/ROS2/Assets/Gazebo/Purple.material | 20 +++ Gems/ROS2/Assets/Gazebo/PurpleGlow.material | 28 ++++ Gems/ROS2/Assets/Gazebo/Red.material | 20 +++ Gems/ROS2/Assets/Gazebo/RedBright.material | 20 +++ Gems/ROS2/Assets/Gazebo/RedGlow.material | 28 ++++ .../Assets/Gazebo/RedTransparent.material | 23 +++ Gems/ROS2/Assets/Gazebo/Residential.material | 20 +++ Gems/ROS2/Assets/Gazebo/Road.material | 20 +++ Gems/ROS2/Assets/Gazebo/SkyBlue.material | 20 +++ Gems/ROS2/Assets/Gazebo/Turquoise.material | 20 +++ .../ROS2/Assets/Gazebo/TurquoiseGlow.material | 28 ++++ Gems/ROS2/Assets/Gazebo/White.material | 20 +++ Gems/ROS2/Assets/Gazebo/WhiteGlow.material | 28 ++++ Gems/ROS2/Assets/Gazebo/Wood.material | 20 +++ Gems/ROS2/Assets/Gazebo/WoodFloor.material | 20 +++ Gems/ROS2/Assets/Gazebo/WoodPallet.material | 20 +++ Gems/ROS2/Assets/Gazebo/Yellow.material | 20 +++ Gems/ROS2/Assets/Gazebo/YellowGlow.material | 28 ++++ .../Assets/Gazebo/YellowTransparent.material | 23 +++ Gems/ROS2/Assets/Gazebo/ZincYellow.material | 20 +++ Gems/ROS2/Assets/Sdf/empty.sdf | 7 + .../RobotImporter/Pages/CheckAssetPage.cpp | 22 +-- ...ROS2RobotImporterEditorSystemComponent.cpp | 6 +- .../RobotImporter/RobotImporterWidget.cpp | 53 +++--- .../RobotImporter/RobotImporterWidget.h | 2 +- .../RobotImporter/URDF/CollidersMaker.cpp | 153 +----------------- .../RobotImporter/URDF/CollidersMaker.h | 4 - .../RobotImporter/URDF/VisualsMaker.cpp | 92 ++++++++--- .../Utils/RobotImporterUtils.cpp | 93 ++++++++--- .../RobotImporter/Utils/RobotImporterUtils.h | 9 +- .../Utils/SourceAssetsStorage.cpp | 136 +++++++++------- .../RobotImporter/Utils/SourceAssetsStorage.h | 63 ++++---- .../SdfAssetBuilder/SdfAssetBuilder.cpp | 4 +- 65 files changed, 1460 insertions(+), 326 deletions(-) create mode 100644 Gems/ROS2/Assets/Gazebo/Black.material create mode 100644 Gems/ROS2/Assets/Gazebo/BlackTransparent.material create mode 100644 Gems/ROS2/Assets/Gazebo/Blue.material create mode 100644 Gems/ROS2/Assets/Gazebo/BlueGlow.material create mode 100644 Gems/ROS2/Assets/Gazebo/BlueLaser.material create mode 100644 Gems/ROS2/Assets/Gazebo/BlueTransparent.material create mode 100644 Gems/ROS2/Assets/Gazebo/Bricks.material create mode 100644 Gems/ROS2/Assets/Gazebo/CeilingTiled.material create mode 100644 Gems/ROS2/Assets/Gazebo/CloudySky.material create mode 100644 Gems/ROS2/Assets/Gazebo/DarkGray.material create mode 100644 Gems/ROS2/Assets/Gazebo/DarkGrey.material create mode 100644 Gems/ROS2/Assets/Gazebo/DarkMagentaTransparent.material create mode 100644 Gems/ROS2/Assets/Gazebo/DarkYellow.material create mode 100644 Gems/ROS2/Assets/Gazebo/FlatBlack.material create mode 100644 Gems/ROS2/Assets/Gazebo/Footway.material create mode 100644 Gems/ROS2/Assets/Gazebo/Gold.material create mode 100644 Gems/ROS2/Assets/Gazebo/Grass.material create mode 100644 Gems/ROS2/Assets/Gazebo/Gray.material create mode 100644 Gems/ROS2/Assets/Gazebo/Green.material create mode 100644 Gems/ROS2/Assets/Gazebo/GreenGlow.material create mode 100644 Gems/ROS2/Assets/Gazebo/GreenTransparent.material create mode 100644 Gems/ROS2/Assets/Gazebo/Grey.material create mode 100644 Gems/ROS2/Assets/Gazebo/GreyGradientSky.material create mode 100644 Gems/ROS2/Assets/Gazebo/GreyTransparent.material create mode 100644 Gems/ROS2/Assets/Gazebo/Indigo.material create mode 100644 Gems/ROS2/Assets/Gazebo/LightBlueLaser.material create mode 100644 Gems/ROS2/Assets/Gazebo/LightOff.material create mode 100644 Gems/ROS2/Assets/Gazebo/LightOn.material create mode 100644 Gems/ROS2/Assets/Gazebo/Orange.material create mode 100644 Gems/ROS2/Assets/Gazebo/OrangeTransparent.material create mode 100644 Gems/ROS2/Assets/Gazebo/PaintedWall.material create mode 100644 Gems/ROS2/Assets/Gazebo/Pedestrian.material create mode 100644 Gems/ROS2/Assets/Gazebo/Purple.material create mode 100644 Gems/ROS2/Assets/Gazebo/PurpleGlow.material create mode 100644 Gems/ROS2/Assets/Gazebo/Red.material create mode 100644 Gems/ROS2/Assets/Gazebo/RedBright.material create mode 100644 Gems/ROS2/Assets/Gazebo/RedGlow.material create mode 100644 Gems/ROS2/Assets/Gazebo/RedTransparent.material create mode 100644 Gems/ROS2/Assets/Gazebo/Residential.material create mode 100644 Gems/ROS2/Assets/Gazebo/Road.material create mode 100644 Gems/ROS2/Assets/Gazebo/SkyBlue.material create mode 100644 Gems/ROS2/Assets/Gazebo/Turquoise.material create mode 100644 Gems/ROS2/Assets/Gazebo/TurquoiseGlow.material create mode 100644 Gems/ROS2/Assets/Gazebo/White.material create mode 100644 Gems/ROS2/Assets/Gazebo/WhiteGlow.material create mode 100644 Gems/ROS2/Assets/Gazebo/Wood.material create mode 100644 Gems/ROS2/Assets/Gazebo/WoodFloor.material create mode 100644 Gems/ROS2/Assets/Gazebo/WoodPallet.material create mode 100644 Gems/ROS2/Assets/Gazebo/Yellow.material create mode 100644 Gems/ROS2/Assets/Gazebo/YellowGlow.material create mode 100644 Gems/ROS2/Assets/Gazebo/YellowTransparent.material create mode 100644 Gems/ROS2/Assets/Gazebo/ZincYellow.material create mode 100644 Gems/ROS2/Assets/Sdf/empty.sdf diff --git a/Gems/ROS2/Assets/Gazebo/Black.material b/Gems/ROS2/Assets/Gazebo/Black.material new file mode 100644 index 000000000..5b59c0f47 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Black.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/BlackTransparent.material b/Gems/ROS2/Assets/Gazebo/BlackTransparent.material new file mode 100644 index 000000000..1f8e7fd47 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/BlackTransparent.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 0.0, + 0.0, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Blue.material b/Gems/ROS2/Assets/Gazebo/Blue.material new file mode 100644 index 000000000..4e5a9152e --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Blue.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 0.0, + 1.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/BlueGlow.material b/Gems/ROS2/Assets/Gazebo/BlueGlow.material new file mode 100644 index 000000000..fa253ccfc --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/BlueGlow.material @@ -0,0 +1,28 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 0.0, + 1.0, + 1.0 + ], + "emissive.enable": true, + "emissive.color": [ + 0.0, + 0.0, + 1.0, + 1.0 + ], + "emissive.intensity": 5.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/BlueLaser.material b/Gems/ROS2/Assets/Gazebo/BlueLaser.material new file mode 100644 index 000000000..803d0bdbf --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/BlueLaser.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 0.0, + 1.0, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.6 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/BlueTransparent.material b/Gems/ROS2/Assets/Gazebo/BlueTransparent.material new file mode 100644 index 000000000..3d671fb9a --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/BlueTransparent.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 0.0, + 1.0, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Bricks.material b/Gems/ROS2/Assets/Gazebo/Bricks.material new file mode 100644 index 000000000..b7179ebd7 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Bricks.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 1.0, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.6627, + 0.302, + 0.1568, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/CeilingTiled.material b/Gems/ROS2/Assets/Gazebo/CeilingTiled.material new file mode 100644 index 000000000..e9d6ee1d5 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/CeilingTiled.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.921, + 0.921, + 0.921, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/CloudySky.material b/Gems/ROS2/Assets/Gazebo/CloudySky.material new file mode 100644 index 000000000..9810399e8 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/CloudySky.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.1, + "metallic.factor": 0.6, + "baseColor.color": [ + 0.0, + 0.0, + 1.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/DarkGray.material b/Gems/ROS2/Assets/Gazebo/DarkGray.material new file mode 100644 index 000000000..726b549bb --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/DarkGray.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.825, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.175, + 0.175, + 0.175, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/DarkGrey.material b/Gems/ROS2/Assets/Gazebo/DarkGrey.material new file mode 100644 index 000000000..726b549bb --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/DarkGrey.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.825, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.175, + 0.175, + 0.175, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/DarkMagentaTransparent.material b/Gems/ROS2/Assets/Gazebo/DarkMagentaTransparent.material new file mode 100644 index 000000000..ddd33cac7 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/DarkMagentaTransparent.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.6, + 0.0, + 0.6, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/DarkYellow.material b/Gems/ROS2/Assets/Gazebo/DarkYellow.material new file mode 100644 index 000000000..edf802293 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/DarkYellow.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 1.0, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.7, + 0.7, + 0.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/FlatBlack.material b/Gems/ROS2/Assets/Gazebo/FlatBlack.material new file mode 100644 index 000000000..707889a7e --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/FlatBlack.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.99, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.1, + 0.1, + 0.1, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Footway.material b/Gems/ROS2/Assets/Gazebo/Footway.material new file mode 100644 index 000000000..3f75b925e --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Footway.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.95, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.607, + 0.607, + 0.5607, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Gold.material b/Gems/ROS2/Assets/Gazebo/Gold.material new file mode 100644 index 000000000..c04c701ff --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Gold.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.1, + "metallic.factor": 1.0, + "baseColor.color": [ + 0.8, + 0.64869, + 0.120759, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Grass.material b/Gems/ROS2/Assets/Gazebo/Grass.material new file mode 100644 index 000000000..ef191167a --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Grass.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 1.0, + 0.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Gray.material b/Gems/ROS2/Assets/Gazebo/Gray.material new file mode 100644 index 000000000..9fc720ed0 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Gray.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.99, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.7, + 0.7, + 0.7, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Green.material b/Gems/ROS2/Assets/Gazebo/Green.material new file mode 100644 index 000000000..ef191167a --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Green.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 1.0, + 0.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/GreenGlow.material b/Gems/ROS2/Assets/Gazebo/GreenGlow.material new file mode 100644 index 000000000..2b4193cc8 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/GreenGlow.material @@ -0,0 +1,28 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 1.0, + 0.0, + 1.0 + ], + "emissive.enable": true, + "emissive.color": [ + 0.0, + 1.0, + 0.0, + 1.0 + ], + "emissive.intensity": 5.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/GreenTransparent.material b/Gems/ROS2/Assets/Gazebo/GreenTransparent.material new file mode 100644 index 000000000..0e5045879 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/GreenTransparent.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 1.0, + 0.0, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Grey.material b/Gems/ROS2/Assets/Gazebo/Grey.material new file mode 100644 index 000000000..9fc720ed0 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Grey.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.99, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.7, + 0.7, + 0.7, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/GreyGradientSky.material b/Gems/ROS2/Assets/Gazebo/GreyGradientSky.material new file mode 100644 index 000000000..9fc720ed0 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/GreyGradientSky.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.99, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.7, + 0.7, + 0.7, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/GreyTransparent.material b/Gems/ROS2/Assets/Gazebo/GreyTransparent.material new file mode 100644 index 000000000..62014de7f --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/GreyTransparent.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.5, + 0.5, + 0.5, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Indigo.material b/Gems/ROS2/Assets/Gazebo/Indigo.material new file mode 100644 index 000000000..1c8560962 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Indigo.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.33, + 0.0, + 0.5, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/LightBlueLaser.material b/Gems/ROS2/Assets/Gazebo/LightBlueLaser.material new file mode 100644 index 000000000..2100545f2 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/LightBlueLaser.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.5, + 0.5, + 1.0, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.6 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/LightOff.material b/Gems/ROS2/Assets/Gazebo/LightOff.material new file mode 100644 index 000000000..f06493e99 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/LightOff.material @@ -0,0 +1,28 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 0.0, + 0.0, + 1.0 + ], + "emissive.enable": true, + "emissive.color": [ + 1.0, + 0.0, + 0.0, + 1.0 + ], + "emissive.intensity": 5.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/LightOn.material b/Gems/ROS2/Assets/Gazebo/LightOn.material new file mode 100644 index 000000000..2b4193cc8 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/LightOn.material @@ -0,0 +1,28 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 1.0, + 0.0, + 1.0 + ], + "emissive.enable": true, + "emissive.color": [ + 0.0, + 1.0, + 0.0, + 1.0 + ], + "emissive.intensity": 5.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Orange.material b/Gems/ROS2/Assets/Gazebo/Orange.material new file mode 100644 index 000000000..c1f496421 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Orange.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.5, + "metallic.factor": 0.125, + "baseColor.color": [ + 1.0, + 0.5088, + 0.0468, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/OrangeTransparent.material b/Gems/ROS2/Assets/Gazebo/OrangeTransparent.material new file mode 100644 index 000000000..5dc77b913 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/OrangeTransparent.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 0.44, + 0.0, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.6 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/PaintedWall.material b/Gems/ROS2/Assets/Gazebo/PaintedWall.material new file mode 100644 index 000000000..1fc08cdfb --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/PaintedWall.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 1.0, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 1.0, + 0.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Pedestrian.material b/Gems/ROS2/Assets/Gazebo/Pedestrian.material new file mode 100644 index 000000000..3f75b925e --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Pedestrian.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.95, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.607, + 0.607, + 0.5607, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Purple.material b/Gems/ROS2/Assets/Gazebo/Purple.material new file mode 100644 index 000000000..5264559ce --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Purple.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 0.0, + 1.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/PurpleGlow.material b/Gems/ROS2/Assets/Gazebo/PurpleGlow.material new file mode 100644 index 000000000..b21428140 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/PurpleGlow.material @@ -0,0 +1,28 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 0.0, + 1.0, + 1.0 + ], + "emissive.enable": true, + "emissive.color": [ + 1.0, + 0.0, + 1.0, + 1.0 + ], + "emissive.intensity": 5.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Red.material b/Gems/ROS2/Assets/Gazebo/Red.material new file mode 100644 index 000000000..6ea55fb08 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Red.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 0.0, + 0.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/RedBright.material b/Gems/ROS2/Assets/Gazebo/RedBright.material new file mode 100644 index 000000000..4f0f85fa5 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/RedBright.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.6, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.87, + 0.26, + 0.07, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/RedGlow.material b/Gems/ROS2/Assets/Gazebo/RedGlow.material new file mode 100644 index 000000000..f06493e99 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/RedGlow.material @@ -0,0 +1,28 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 0.0, + 0.0, + 1.0 + ], + "emissive.enable": true, + "emissive.color": [ + 1.0, + 0.0, + 0.0, + 1.0 + ], + "emissive.intensity": 5.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/RedTransparent.material b/Gems/ROS2/Assets/Gazebo/RedTransparent.material new file mode 100644 index 000000000..a2e72676d --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/RedTransparent.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 0.0, + 0.0, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Residential.material b/Gems/ROS2/Assets/Gazebo/Residential.material new file mode 100644 index 000000000..da4a3a865 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Residential.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.95, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.35, + 0.35, + 0.35, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Road.material b/Gems/ROS2/Assets/Gazebo/Road.material new file mode 100644 index 000000000..da4a3a865 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Road.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.95, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.35, + 0.35, + 0.35, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/SkyBlue.material b/Gems/ROS2/Assets/Gazebo/SkyBlue.material new file mode 100644 index 000000000..9810399e8 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/SkyBlue.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.1, + "metallic.factor": 0.6, + "baseColor.color": [ + 0.0, + 0.0, + 1.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Turquoise.material b/Gems/ROS2/Assets/Gazebo/Turquoise.material new file mode 100644 index 000000000..119c49828 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Turquoise.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 1.0, + 1.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/TurquoiseGlow.material b/Gems/ROS2/Assets/Gazebo/TurquoiseGlow.material new file mode 100644 index 000000000..2fceed5cd --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/TurquoiseGlow.material @@ -0,0 +1,28 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 1.0, + 1.0, + 1.0 + ], + "emissive.enable": true, + "emissive.color": [ + 0.0, + 1.0, + 1.0, + 1.0 + ], + "emissive.intensity": 5.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/White.material b/Gems/ROS2/Assets/Gazebo/White.material new file mode 100644 index 000000000..4e6330c76 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/White.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 1.0, + 1.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/WhiteGlow.material b/Gems/ROS2/Assets/Gazebo/WhiteGlow.material new file mode 100644 index 000000000..43bb814de --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/WhiteGlow.material @@ -0,0 +1,28 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 1.0, + 1.0, + 1.0 + ], + "emissive.enable": true, + "emissive.color": [ + 1.0, + 1.0, + 1.0, + 1.0 + ], + "emissive.intensity": 8.0 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Wood.material b/Gems/ROS2/Assets/Gazebo/Wood.material new file mode 100644 index 000000000..9784b3d7a --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Wood.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.1, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.698, + 0.4314, + 0.1921, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/WoodFloor.material b/Gems/ROS2/Assets/Gazebo/WoodFloor.material new file mode 100644 index 000000000..56a671b1b --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/WoodFloor.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.1, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.557, + 0.275, + 0.067, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/WoodPallet.material b/Gems/ROS2/Assets/Gazebo/WoodPallet.material new file mode 100644 index 000000000..a8e122d13 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/WoodPallet.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.6627, + 0.5, + 0.2784, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Yellow.material b/Gems/ROS2/Assets/Gazebo/Yellow.material new file mode 100644 index 000000000..1fc08cdfb --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Yellow.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 1.0, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 1.0, + 0.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/YellowGlow.material b/Gems/ROS2/Assets/Gazebo/YellowGlow.material new file mode 100644 index 000000000..0830ed3b7 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/YellowGlow.material @@ -0,0 +1,28 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 1.0, + 0.0, + 1.0 + ], + "emissive.enable": true, + "emissive.color": [ + 1.0, + 1.0, + 0.0, + 1.0 + ], + "emissive.intensity": 5.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/YellowTransparent.material b/Gems/ROS2/Assets/Gazebo/YellowTransparent.material new file mode 100644 index 000000000..24bc9eecf --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/YellowTransparent.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 1.0, + 0.0, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/ZincYellow.material b/Gems/ROS2/Assets/Gazebo/ZincYellow.material new file mode 100644 index 000000000..5a519f77a --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/ZincYellow.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.05, + "metallic.factor": 1.0, + "baseColor.color": [ + 0.9725, + 0.9529, + 0.2078, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Sdf/empty.sdf b/Gems/ROS2/Assets/Sdf/empty.sdf new file mode 100644 index 000000000..b9f8515a8 --- /dev/null +++ b/Gems/ROS2/Assets/Sdf/empty.sdf @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckAssetPage.cpp b/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckAssetPage.cpp index 4754f93af..f655e4c57 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckAssetPage.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckAssetPage.cpp @@ -48,10 +48,10 @@ namespace ROS2 m_table->setSelectionBehavior(QAbstractItemView::SelectRows); m_table->setSelectionMode(QAbstractItemView::SingleSelection); // Set the header items. - QTableWidgetItem* headerItem = new QTableWidgetItem(tr("URDF/SDF mesh path")); + QTableWidgetItem* headerItem = new QTableWidgetItem(tr("URDF/SDF asset path")); headerItem->setTextAlignment(Qt::AlignVCenter | Qt::AlignLeft); m_table->setHorizontalHeaderItem(Columns::SdfMeshPath, headerItem); - headerItem = new QTableWidgetItem(tr("Resolved mesh from URDF/SDF")); + headerItem = new QTableWidgetItem(tr("Resolved asset from URDF/SDF")); headerItem->setTextAlignment(Qt::AlignVCenter | Qt::AlignLeft); m_table->setHorizontalHeaderItem(Columns::ResolvedMeshPath, headerItem); headerItem = new QTableWidgetItem(tr("Type")); @@ -81,11 +81,11 @@ namespace ROS2 { if (m_missingCount == 0) { - setTitle(tr("Resolved meshes")); + setTitle(tr("Resolved assets")); } else { - setTitle(tr("There are ") + QString::number(m_missingCount) + tr(" unresolved meshes")); + setTitle(tr("There are ") + QString::number(m_missingCount) + tr(" unresolved assets")); } } @@ -243,10 +243,12 @@ namespace ROS2 { if (!failed) { - const AZStd::string productRelPathVisual = Utils::GetModelProductAsset(assetUuid); - const AZStd::string productRelPathCollider = Utils::GetPhysXMeshProductAsset(assetUuid); - QString text = QString::fromUtf8(productRelPathVisual.data(), productRelPathVisual.size()) + " " + - QString::fromUtf8(productRelPathCollider.data(), productRelPathCollider.size()); + const AZStd::vector productPaths = Utils::GetProductAssets(assetUuid); + QString text; + for (const auto& productPath : productPaths) + { + text += QString::fromUtf8(productPath.data(), productPath.size()) + " "; + } m_table->setItem(i, Columns::ProductAsset, createCell(true, text)); m_table->item(i, Columns::ProductAsset)->setIcon(m_okIcon); } @@ -265,12 +267,12 @@ namespace ROS2 m_refreshTimer->stop(); if (m_failedCount == 0 && m_missingCount == 0) { - setTitle(tr("All meshes were processed")); + setTitle(tr("All assets were processed")); } else { setTitle( - tr("There are ") + QString::number(m_missingCount) + tr(" unresolved meshes.") + tr("There are ") + + tr("There are ") + QString::number(m_missingCount) + tr(" unresolved assets.") + tr("There are ") + QString::number(m_failedCount) + tr(" failed asset processor jobs.")); } } diff --git a/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp b/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp index c418959ea..730380056 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp @@ -148,14 +148,12 @@ namespace ROS2 // Urdf Root has been parsed successfully retrieve it from the Outcome const sdf::Root& parsedSdfRoot = parsedSdfOutcome.GetRoot(); - auto collidersNames = Utils::GetMeshesFilenames(parsedSdfRoot, false, true); - auto visualNames = Utils::GetMeshesFilenames(parsedSdfRoot, true, false); - auto meshNames = Utils::GetMeshesFilenames(parsedSdfRoot, true, true); + auto assetNames = Utils::GetReferencedAssetFilenames(parsedSdfRoot); AZStd::shared_ptr urdfAssetsMapping = AZStd::make_shared(); if (importAssetWithUrdf) { urdfAssetsMapping = AZStd::make_shared( - Utils::CopyAssetForURDFAndCreateAssetMap(meshNames, filePath, collidersNames, visualNames, sdfBuilderSettings)); + Utils::CopyReferencedAssetsAndCreateAssetMap(assetNames, filePath, sdfBuilderSettings)); } bool allAssetProcessed = false; bool assetProcessorFailed = false; diff --git a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp index 87835a3e3..b1554cb77 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp @@ -217,7 +217,7 @@ namespace ROS2 m_parsedSdf = AZStd::move(parsedSdfOutcome.GetRoot()); m_prefabMaker.reset(); // Report the status of skipping this page - m_meshNames = Utils::GetMeshesFilenames(m_parsedSdf, true, true); + m_assetNames = Utils::GetReferencedAssetFilenames(m_parsedSdf); m_assetPage->ClearAssetsList(); } else @@ -265,9 +265,6 @@ namespace ROS2 { if (m_assetPage->IsEmpty()) { - auto collidersNames = Utils::GetMeshesFilenames(m_parsedSdf, false, true); - auto visualNames = Utils::GetMeshesFilenames(m_parsedSdf, true, false); - AZ::Uuid::FixedString dirSuffix; if (!m_params.empty()) { @@ -286,26 +283,29 @@ namespace ROS2 if (m_importAssetWithUrdf) { - m_urdfAssetsMapping = AZStd::make_shared(Utils::CopyAssetForURDFAndCreateAssetMap( - m_meshNames, m_urdfPath.String(), collidersNames, visualNames, sdfBuilderSettings, dirSuffix)); + m_urdfAssetsMapping = AZStd::make_shared(Utils::CopyReferencedAssetsAndCreateAssetMap( + m_assetNames, m_urdfPath.String(), sdfBuilderSettings, dirSuffix)); } else { m_urdfAssetsMapping = - AZStd::make_shared(Utils::FindAssetsForUrdf(m_meshNames, m_urdfPath.String(), sdfBuilderSettings)); - for (const AZStd::string& meshPath : m_meshNames) + AZStd::make_shared(Utils::FindReferencedAssets(m_assetNames, m_urdfPath.String(), sdfBuilderSettings)); + for (const auto& [assetPath, assetReferenceType] : m_assetNames) { - if (m_urdfAssetsMapping->contains(meshPath)) + if (m_urdfAssetsMapping->contains(assetPath)) { - const auto& asset = m_urdfAssetsMapping->at(meshPath); - bool visual = visualNames.contains(meshPath); - bool collider = collidersNames.contains(meshPath); - Utils::CreateSceneManifest(asset.m_availableAssetInfo.m_sourceAssetGlobalPath, collider, visual); + const auto& asset = m_urdfAssetsMapping->at(assetPath); + bool visual = (assetReferenceType & Utils::ReferencedAssetType::VisualMesh) == Utils::ReferencedAssetType::VisualMesh; + bool collider = (assetReferenceType & Utils::ReferencedAssetType::ColliderMesh) == Utils::ReferencedAssetType::ColliderMesh; + if (visual || collider) + { + Utils::CreateSceneManifest(asset.m_availableAssetInfo.m_sourceAssetGlobalPath, collider, visual); + } } } }; - for (const AZStd::string& meshPath : m_meshNames) + for (const auto& [assetPath, assetReferenceType] : m_assetNames) { AZ::Uuid sourceAssetUuid = AZ::Uuid::CreateNull(); QString type = tr("Unknown"); @@ -313,30 +313,35 @@ namespace ROS2 AZStd::optional sourcePath; AZStd::optional resolvedPath; - if (m_urdfAssetsMapping->contains(meshPath)) + if (m_urdfAssetsMapping->contains(assetPath)) { - bool visual = visualNames.contains(meshPath); - bool collider = collidersNames.contains(meshPath); + bool visual = (assetReferenceType & Utils::ReferencedAssetType::VisualMesh) == Utils::ReferencedAssetType::VisualMesh; + bool collider = (assetReferenceType & Utils::ReferencedAssetType::ColliderMesh) == Utils::ReferencedAssetType::ColliderMesh; + bool texture = (assetReferenceType & Utils::ReferencedAssetType::Texture) == Utils::ReferencedAssetType::Texture; if (visual && collider) { - type = tr("Visual and Collider"); + type = tr("Visual and Collider Mesh"); } else if (visual) { - type = tr("Visual"); + type = tr("Visual Mesh"); } else if (collider) { - type = tr("Collider"); + type = tr("Collider Mesh"); + } + else if (texture) + { + type = tr("Texture"); } - const auto& asset = m_urdfAssetsMapping->at(meshPath); + const auto& asset = m_urdfAssetsMapping->at(assetPath); sourceAssetUuid = asset.m_availableAssetInfo.m_sourceGuid; sourcePath = asset.m_availableAssetInfo.m_sourceAssetRelativePath.String(); resolvedPath = asset.m_resolvedUrdfPath.String(); crc = asset.m_urdfFileCRC; } - m_assetPage->ReportAsset(sourceAssetUuid, meshPath, type, sourcePath, crc, resolvedPath); + m_assetPage->ReportAsset(sourceAssetUuid, assetPath, type, sourcePath, crc, resolvedPath); } m_assetPage->StartWatchAsset(); } @@ -411,9 +416,9 @@ namespace ROS2 } if (m_checkUrdfPage->isComplete()) { - if (m_meshNames.size() == 0) + if (m_assetNames.empty()) { - // skip two pages when urdf/sdf is parsed without problems, and it has no meshes + // skip two pages when urdf/sdf is parsed without problems, and it has no assets return m_assetPage->nextId(); } else diff --git a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.h b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.h index 57f9c49e8..1d6cfc5b8 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.h +++ b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.h @@ -78,7 +78,7 @@ namespace ROS2 /// mapping from urdf path to asset source AZStd::shared_ptr m_urdfAssetsMapping; AZStd::unique_ptr m_prefabMaker; - AZStd::unordered_set m_meshNames; + Utils::AssetFilenameReferences m_assetNames; /// Xacro params Utils::xacro::Params m_params; diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.cpp index 0a1a46ef9..0172c44f0 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.cpp @@ -64,147 +64,6 @@ namespace ROS2 } } - void CollidersMaker::BuildColliders(const sdf::Link* link) - { - if (!link) - { - return; - } - - for (uint64_t index = 0; index < link->CollisionCount(); index++) - { - BuildCollider(link->CollisionByIndex(index)); - } - } - - void CollidersMaker::BuildCollider(const sdf::Collision* collision) - { - if (!collision) - { // it is ok not to have collision in a link - return; - } - - auto geometry = collision->Geom(); - bool isPrimitiveShape = geometry->Type() != sdf::GeometryType::MESH; - if (!isPrimitiveShape) - { - auto meshGeometry = geometry->MeshShape(); - if (!meshGeometry) - { - return; - } - const auto asset = PrefabMakerUtils::GetAssetFromPath(*m_urdfAssetsMapping, meshGeometry->Uri()); - if (!asset) - { - return; - } - const auto& azMeshPath = asset->m_sourceAssetGlobalPath; - - AZStd::shared_ptr scene; - AZ::SceneAPI::Events::SceneSerializationBus::BroadcastResult( - scene, &AZ::SceneAPI::Events::SceneSerialization::LoadScene, azMeshPath.c_str(), AZ::Uuid::CreateNull(), ""); - if (!scene) - { - AZ_Error( - Internal::CollidersMakerLoggingTag, - false, - "Error loading collider. Invalid scene: %s, URDF/SDF path: %s", - azMeshPath.c_str(), - meshGeometry->Uri().c_str()); - return; - } - - AZ::SceneAPI::Containers::SceneManifest& manifest = scene->GetManifest(); - auto valueStorage = manifest.GetValueStorage(); - if (valueStorage.empty()) - { - AZ_Error( - Internal::CollidersMakerLoggingTag, false, "Error loading collider. Invalid value storage: %s", azMeshPath.c_str()); - return; - } - - auto view = AZ::SceneAPI::Containers::MakeDerivedFilterView(valueStorage); - if (view.empty()) - { - AZ_Error(Internal::CollidersMakerLoggingTag, false, "Error loading collider. Invalid node views: %s", azMeshPath.c_str()); - return; - } - - // Select all nodes for both visual and collision nodes - for (AZ::SceneAPI::DataTypes::ISceneNodeGroup& mg : view) - { - AZ::SceneAPI::Utilities::SceneGraphSelector::SelectAll(scene->GetGraph(), mg.GetSceneNodeSelectionList()); - } - - // Update scene with all nodes selected - AZ::SceneAPI::Events::ProcessingResultCombiner result; - AZ::SceneAPI::Events::AssetImportRequestBus::BroadcastResult( - result, - &AZ::SceneAPI::Events::AssetImportRequest::UpdateManifest, - *scene, - AZ::SceneAPI::Events::AssetImportRequest::ManifestAction::Update, - AZ::SceneAPI::Events::AssetImportRequest::RequestingApplication::Editor); - - if (result.GetResult() != AZ::SceneAPI::Events::ProcessingResult::Success) - { - AZ_Trace(Internal::CollidersMakerLoggingTag, "Scene updated\n"); - return; - } - - auto assetInfoFilePath = AZ::IO::Path{ azMeshPath }; - assetInfoFilePath.Native() += ".assetinfo"; - AZ_Printf(Internal::CollidersMakerLoggingTag, "Saving collider manifest to %s\n", assetInfoFilePath.c_str()); - scene->GetManifest().SaveToFile(assetInfoFilePath.c_str()); - - // Set export method to convex mesh - auto readOutcome = AZ::JsonSerializationUtils::ReadJsonFile(assetInfoFilePath.c_str()); - if (!readOutcome.IsSuccess()) - { - AZ_Error( - Internal::CollidersMakerLoggingTag, - false, - "Could not read %s with %s", - assetInfoFilePath.c_str(), - readOutcome.GetError().c_str()); - return; - } - rapidjson::Document assetInfoJson = readOutcome.TakeValue(); - auto manifestObject = assetInfoJson.GetObject(); - auto valuesIterator = manifestObject.FindMember("values"); - if (valuesIterator == manifestObject.MemberEnd()) - { - AZ_Error( - Internal::CollidersMakerLoggingTag, false, "Invalid json file: %s (Missing 'values' node)", assetInfoFilePath.c_str()); - return; - } - - constexpr AZStd::string_view physXMeshGroupType = "{5B03C8E6-8CEE-4DA0-A7FA-CD88689DD45B} MeshGroup"; - auto valuesArray = valuesIterator->value.GetArray(); - for (auto& value : valuesArray) - { - auto object = value.GetObject(); - - auto physXMeshGroupIterator = object.FindMember("$type"); - if (AZ::StringFunc::Equal(physXMeshGroupIterator->value.GetString(), physXMeshGroupType)) - { - value.AddMember(rapidjson::StringRef("export method"), rapidjson::StringRef("1"), assetInfoJson.GetAllocator()); - } - } - - auto saveOutcome = AZ::JsonSerializationUtils::WriteJsonFile(assetInfoJson, assetInfoFilePath.c_str()); - if (!saveOutcome.IsSuccess()) - { - AZ_Error( - Internal::CollidersMakerLoggingTag, - false, - "Could not save %s with %s", - assetInfoFilePath.c_str(), - saveOutcome.GetError().c_str()); - return; - } - } - } - void CollidersMaker::AddColliders(const sdf::Model& model, const sdf::Link* link, AZ::EntityId entityId) { AZStd::string typeString = "collider"; @@ -275,19 +134,13 @@ namespace ROS2 return; } - AZStd::string pxmodelPath = Utils::GetPhysXMeshProductAsset(asset->m_sourceGuid); - if (pxmodelPath.empty()) + AZ::Data::AssetId assetId = Utils::GetPhysXMeshProductAssetId(asset->m_sourceGuid); + if (!assetId.IsValid()) { AZ_Error( Internal::CollidersMakerLoggingTag, false, "Could not find pxmodel for %s", asset->m_sourceAssetGlobalPath.c_str()); return; } - AZ_Printf(Internal::CollidersMakerLoggingTag, "pxmodelPath %s\n", pxmodelPath.c_str()); - // Get asset product id (pxmesh) - AZ::Data::AssetId assetId; - const AZ::Data::AssetType PhysxMeshAssetType = azrtti_typeid(); - AZ::Data::AssetCatalogRequestBus::BroadcastResult( - assetId, &AZ::Data::AssetCatalogRequests::GetAssetIdByPath, pxmodelPath.c_str(), PhysxMeshAssetType, true); AZ_Printf( Internal::CollidersMakerLoggingTag, "Collider %s has assetId %s\n", @@ -300,7 +153,7 @@ namespace ROS2 shapeConfiguration.m_useMaterialsFromAsset = false; if (assetId.IsValid()) { - auto mesh = AZ::Data::Asset(assetId, PhysxMeshAssetType); + auto mesh = AZ::Data::Asset(assetId, azrtti_typeid()); shapeConfiguration.m_asset = mesh; entity->CreateComponent(colliderConfig, shapeConfiguration); } diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.h index 2535d347f..5ea9284a1 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.h @@ -37,9 +37,6 @@ namespace ROS2 //! Prevent copying of existing CollidersMaker CollidersMaker(const CollidersMaker& other) = delete; - //! Builds .pxmeshes for every collider in link collider mesh. - //! @param link A parsed SDF tree link node which could hold information about colliders. - void BuildColliders(const sdf::Link* link); //! Add zero, one or many collider elements (depending on link content). //! @param model An SDF model object provided by libsdformat from a parsed URDF/SDF //! @param link A parsed SDF tree link node which could hold information about colliders. @@ -51,7 +48,6 @@ namespace ROS2 private: void FindWheelMaterial(); - void BuildCollider(const sdf::Collision* collision); void AddCollider( const sdf::Collision* collision, AZ::EntityId entityId, diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp index 6a5ff760f..79a47b0b5 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp @@ -10,6 +10,7 @@ #include "RobotImporter/URDF/PrefabMakerUtils.h" #include "RobotImporter/Utils/TypeConversions.h" +#include #include #include #include @@ -231,7 +232,56 @@ namespace ROS2 entity->Deactivate(); } - static void OverrideMaterialPbrSettings(const sdf::Material* material, [[maybe_unused]] AZ::Render::MaterialAssignmentMap& overrides) + static void OverrideScriptMaterial(const sdf::Material* material, AZ::Render::MaterialAssignmentMap& overrides) + { + AZStd::string materialName(material->ScriptName().c_str(), material->ScriptName().size()); + if (materialName.empty()) + { + return; + } + + // Make sure the material name is lowercased before checking the path in the Asset Cache + AZStd::to_lower(materialName); + // If a material has a