diff --git a/Gems/ROS2/Code/Source/ROS2ModuleInterface.h b/Gems/ROS2/Code/Source/ROS2ModuleInterface.h index 308b0f347..583c2bc91 100644 --- a/Gems/ROS2/Code/Source/ROS2ModuleInterface.h +++ b/Gems/ROS2/Code/Source/ROS2ModuleInterface.h @@ -11,8 +11,8 @@ #include "GNSS/ROS2GNSSSensorComponent.h" #include "Imu/ROS2ImuSensorComponent.h" #include "Lidar/ROS2LidarSensorComponent.h" -#include "ROS2/Frame/ROS2FrameComponent.h" #include "Odometry/ROS2OdometrySensorComponent.h" +#include "ROS2/Frame/ROS2FrameComponent.h" #include "ROS2/Manipulator/MotorizedJoint.h" #include "ROS2SystemComponent.h" #include "RobotControl/Controllers/AckermannController/AckermannControlComponent.h" diff --git a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp index d310d6d07..b13da337b 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp @@ -19,8 +19,9 @@ namespace ROS2 { RobotImporterWidget::RobotImporterWidget(QWidget* parent) : QWidget(parent) - , m_statusLabel("", this) + , m_statusText("", this) , m_selectFileButton(QObject::tr("Load"), this) + , m_statusLabel(QObject::tr("Created Entities:"), this) , m_importerUpdateTimer(this) , m_robotImporter( [this](RobotImporter::LogLevel level, const AZStd::string& message) @@ -38,19 +39,20 @@ namespace ROS2 { setWindowTitle(QObject::tr("Robot definition file importer")); QVBoxLayout* mainLayout = new QVBoxLayout(this); - mainLayout->setSpacing(20); QLabel* captionLabel = new QLabel(QObject::tr("Select a Unified Robot Description Format (URDF) file to import"), this); captionLabel->setWordWrap(true); mainLayout->addWidget(captionLabel); mainLayout->addWidget(&m_selectFileButton); mainLayout->addWidget(&m_statusLabel); - mainLayout->addStretch(); - + mainLayout->addWidget(&m_statusText); + m_statusText.setReadOnly(true); connect( &m_importerUpdateTimer, &QTimer::timeout, [this] { + AZStd::string progress = m_robotImporter.GetProgress(); + m_statusText.setText(progress.c_str()); m_robotImporter.CheckIfAssetsWereLoadedAndCreatePrefab( [this]() { @@ -88,19 +90,22 @@ namespace ROS2 // Check whether import is still in progress every 0.5 seconds m_importerUpdateTimer.start(500); }); + setLayout(mainLayout); } void RobotImporterWidget::ReportError(const AZStd::string& errorMessage) { QMessageBox::critical(this, QObject::tr("Error"), QObject::tr(errorMessage.c_str())); - m_statusLabel.setText(errorMessage.c_str()); + AZStd::string progress = m_robotImporter.GetProgress(); + m_statusText.setText(QObject::tr((progress + errorMessage).c_str())); AZ_Error("RobotImporterWidget", false, errorMessage.c_str()); } void RobotImporterWidget::ReportInfo(const AZStd::string& infoMessage) { - m_statusLabel.setText(QObject::tr(infoMessage.c_str())); + AZStd::string progress = m_robotImporter.GetProgress(); + m_statusText.setText(QObject::tr((progress + infoMessage).c_str())); AZ::Debug::Trace::Instance().Output("RobotImporterWidget", infoMessage.c_str()); } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.h b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.h index d9f52c798..ca7265dc4 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.h +++ b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.h @@ -11,9 +11,11 @@ #if !defined(Q_MOC_RUN) #include "RobotImporter/URDF/RobotImporter.h" #include +#include #include #include #include +#include #include #include #endif @@ -38,8 +40,8 @@ namespace ROS2 //! Populates the log and sets status information in the status label //! @param infoMessage info message to display to the user void ReportInfo(const AZStd::string& infoMessage); - QLabel m_statusLabel; + QTextEdit m_statusText; QPushButton m_selectFileButton; QTimer m_importerUpdateTimer; diff --git a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidgetUtils.cpp b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidgetUtils.cpp index 7852db469..01cc47d90 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidgetUtils.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidgetUtils.cpp @@ -74,7 +74,7 @@ namespace ROS2::RobotImporterWidgetUtils if (!QFile::exists(path.value())) { - QMessageBox::critical(parent, QObject::tr("Does not exist"), QObject::tr("Provided path does not exist. Please try again")); + QMessageBox::critical(parent, QObject::tr("Does not exist"), QObject::tr("File does not exist. Please try again")); return QueryUserForURDFPath(); } diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.cpp index 5e02419a1..223f68ee1 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.cpp @@ -8,8 +8,8 @@ #include "RobotImporter/URDF/CollidersMaker.h" #include "RobotImporter/URDF/PrefabMakerUtils.h" -#include "RobotImporter/URDF/TypeConversions.h" #include "RobotImporter/Utils/RobotImporterUtils.h" +#include "RobotImporter/Utils/TypeConversions.h" #include #include #include @@ -138,7 +138,7 @@ namespace ROS2 { m_wheelMaterial = AZ::Data::Asset(assetId, Physics::MaterialAsset::TYPEINFO_Uuid(), physicsMaterialAssetRelPath); - AZ_TracePrintf(Internal::collidersMakerLoggingTag, "Wait for loading asset\n"); + AZ_TracePrintf(Internal::collidersMakerLoggingTag, "Waiting for asset load\n"); m_wheelMaterial.BlockUntilLoadComplete(); } else @@ -302,7 +302,7 @@ namespace ROS2 const bool isWheelEntity = Utils::IsWheelURDFHeuristics(link); if (isWheelEntity) { - AZ_Printf(Internal::collidersMakerLoggingTag, "%s is wheel", link->name.c_str()); + AZ_Printf(Internal::collidersMakerLoggingTag, "Due to its name, %s is considered a wheel entity", link->name.c_str()); } const AZ::Data::Asset materialAsset = isWheelEntity ? m_wheelMaterial : AZ::Data::Asset(); @@ -335,7 +335,7 @@ namespace ROS2 auto geometry = collision->geometry; if (!geometry) { // non-empty visual should have a geometry - AZ_Warning(Internal::collidersMakerLoggingTag, false, "No Geometry for a collider"); + AZ_Warning(Internal::collidersMakerLoggingTag, false, "No Geometry for a collider of entity %s", entityId.ToString().c_str()); return; } @@ -374,6 +374,7 @@ namespace ROS2 if (!pxmodelPath) { AZ_Error(Internal::collidersMakerLoggingTag, false, "Could not find pxmodel for %s", azMeshPath.c_str()); + entity->Deactivate(); return; } @@ -389,7 +390,7 @@ namespace ROS2 return; } - AZ_Printf(Internal::collidersMakerLoggingTag, "URDF geometry type : %d\n", geometry->type); + AZ_Printf(Internal::collidersMakerLoggingTag, "URDF geometry type: %d\n", geometry->type); switch (geometry->type) { case urdf::Geometry::SPHERE: @@ -436,7 +437,7 @@ namespace ROS2 } break; default: - AZ_Warning(Internal::collidersMakerLoggingTag, false, "Unsupported collider geometry type, %d", geometry->type); + AZ_Warning(Internal::collidersMakerLoggingTag, false, "Unsupported collider geometry type: %d", geometry->type); break; } } @@ -464,7 +465,7 @@ namespace ROS2 } } - AZ_Printf(Internal::collidersMakerLoggingTag, "All URDF assets ready!\n"); + AZ_Printf(Internal::collidersMakerLoggingTag, "All URDF assets are ready!\n"); // Notify the caller that we can continue with constructing the prefab. notifyBuildReadyCb(); }); diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/InertialsMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/InertialsMaker.cpp index c60342196..107b3f6f4 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/InertialsMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/InertialsMaker.cpp @@ -7,7 +7,7 @@ */ #include "RobotImporter/URDF/InertialsMaker.h" -#include "RobotImporter/URDF/TypeConversions.h" +#include "RobotImporter/Utils/TypeConversions.h" #include #include #include @@ -25,7 +25,7 @@ namespace ROS2 { // it is ok not to have inertia in a link return; } - AZ_TracePrintf("AddInertial", "Processing inertial for entity id:%s\n", entityId.ToString().c_str()); + AZ_TracePrintf("AddInertial", "Processing inertial for entity id: %s\n", entityId.ToString().c_str()); AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId); PhysX::EditorRigidBodyConfiguration rigidBodyConfiguration; diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.cpp index d8130d838..c5f47c116 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.cpp @@ -8,7 +8,7 @@ #include "RobotImporter/URDF/JointsMaker.h" #include "RobotImporter/URDF/PrefabMakerUtils.h" -#include "RobotImporter/URDF/TypeConversions.h" +#include "RobotImporter/Utils/TypeConversions.h" #include #include #include @@ -17,20 +17,9 @@ #include namespace ROS2 { - void JointsMaker::AddJoint( - urdf::LinkSharedPtr parentLink, urdf::LinkSharedPtr childLink, AZ::EntityId linkChildId, AZ::EntityId linkParentId) - { // Find if there is a joint between this child and its parent, add / change relevant components - for (auto joint : parentLink->child_joints) - { - if (joint->child_link_name == childLink->name) - { // Found a match! - PrefabMakerUtils::SetEntityTransform(joint->parent_to_joint_origin_transform, linkChildId); - return AddJointComponent(joint, linkChildId, linkParentId); - } - } - } - void JointsMaker::AddJointComponent(urdf::JointSharedPtr joint, AZ::EntityId followColliderEntityId, AZ::EntityId leadColliderEntityId) + JointsMaker::JointsMakerResult JointsMaker::AddJointComponent( + urdf::JointSharedPtr joint, AZ::EntityId followColliderEntityId, AZ::EntityId leadColliderEntityId) { AZ::Entity* followColliderEntity = AzToolsFramework::GetEntityById(followColliderEntityId); PhysX::EditorJointComponent* jointComponent = nullptr; @@ -44,7 +33,9 @@ namespace ROS2 // converting the unit quaternion to Euler vector. const AZ::Vector3 o3de_joint_dir{ 1.0, 0.0, 0.0 }; const AZ::Vector3 joint_axis = URDF::TypeConversions::ConvertVector3(joint->axis); - const auto quaternion = AZ::Quaternion::CreateShortestArc(o3de_joint_dir, joint_axis); + const auto quaternion = + joint_axis.IsZero() ? AZ::Quaternion::CreateIdentity() : AZ::Quaternion::CreateShortestArc(o3de_joint_dir, joint_axis); + AZ_Printf( "JointsMaker", "Quaternion from URDF to o3de %f, %f, %f, %f", @@ -138,12 +129,7 @@ namespace ROS2 break; default: AZ_Warning("AddJointComponent", false, "Unknown or unsupported joint type %d for joint %s", joint->type, joint->name.c_str()); - break; - } - - if (!jointComponent) - { - return; + return AZ::Failure(AZStd::string::format("unsupported joint type : %d", joint->type)); } followColliderEntity->Activate(); @@ -153,5 +139,6 @@ namespace ROS2 PhysX::JointsComponentModeCommon::ParamaterNames::LeadEntity, leadColliderEntityId); followColliderEntity->Deactivate(); + return AZ::Success(jointComponent->GetId()); } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.h index a87ba92b0..e9666dc3f 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.h @@ -9,7 +9,9 @@ #pragma once #include "UrdfParser.h" +#include #include +#include namespace ROS2 { @@ -18,15 +20,14 @@ namespace ROS2 class JointsMaker { public: - //! Add zero or many joint elements to a given entity with a collider (depending on link content). - //! @param parentLink A parent link for the joint - //! @param childLink A child link for the joint. - //! @param childEntityId A non-active entity which will be populated with Joint components. Needs to have a collider. - //! @param parentEntityId An entity higher in hierarchy which is connected through the joint with the child entity. Needs to have a - //! rigid body and a collider. - void AddJoint(urdf::LinkSharedPtr parentLink, urdf::LinkSharedPtr childLink, AZ::EntityId linkChildId, AZ::EntityId linkParentId); + using JointsMakerResult = AZ::Outcome; - private: - void AddJointComponent(urdf::JointSharedPtr joint, AZ::EntityId followColliderEntityId, AZ::EntityId leadColliderEntityId); + //! Adds joint to entity and sets it accordingly to urdf::Joint + //! @param joint Joint data + //! @param followColliderEntityId A non-active entity which will be populated with Joint components. + //! @param leadColliderEntityId An entity higher in hierarchy which is connected through the joint with the child entity. + //! @returns created components Id or string with fail + JointsMakerResult AddJointComponent( + urdf::JointSharedPtr joint, AZ::EntityId followColliderEntityId, AZ::EntityId leadColliderEntityId); }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp index 93da895f7..64cf0b7f9 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp @@ -7,7 +7,7 @@ */ #include "RobotImporter/URDF/PrefabMakerUtils.h" -#include "RobotImporter/URDF/TypeConversions.h" +#include "RobotImporter/Utils/TypeConversions.h" #include #include #include @@ -43,7 +43,7 @@ namespace ROS2::PrefabMakerUtils return assetPath; } - void SetEntityTransform(const urdf::Pose& origin, AZ::EntityId entityId) + void SetEntityTransformLocal(const urdf::Pose& origin, AZ::EntityId entityId) { urdf::Vector3 urdfPosition = origin.position; urdf::Rotation urdfRotation = origin.rotation; @@ -80,7 +80,7 @@ namespace ROS2::PrefabMakerUtils return AZ::Failure(AZStd::string("Invalid id for created entity")); } - AZ_TracePrintf("CreateEntity", "Processing entity id:%s with name:%s\n", entityId.ToString().c_str(), name.c_str()); + AZ_TracePrintf("CreateEntity", "Processing entity id: %s with name: %s\n", entityId.ToString().c_str(), name.c_str()); AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId); entity->SetName(name); entity->Deactivate(); @@ -132,4 +132,5 @@ namespace ROS2::PrefabMakerUtils const AZStd::string suffix = index == 0 ? AZStd::string() : AZStd::string::format("_%zu", index); return AZStd::string::format("%s_%s%s", rootName.c_str(), type.c_str(), suffix.c_str()); } + } // namespace ROS2::PrefabMakerUtils diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h index 676ce93e0..c46a22fa7 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h @@ -17,7 +17,7 @@ namespace ROS2::PrefabMakerUtils { void AddRequiredComponentsToEntity(AZ::EntityId entityId); AZ::IO::Path GetAzModelAssetPathFromModelPath(const AZ::IO::Path& modelPath); - void SetEntityTransform(const urdf::Pose& origin, AZ::EntityId entityId); + void SetEntityTransformLocal(const urdf::Pose& origin, AZ::EntityId entityId); AzToolsFramework::Prefab::PrefabEntityResult CreateEntity(AZ::EntityId parentEntityId, const AZStd::string& name); AzToolsFramework::Prefab::PrefabOperationResult RemoveEntityWithDescendants(AZ::EntityId parentEntityId); AzToolsFramework::EntityIdList GetColliderChildren(AZ::EntityId parentEntityId); diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/RobotImporter.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/RobotImporter.h index 260ee5467..98aad6efb 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/RobotImporter.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/RobotImporter.h @@ -51,6 +51,15 @@ namespace ROS2 //! @param importFinishedCb Function that is called when the import process is finished void CheckIfAssetsWereLoadedAndCreatePrefab(std::function importFinishedCb); + AZStd::string GetProgress() + { + if (m_prefabMaker) + { + return m_prefabMaker->getStatus(); + } + return ""; + } + private: std::atomic_bool m_isProcessingAssets; std::atomic_bool m_loadingURDFFailed; diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp index bc28f3c58..8a23c6b4b 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp @@ -56,22 +56,158 @@ namespace ROS2 } AzToolsFramework::Prefab::CreatePrefabResult URDFPrefabMaker::CreatePrefabFromURDF() - { // TODO - this is PoC code, restructure when developing semantics of URDF->Prefab/Entities/Components mapping + { + { + AZStd::lock_guard lck(m_statusLock); + m_status.clear(); + } + // TODO - this is PoC code, restructure when developing semantics of URDF->Prefab/Entities/Components mapping + AZStd::unordered_map created_links; + auto createEntityRoot = AddEntitiesForLink(m_model->root_link_, AZ::EntityId()); + AZStd::string root_name(m_model->root_link_->name.c_str(), m_model->root_link_->name.size()); + created_links[root_name] = createEntityRoot; + if (!createEntityRoot.IsSuccess()) + { + return AZ::Failure(AZStd::string(createEntityRoot.GetError())); + } - // recursively add all entities - auto createEntityResult = AddEntitiesForLink(m_model->root_link_, AZ::EntityId()); - if (!createEntityResult.IsSuccess()) + auto links = Utils::getAllLinks(m_model->root_link_->child_links); + + // create links + for (const auto& [name, link_ptr] : links) + { + created_links[name] = AddEntitiesForLink(link_ptr, createEntityRoot.GetValue()); + } + + for (const auto& [name, result] : created_links) + { + AZ_TracePrintf( + "CreatePrefabFromURDF", + "Link with name %s was created as: %s", + name.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())); + } + else + { + m_status.emplace(name, AZStd::string::format("failed : %s", result.GetError().c_str())); + } + } + + // set hierarchy + for (const auto& [name, link_ptr] : links) + { + const auto this_entry = created_links.at(name); + if (!this_entry.IsSuccess()) + { + AZ_TracePrintf("CreatePrefabFromURDF", "Link %s creation failed", name.c_str()); + continue; + } + auto parent_ptr = link_ptr->getParent(); + if (!parent_ptr) + { + AZ_TracePrintf("CreatePrefabFromURDF", "Link %s has no parents", name.c_str()); + continue; + } + AZStd::string parent_name(parent_ptr->name.c_str(), parent_ptr->name.size()); + const auto parent_entry = created_links.find(parent_name); + if (parent_entry == created_links.end()) + { + AZ_TracePrintf("CreatePrefabFromURDF", "Link %s has invalid parent name %s", name.c_str(), parent_name.c_str()); + continue; + } + if (!parent_entry->second.IsSuccess()) + { + AZ_TracePrintf( + "CreatePrefabFromURDF", "Link %s has parent %s which has failed to create", name.c_str(), parent_name.c_str()); + continue; + } + AZ::TransformBus::Event(this_entry.GetValue(), &AZ::TransformBus::Events::SetParent, parent_entry->second.GetValue()); + } + + // set transforms of links + for (const auto& [name, link_ptr] : links) + { + const auto this_entry = created_links.at(name); + if (this_entry.IsSuccess()) + { + AZ::Transform tf = Utils::getWorldTransformURDF(link_ptr); + auto* entity = AzToolsFramework::GetEntityById(this_entry.GetValue()); + if (entity) + { + auto* transformInterface = entity->FindComponent(); + if (transformInterface) + { + AZ_TracePrintf( + "CreatePrefabFromURDF", + "Setting transform %s %s to [%f %f %f] [%f %f %f %f]", + name.c_str(), + this_entry.GetValue().ToString().c_str(), + tf.GetTranslation().GetX(), + tf.GetTranslation().GetY(), + tf.GetTranslation().GetZ(), + tf.GetRotation().GetX(), + tf.GetRotation().GetY(), + tf.GetRotation().GetZ(), + tf.GetRotation().GetW()); + transformInterface->SetWorldTM(tf); + } + else + { + AZ_TracePrintf( + "CreatePrefabFromURDF", "Setting transform failed: %s does not have transform interface", name.c_str()); + } + } + } + } + + // create joint + auto joints = Utils::getAllJoints(m_model->root_link_->child_links); + for (const auto& [name, joint_ptr] : joints) { - return AZ::Failure(AZStd::string(createEntityResult.GetError())); + AZ_Assert(joint_ptr, "joint %s is null", name.c_str()); + AZ_TracePrintf( + "CreatePrefabFromURDF", + "Creating joint %s : %s -> %s", + name.c_str(), + joint_ptr->parent_link_name.c_str(), + joint_ptr->child_link_name.c_str()); + + auto lead_entity = created_links.at(joint_ptr->parent_link_name.c_str()); + auto child_entity = created_links.at(joint_ptr->child_link_name.c_str()); + // check if both has RigidBody + if (lead_entity.IsSuccess() && child_entity.IsSuccess()) + { + AZStd::lock_guard lck(m_statusLock); + auto result = m_jointsMaker.AddJointComponent(joint_ptr, child_entity.GetValue(), lead_entity.GetValue()); + if (result.IsSuccess()) + { + m_status.emplace(name, AZStd::string::format("created as %llu", result.GetValue())); + } + else + { + m_status.emplace(name, AZStd::string::format("Failed: %s", result.GetError().c_str())); + } + } + else + { + AZ_Warning("CreatePrefabFromURDF", false, "cannot create joint %s", name.c_str()); + } } - auto contentEntityId = createEntityResult.GetValue(); + // move to spawn point + MoveEntityToDefaultSpawnPoint(createEntityRoot.GetValue()); + + auto contentEntityId = createEntityRoot.GetValue(); AddRobotControl(contentEntityId); // Create prefab, save it to disk immediately // Remove prefab, if it was already created. - AZ::EntityId entityId = createEntityResult.GetValue(); + AZ::EntityId entityId = createEntityRoot.GetValue(); auto prefabSystemComponent = AZ::Interface::Get(); auto prefabLoaderInterface = AZ::Interface::Get(); @@ -84,7 +220,7 @@ namespace ROS2 if (templateId != AzToolsFramework::Prefab::InvalidTemplateId) { - AZ_TracePrintf("CreatePrefabFromURDF", "Prefab was already loaded \n"); + AZ_TracePrintf("CreatePrefabFromURDF", "Prefab was already loaded\n"); prefabSystemComponent->RemoveTemplate(templateId); } @@ -94,7 +230,7 @@ namespace ROS2 AZ::EntityId prefabContainerEntityId = outcome.GetValue(); PrefabMakerUtils::AddRequiredComponentsToEntity(prefabContainerEntityId); } - AZ_TracePrintf("CreatePrefabFromURDF", "Successfully created %s prefab\n", m_prefabPath.c_str()); + AZ_TracePrintf("CreatePrefabFromURDF", "Successfully created prefab %s\n", m_prefabPath.c_str()); return outcome; } @@ -119,28 +255,12 @@ namespace ROS2 if (frameCompontentId) { auto* component = Utils::GetGameOrEditorComponent(entity); - AZ_Assert(component, "Component not exists for %s", entityId.ToString().c_str()); + 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())); } m_visualsMaker.AddVisuals(link, entityId); m_collidersMaker.AddColliders(link, entityId); m_inertialsMaker.AddInertial(link->inertial, entityId); - - for (auto childLink : link->child_links) - { - auto outcome = AddEntitiesForLink(childLink, entityId); // recursive call - if (!outcome.IsSuccess()) - { // TODO - decide on behavior. Still proceed to load other children? - AZ_Warning("AddEntitiesForLink", false, "Unable to add entity due to an error: %s", outcome.GetError().c_str()); - continue; - } - - AZ::EntityId childEntityId = outcome.GetValue(); - m_jointsMaker.AddJoint(link, childLink, childEntityId, entityId); - } - - MoveEntityToDefaultSpawnPoint(entityId); - return AZ::Success(entityId); } @@ -185,4 +305,15 @@ namespace ROS2 transformInterface_->SetWorldTM(pose); } + + AZStd::string URDFPrefabMaker::getStatus() + { + AZStd::string str; + AZStd::lock_guard lck(m_statusLock); + for (const auto& [entry, entry_status] : m_status) + { + str += entry + " " + entry_status + "\n"; + } + return str; + } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h index 39035b9bc..478d4f569 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h @@ -32,6 +32,8 @@ namespace ROS2 AzToolsFramework::Prefab::CreatePrefabResult CreatePrefabFromURDF(); const AZStd::string& GetPrefabPath() const; + AZStd::string getStatus(); + private: AzToolsFramework::Prefab::PrefabEntityResult AddEntitiesForLink(urdf::LinkSharedPtr link, AZ::EntityId parentEntityId); void BuildAssetsForLink(urdf::LinkSharedPtr link); @@ -46,5 +48,7 @@ namespace ROS2 JointsMaker m_jointsMaker; BuildReadyCallback m_notifyBuildReadyCb; + AZStd::mutex m_statusLock; + AZStd::multimap m_status; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp index 21e8b8f43..388f1b91a 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp @@ -22,12 +22,10 @@ namespace ROS2 void checkIfCurrentLocaleHasDotAsADecimalSeparator() { // Due to the fact that URDF parser takes into account the locale information, incompatibility between URDF file locale and - // system locale might lead to incorrect URDF parsing. Mainly it affects floating point numbers, and its decimal separator. When + // system locale might lead to incorrect URDF parsing. Mainly it affects floating point numbers, and the decimal separator. When // locales are set to system with comma as decimal separator and URDF file is created with dot as decimal separator, URDF parser // will trim the floating point number after comma. For example, if parsing 0.1, URDF parser will parse it as 0. - // This might lead to incorrect URDF loading. Most widely used separator is a dot. If in current locale it is not the case a - // warning is presented to the user that his system's locale seem unusual, and he should double-check it - + // This might lead to incorrect URDF loading. If the current locale is not a dot (as per standard ROS locale), we warn the user. std::locale currentLocale(""); if (std::use_facet>(currentLocale).decimal_point() != '.') { diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp index 87db7bd6d..ef6b9db35 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp @@ -8,7 +8,7 @@ #include "RobotImporter/URDF/VisualsMaker.h" #include "RobotImporter/URDF/PrefabMakerUtils.h" -#include "RobotImporter/URDF/TypeConversions.h" +#include "RobotImporter/Utils/TypeConversions.h" #include #include @@ -86,7 +86,7 @@ namespace ROS2 void VisualsMaker::AddVisualToEntity(urdf::VisualSharedPtr visual, AZ::EntityId entityId) { // Apply transform as per origin - PrefabMakerUtils::SetEntityTransform(visual->origin, entityId); + PrefabMakerUtils::SetEntityTransformLocal(visual->origin, entityId); AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId); auto geometry = visual->geometry; diff --git a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp index 33a68fcbf..7f290328a 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp @@ -7,6 +7,7 @@ */ #include "RobotImporterUtils.h" +#include "TypeConversions.h" #include namespace ROS2 { @@ -40,4 +41,49 @@ namespace ROS2 return false; } + AZ::Transform Utils::getWorldTransformURDF(const urdf::LinkSharedPtr& link, AZ::Transform t) + { + if (link->getParent() != nullptr) + { + t = URDF::TypeConversions::ConvertPose(link->parent_joint->parent_to_joint_origin_transform) * t; + return getWorldTransformURDF(link->getParent(), t); + } + return t; + } + + AZStd::unordered_map Utils::getAllLinks(const std::vector& child_links) + { + AZStd::unordered_map pointers; + std::function&)> link_visitor = + [&](const std::vector& child_links) -> void + { + for (auto child_link : child_links) + { + AZStd::string link_name(child_link->name.c_str(), child_link->name.size()); + pointers[link_name] = child_link; + link_visitor(child_link->child_links); + } + }; + link_visitor(child_links); + return pointers; + } + + AZStd::unordered_map Utils::getAllJoints(const std::vector& child_links) + { + AZStd::unordered_map joints; + std::function&)> link_visitor = + [&](const std::vector& child_links) -> void + { + for (auto child_link : child_links) + { + const auto& joint = child_link->parent_joint; + AZStd::string joint_name(joint->name.c_str(), joint->name.size()); + joints[joint_name] = joint; + link_visitor(child_link->child_links); + } + }; + link_visitor(child_links); + return joints; + } + } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h index 33e1a9fb8..db02c65aa 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h +++ b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h @@ -10,11 +10,22 @@ #include "AzCore/Component/ComponentBus.h" #include "AzCore/std/string/string.h" #include "RobotImporter/URDF/UrdfParser.h" +#include +#include namespace ROS2 { namespace Utils { bool IsWheelURDFHeuristics(const urdf::LinkConstSharedPtr& link); + /// Goes through URDF and finds world to entity transformation for us + AZ::Transform getWorldTransformURDF(const urdf::LinkSharedPtr& link, AZ::Transform t = AZ::Transform::Identity()); + + /// Retrieve all links in urdf file + AZStd::unordered_map getAllLinks(const std::vector& links); + + /// Retrieve all links in urdf file + AZStd::unordered_map getAllJoints(const std::vector& links); + } // namespace Utils } // namespace ROS2 \ No newline at end of file diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/TypeConversions.cpp b/Gems/ROS2/Code/Source/RobotImporter/Utils/TypeConversions.cpp similarity index 70% rename from Gems/ROS2/Code/Source/RobotImporter/URDF/TypeConversions.cpp rename to Gems/ROS2/Code/Source/RobotImporter/Utils/TypeConversions.cpp index 716860ea4..e3d4d9b07 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/TypeConversions.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/Utils/TypeConversions.cpp @@ -1,4 +1,4 @@ -/* +/*joint_axis * Copyright (c) Contributors to the Open 3D Engine Project. * For complete copyright and license terms please see the LICENSE at the root of this distribution. * @@ -24,4 +24,12 @@ namespace ROS2::URDF { return AZ::Color(color.r, color.g, color.b, color.a); } + + AZ::Transform TypeConversions::ConvertPose(const urdf::Pose& pose) + { + AZ::Quaternion azRotation = URDF::TypeConversions::ConvertQuaternion(pose.rotation); + AZ::Vector3 azPosition = URDF::TypeConversions::ConvertVector3(pose.position); + return AZ::Transform(azPosition, azRotation, 1.0f); + } + } // namespace ROS2::URDF diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/TypeConversions.h b/Gems/ROS2/Code/Source/RobotImporter/Utils/TypeConversions.h similarity index 83% rename from Gems/ROS2/Code/Source/RobotImporter/URDF/TypeConversions.h rename to Gems/ROS2/Code/Source/RobotImporter/Utils/TypeConversions.h index bfec91dbe..fe9620254 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/TypeConversions.h +++ b/Gems/ROS2/Code/Source/RobotImporter/Utils/TypeConversions.h @@ -8,7 +8,8 @@ #pragma once -#include "UrdfParser.h" +#include "AzCore/Math/Transform.h" +#include "RobotImporter/URDF/UrdfParser.h" #include #include #include @@ -22,5 +23,6 @@ namespace ROS2::URDF static AZ::Vector3 ConvertVector3(const urdf::Vector3& urdfVector); static AZ::Quaternion ConvertQuaternion(const urdf::Rotation& urdfQuaternion); static AZ::Color ConvertColor(const urdf::Color& color); + static AZ::Transform ConvertPose(const urdf::Pose& pose); }; } // namespace ROS2::URDF diff --git a/Gems/ROS2/Code/Tests/UrdfParserTest.cpp b/Gems/ROS2/Code/Tests/UrdfParserTest.cpp index ec2f1fb88..7a8861883 100644 --- a/Gems/ROS2/Code/Tests/UrdfParserTest.cpp +++ b/Gems/ROS2/Code/Tests/UrdfParserTest.cpp @@ -77,6 +77,96 @@ namespace UnitTest " " ""; } + AZStd::string GetURDFWithTranforms() + { + return "\n" + "\n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + ""; + } + AZStd::string GetURDFWithWheel( const AZStd::string& wheel_name, const AZStd::string& wheel_joint_type, @@ -146,7 +236,6 @@ namespace UnitTest EXPECT_EQ(link1->inertial->iyz, 0.0); EXPECT_EQ(link1->inertial->izz, 1.0); - EXPECT_EQ(link1->visual->material->name, "black"); EXPECT_EQ(link1->visual->geometry->type, 1); const auto visualBox = std::dynamic_pointer_cast(link1->visual->geometry); EXPECT_EQ(visualBox->dim.x, 1.0); @@ -259,4 +348,59 @@ namespace UnitTest EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), false); } + TEST_F(UrdfParserTest, TestLinkListing) + { + ROS2::UrdfParser parser; + const auto xmlStr = GetURDFWithTranforms(); + const auto urdf = parser.Parse(xmlStr); + auto links = ROS2::Utils::getAllLinks(urdf->getRoot()->child_links); + EXPECT_EQ(links.size(), 3); + ASSERT_TRUE(links.contains("link1")); + ASSERT_TRUE(links.contains("link2")); + ASSERT_TRUE(links.contains("link3")); + EXPECT_EQ(links.at("link1")->name, "link1"); + EXPECT_EQ(links.at("link2")->name, "link2"); + EXPECT_EQ(links.at("link3")->name, "link3"); + } + + TEST_F(UrdfParserTest, TestJointLink) + { + ROS2::UrdfParser parser; + const auto xmlStr = GetURDFWithTranforms(); + const auto urdf = parser.Parse(xmlStr); + auto joints = ROS2::Utils::getAllJoints(urdf->getRoot()->child_links); + EXPECT_EQ(joints.size(), 3); + } + + TEST_F(UrdfParserTest, TestTransforms) + { + ROS2::UrdfParser parser; + const auto xmlStr = GetURDFWithTranforms(); + const auto urdf = parser.Parse(xmlStr); + const auto links = ROS2::Utils::getAllLinks(urdf->getRoot()->child_links); + const auto link1_ptr = links.at("link1"); + const auto link2_ptr = links.at("link2"); + const auto link3_ptr = links.at("link3"); + + // values exported from Blender + const AZ::Vector3 expected_translation_link1{ 0.0, 0.0, 0.0 }; + const AZ::Vector3 expected_translation_link2{ -1.2000000476837158, 2.0784599781036377, 0.0 }; + const AZ::Vector3 expected_translation_link3{ -2.4000000953674316, 0.0, 0.0 }; + + const AZ::Transform transform_from_urdf_link1 = ROS2::Utils::getWorldTransformURDF(link1_ptr); + EXPECT_NEAR(expected_translation_link1.GetX(), transform_from_urdf_link1.GetTranslation().GetX(), 1e-5); + EXPECT_NEAR(expected_translation_link1.GetY(), transform_from_urdf_link1.GetTranslation().GetY(), 1e-5); + EXPECT_NEAR(expected_translation_link1.GetZ(), transform_from_urdf_link1.GetTranslation().GetZ(), 1e-5); + + const AZ::Transform transform_from_urdf_link2 = ROS2::Utils::getWorldTransformURDF(link2_ptr); + EXPECT_NEAR(expected_translation_link2.GetX(), transform_from_urdf_link2.GetTranslation().GetX(), 1e-5); + EXPECT_NEAR(expected_translation_link2.GetY(), transform_from_urdf_link2.GetTranslation().GetY(), 1e-5); + EXPECT_NEAR(expected_translation_link2.GetZ(), transform_from_urdf_link2.GetTranslation().GetZ(), 1e-5); + + const AZ::Transform transform_from_urdf_link3 = ROS2::Utils::getWorldTransformURDF(link3_ptr); + EXPECT_NEAR(expected_translation_link3.GetX(), transform_from_urdf_link3.GetTranslation().GetX(), 1e-5); + EXPECT_NEAR(expected_translation_link3.GetY(), transform_from_urdf_link3.GetTranslation().GetY(), 1e-5); + EXPECT_NEAR(expected_translation_link3.GetZ(), transform_from_urdf_link3.GetTranslation().GetZ(), 1e-5); + } + } // namespace UnitTest diff --git a/Gems/ROS2/Code/ros2_editor_files.cmake b/Gems/ROS2/Code/ros2_editor_files.cmake index 0e185a2d6..74624f8a5 100644 --- a/Gems/ROS2/Code/ros2_editor_files.cmake +++ b/Gems/ROS2/Code/ros2_editor_files.cmake @@ -14,8 +14,6 @@ set(FILES Source/RobotImporter/RobotImporterWidget.h Source/RobotImporter/RobotImporterWidgetUtils.cpp Source/RobotImporter/RobotImporterWidgetUtils.h - Source/RobotImporter/URDF/TypeConversions.h - Source/RobotImporter/URDF/TypeConversions.cpp Source/RobotImporter/URDF/URDFPrefabMaker.cpp Source/RobotImporter/URDF/URDFPrefabMaker.h Source/RobotImporter/URDF/VisualsMaker.cpp diff --git a/Gems/ROS2/Code/ros2_files.cmake b/Gems/ROS2/Code/ros2_files.cmake index 76d938d9b..5188d6cc6 100644 --- a/Gems/ROS2/Code/ros2_files.cmake +++ b/Gems/ROS2/Code/ros2_files.cmake @@ -62,6 +62,8 @@ set(FILES Source/RobotImporter/ROS2RobotImporterSystemComponent.h Source/RobotImporter/Utils/RobotImporterUtils.h Source/RobotImporter/Utils/RobotImporterUtils.cpp + Source/RobotImporter/Utils/TypeConversions.h + Source/RobotImporter/Utils/TypeConversions.cpp Source/RobotImporter/URDF/UrdfParser.cpp Source/RobotImporter/URDF/UrdfParser.h Source/Converters/URDF/ToFBX/FbxGenerator.cpp