redo AddSensor in RobotImporter

Signed-off-by: Jan Hanca <[email protected]>
o3de · Jan 11, 2024 · 92484b6 · 92484b6
1 parent 3f98b47
commit 92484b6
Show file tree

Hide file tree

Showing 2 changed files with 62 additions and 23 deletions.
diff --git a/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp b/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp
@@ -35,20 +35,7 @@ namespace ROS2
     {
         if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
         {
-            const auto& importerHookCamera = SDFormat::ROS2SensorHooks::ROS2CameraSensor();
-            const auto& importerHookGNSS = SDFormat::ROS2SensorHooks::ROS2GNSSSensor();
-            const auto& importerHookImu = SDFormat::ROS2SensorHooks::ROS2ImuSensor();
-            const auto& importerHookLidar = SDFormat::ROS2SensorHooks::ROS2LidarSensor();
-            const auto& importerHookSkidSteering = SDFormat::ROS2ModelPluginHooks::ROS2SkidSteeringModel();
-            serializeContext->Class<ROS2RobotImporterEditorSystemComponent, ROS2RobotImporterSystemComponent>()
-                ->Version(0)
-                ->Attribute(
-                    "SensorImporterHooks",
-                    SDFormat::SensorImporterHooksStorage{ AZStd::move(importerHookCamera),
-                                                          AZStd::move(importerHookGNSS),
-                                                          AZStd::move(importerHookImu),
-                                                          AZStd::move(importerHookLidar) })
-                ->Attribute("ModelPluginImporterHooks", SDFormat::ModelPluginImporterHooksStorage{ AZStd::move(importerHookSkidSteering) });
+            serializeContext->Class<ROS2RobotImporterEditorSystemComponent, ROS2RobotImporterSystemComponent>()->Version(0);
         }
 
         if (AZ::BehaviorContext* behaviorContext = azrtti_cast<AZ::BehaviorContext*>(context))
@@ -79,6 +66,14 @@ namespace ROS2
         AzToolsFramework::EditorEvents::Bus::Handler::BusConnect();
         RobotImporterRequestBus::Handler::BusConnect();
 
+        // Register default sensor and plugin hooks
+        m_sensorHooks.emplace_back(SDFormat::ROS2SensorHooks::ROS2CameraSensor());
+        m_sensorHooks.emplace_back(SDFormat::ROS2SensorHooks::ROS2GNSSSensor());
+        m_sensorHooks.emplace_back(SDFormat::ROS2SensorHooks::ROS2ImuSensor());
+        m_sensorHooks.emplace_back(SDFormat::ROS2SensorHooks::ROS2LidarSensor());
+        m_modelPluginHooks.emplace_back(SDFormat::ROS2ModelPluginHooks::ROS2SkidSteeringModel());
+
+        // Query user-defined sensor and plugin hooks
         auto serializeContext = AZ::Interface<AZ::ComponentApplicationRequests>::Get()->GetSerializeContext();
         serializeContext->EnumerateAll(
             [&](const AZ::SerializeContext::ClassData* classData, const AZ::Uuid& typeId) -> bool

diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/SensorsMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/SensorsMaker.cpp
@@ -18,30 +18,74 @@
 
 namespace ROS2
 {
-    bool AddSensor(AZ::EntityId entityId, const sdf::Sensor* sensor)
+    void AddSensor(AZ::EntityId entityId, const sdf::Sensor* sensor)
     {
         SDFormat::SensorImporterHooksStorage sensorHooks;
         ROS2::RobotImporterRequestBus::BroadcastResult(sensorHooks, &ROS2::RobotImporterRequest::GetSensorHooks);
-        for (const auto& hook : sensorHooks)
+
+        const auto& sensorPlugins = sensor->Plugins();
+        if (sensorPlugins.empty())
         {
-            if (hook.m_sensorTypes.contains(sensor->Type()))
+            for (auto& hook : sensorHooks)
             {
-                AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId);
-                hook.m_sdfSensorToComponentCallback(*entity, *sensor);
-                return true;
+                if (hook.m_sensorTypes.contains(sensor->Type()))
+                {
+                    AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId);
+                    hook.m_sdfSensorToComponentCallback(*entity, *sensor);
+                    return;
+                }
             }
+            AZ_Warning(
+                "SensorMaker", false, "Cannot find a hook for %s sensor (type %s)", sensor->Name().c_str(), sensor->TypeStr().c_str());
+            return;
         }
 
-        return false;
+        for (const auto& sp : sensorPlugins)
+        {
+            bool currentAdded = false;
+            ROS2::SDFormat::SensorImporterHook* defaultHook = nullptr;
+            for (auto& hook : sensorHooks)
+            {
+                if (hook.m_sensorTypes.contains(sensor->Type()))
+                {
+                    if (hook.m_pluginNames.contains(sp.Filename().c_str()))
+                    {
+                        AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId);
+                        hook.m_sdfSensorToComponentCallback(*entity, *sensor);
+                        currentAdded = true;
+                        break;
+                    }
+                    else if (defaultHook == nullptr)
+                    {
+                        defaultHook = &hook;
+                    }
+                }
+            }
+
+            AZ_Warning(
+                "SensorMaker",
+                currentAdded,
+                "Cannot find a hook for %s sensor (type %s) with plugin %s",
+                sensor->Name().c_str(),
+                sensor->TypeStr().c_str(),
+                sp.Filename().c_str());
+            if (currentAdded == false && defaultHook != nullptr)
+            {
+                AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId);
+                defaultHook->m_sdfSensorToComponentCallback(*entity, *sensor);
+                currentAdded = true;
+                AZ_Warning(
+                    "SensorMaker", false, "Default hook for %s sensor (type %s) used.", sensor->Name().c_str(), sensor->TypeStr().c_str());
+            }
+        }
     }
 
     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);
-            const bool success = AddSensor(entityId, sensor);
-            AZ_Warning("SensorMaker", success, "Cannot find a sensor hook for sensor %d", sensor->Type());
+            AddSensor(entityId, sensor);
         }
     }
 } // namespace ROS2