add SensorsMaker for SDFormat data

Signed-off-by: Jan Hanca <[email protected]>

Gems/ROS2/Code/Source/RobotImporter/URDF/SensorsMaker.cpp

@@ -0,0 +1,60 @@
+/*
+ * 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.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#include "SensorsMaker.h"
+
+#include <AzCore/Component/EntityId.h>
+#include <AzToolsFramework/Entity/EditorEntityHelpers.h>
+
+#include <ROS2/RobotImporter/SDFormatSensorImporterHook.h>
+
+#include <sdf/Link.hh>
+#include <sdf/Sensor.hh>
+
+namespace ROS2
+{
+    void AddSensor(AZ::EntityId entityId, const sdf::Sensor* sensor)
+    {
+        AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId);
+        auto serializeContext = AZ::Interface<AZ::ComponentApplicationRequests>::Get()->GetSerializeContext();
+        serializeContext->EnumerateAll(
+            [&](const AZ::SerializeContext::ClassData* classData, const AZ::Uuid& typeId) -> bool
+            {
+                auto* attribute = AZ::FindAttribute(AZ::Crc32("SDFormatSensorImporter"), classData->m_attributes);
+                if (attribute == nullptr)
+                {
+                    return true;
+                }
+
+                AZ::AttributeReader reader(nullptr, attribute);
+                SDFormat::SensorImporterHook sensorHook;
+                if (reader.Read<SDFormat::SensorImporterHook>(sensorHook))
+                {
+                    for (const auto& t : sensorHook.m_sensorTypes)
+                    {
+                        if (sensor->Type() == t)
+                        {
+                            sensorHook.m_sdfSensorToComponentCallback(*entity, *sensor);
+                            return false;
+                        }
+                    }
+                }
+
+                return true;
+            });
+    }
+
+    void SensorsMaker::AddSensors(const sdf::Model& model, const sdf::Link* link, AZ::EntityId entityId) const
+    {
+        for (size_t si = 0; si < link->SensorCount(); ++si)
+        {
+            const auto* sensor = link->SensorByIndex(si);
+            AddSensor(entityId, sensor);
+        }
+    }
+} // namespace ROS2

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

@@ -0,0 +1,32 @@
+/*
+ * 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.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#pragma once
+
+#include "UrdfParser.h"
+#include <AzCore/Component/ComponentBus.h>
+#include <AzCore/Component/EntityId.h>
+#include <AzCore/Outcome/Outcome.h>
+
+#include <sdf/sdf.hh>
+
+namespace ROS2
+{
+    //! Populates a given entity with all the contents of the <sensor> tag in robot description of Gazebo.
+    //! In SDFormat, sensors are specified as children of link of joint.
+    class SensorsMaker
+    {
+    public:
+        //! Adds a Gazebo sensor to an entity and sets it accordingly
+        //! @param model A parsed SDF model which could hold information about sensor to be made.
+        //! @param link A parsed SDF tree link node used to identify link being currently processed.
+        //! @param entityId A non-active entity which will be affected.
+        //! @returns created components Id or string with fail
+        void AddSensors(const sdf::Model& model, const sdf::Link* link, AZ::EntityId entityId) const;
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp

@@ -459,6 +459,8 @@ namespace ROS2
         }
 
         m_collidersMaker.AddColliders(*GetFirstModel(), link, entityId);
+        m_gazeboSensorsMaker.AddSensors(*GetFirstModel(), link, entityId);
+
         return AZ::Success(entityId);
     }
 

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

@@ -12,6 +12,7 @@
 #include "CollidersMaker.h"
 #include "InertialsMaker.h"
 #include "JointsMaker.h"
+#include "SensorsMaker.h"
 #include "UrdfParser.h"
 #include "VisualsMaker.h"
 #include <AzCore/Component/EntityId.h>
@@ -69,7 +70,8 @@ namespace ROS2
         AZStd::string GetStatus();
 
     private:
-        AzToolsFramework::Prefab::PrefabEntityResult AddEntitiesForLink(const sdf::Link* link, AZ::EntityId parentEntityId, AZStd::vector<AZ::EntityId>& createdEntities);
+        AzToolsFramework::Prefab::PrefabEntityResult AddEntitiesForLink(
+            const sdf::Link* link, AZ::EntityId parentEntityId, AZStd::vector<AZ::EntityId>& createdEntities);
         void BuildAssetsForLink(const sdf::Link* link);
         void AddRobotControl(AZ::EntityId rootEntityId);
         static void MoveEntityToDefaultSpawnPoint(const AZ::EntityId& rootEntityId, AZStd::optional<AZ::Transform> spawnPosition);
@@ -81,6 +83,7 @@ namespace ROS2
         AZStd::string m_prefabPath;
         VisualsMaker m_visualsMaker;
         CollidersMaker m_collidersMaker;
+        SensorsMaker m_gazeboSensorsMaker;
         InertialsMaker m_inertialsMaker;
         JointsMaker m_jointsMaker;
         ArticulationsMaker m_articulationsMaker;

Gems/ROS2/Code/ros2_editor_files.cmake

@@ -47,6 +47,8 @@ set(FILES
     Source/RobotImporter/URDF/JointsMaker.h
     Source/RobotImporter/URDF/PrefabMakerUtils.cpp
     Source/RobotImporter/URDF/PrefabMakerUtils.h
+    Source/RobotImporter/URDF/SensorsMaker.cpp
+    Source/RobotImporter/URDF/SensorsMaker.h
     Source/RobotImporter/URDF/UrdfParser.cpp
     Source/RobotImporter/URDF/UrdfParser.h
     Source/RobotImporter/URDF/URDFPrefabMaker.cpp