Urdf creation in passes (#295)

Changed way in which URDF is parsed:
 - entities are created without hierarchy at first
 - entities transformations are set independent of joint
 - Widgets contain log of created entities and joints
 - Fixed crash in ColliderMaker, that was caused by a non-existing mesh
   collider

Co-authored-by: Adam Dabrowski <adam.dabrowski@robotec.ai>
Signed-off-by: Michał Pełka <michalpelka@gmail.com>

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"

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

Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.h

@@ -11,9 +11,11 @@
 #if !defined(Q_MOC_RUN)
 #include "RobotImporter/URDF/RobotImporter.h"
 #include <AzToolsFramework/API/ToolsApplicationAPI.h>
+#include <QCheckBox>
 #include <QFileDialog>
 #include <QLabel>
 #include <QPushButton>
+#include <QTextEdit>
 #include <QTimer>
 #include <QWidget>
 #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;
 

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();
         }
 

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 <AzCore/Asset/AssetManagerBus.h>
 #include <AzCore/Serialization/Json/JsonUtils.h>
 #include <AzCore/StringFunc/StringFunc.h>
@@ -138,7 +138,7 @@ namespace ROS2
         {
             m_wheelMaterial =
                 AZ::Data::Asset<Physics::MaterialAsset>(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<Physics::MaterialAsset> materialAsset =
             isWheelEntity ? m_wheelMaterial : AZ::Data::Asset<Physics::MaterialAsset>();
@@ -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();
             });

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 <AzCore/Component/EntityId.h>
 #include <AzToolsFramework/Entity/EditorEntityHelpers.h>
 #include <Source/EditorRigidBodyComponent.h>
@@ -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;

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 <AzToolsFramework/Entity/EditorEntityHelpers.h>
 #include <Source/EditorColliderComponent.h>
 #include <Source/EditorFixedJointComponent.h>
@@ -17,20 +17,9 @@
 #include <Source/EditorRigidBodyComponent.h>
 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

Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.h

@@ -9,7 +9,9 @@
 #pragma once
 
 #include "UrdfParser.h"
+#include <AzCore/Component/ComponentBus.h>
 #include <AzCore/Component/EntityId.h>
+#include <AzCore/Outcome/Outcome.h>
 
 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<AZ::ComponentId, AZStd::string>;
 
-    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

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 <AzCore/Math/Quaternion.h>
 #include <AzCore/Utils/Utils.h>
 #include <AzCore/std/string/conversions.h>
@@ -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

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);

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<void()> importFinishedCb);
 
+        AZStd::string GetProgress()
+        {
+            if (m_prefabMaker)
+            {
+                return m_prefabMaker->getStatus();
+            }
+            return "";
+        }
+
     private:
         std::atomic_bool m_isProcessingAssets;
         std::atomic_bool m_loadingURDFFailed;