Added option to spawn entities with WGS84 coords using existing ros service (#704)

* Added option to spawn entities with WGS84 coords using existing ros service
* Added field to enable/disable support for spawning entities using WGS84

---------

Signed-off-by: Patryk Antosz <[email protected]>

Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.cpp

@@ -14,6 +14,7 @@
 #include <AzCore/std/string/string.h>
 #include <AzFramework/Spawnable/Spawnable.h>
 #include <ROS2/Frame/ROS2FrameComponent.h>
+#include <ROS2/Georeference/GeoreferenceBus.h>
 #include <ROS2/ROS2Bus.h>
 #include <ROS2/ROS2GemUtilities.h>
 #include <ROS2/Utilities/ROS2Conversions.h>
@@ -110,12 +111,23 @@ namespace ROS2
     void ROS2SpawnerComponent::SpawnEntity(
         const SpawnEntityServiceHandle service_handle, const std::shared_ptr<rmw_request_id_t> header, const SpawnEntityRequest request)
     {
+        AZStd::string referenceFrame(request->reference_frame.c_str());
+        const bool isWGS{ referenceFrame == "wgs84" && m_controller.GetSupportWGS() };
+
+        SpawnEntityResponse response;
+
+        if (isWGS && !GeoreferenceRequestsBus::HasHandlers())
+        {
+            response.success = false;
+            response.status_message = "Level is not geographically positioned. Action aborted.";
+            service_handle->send_response(*header, response);
+            return;
+        }
+
         AZStd::string spawnableName(request->name.c_str());
         AZStd::string spawnableNamespace(request->robot_namespace.c_str());
         AZStd::string spawnPointName(request->xml.c_str(), request->xml.size());
 
-        SpawnEntityResponse response;
-
         if (auto namespaceValidation = ROS2Names::ValidateNamespace(spawnableNamespace); !namespaceValidation.IsSuccess())
         {
             response.success = false;
@@ -169,19 +181,45 @@ namespace ROS2
 
         AZ::Transform transform;
 
-        if (auto spawnPoints = GetSpawnPoints(); spawnPoints.contains(spawnPointName))
+        if (isWGS)
         {
-            transform = spawnPoints.at(spawnPointName).pose;
+            ROS2::WGS::WGS84Coordinate coordinate;
+            AZ::Vector3 coordinateInLevel = AZ::Vector3(-1);
+            AZ::Quaternion rotationInENU = AZ::Quaternion::CreateIdentity();
+            coordinate.m_latitude = request->initial_pose.position.x;
+            coordinate.m_longitude = request->initial_pose.position.y;
+            coordinate.m_altitude = request->initial_pose.position.z;
+            ROS2::GeoreferenceRequestsBus::BroadcastResult(rotationInENU, &ROS2::GeoreferenceRequests::GetRotationFromLevelToENU);
+            ROS2::GeoreferenceRequestsBus::BroadcastResult(
+                coordinateInLevel, &ROS2::GeoreferenceRequests::ConvertFromWSG84ToLevel, coordinate);
+
+            rotationInENU = (rotationInENU.GetInverseFast() *
+                             AZ::Quaternion(
+                                 request->initial_pose.orientation.x,
+                                 request->initial_pose.orientation.y,
+                                 request->initial_pose.orientation.z,
+                                 request->initial_pose.orientation.w))
+                                .GetNormalized();
+
+            transform = { coordinateInLevel, rotationInENU, 1.0f };
         }
         else
         {
-            transform = { AZ::Vector3(request->initial_pose.position.x, request->initial_pose.position.y, request->initial_pose.position.z),
-                          AZ::Quaternion(
-                              request->initial_pose.orientation.x,
-                              request->initial_pose.orientation.y,
-                              request->initial_pose.orientation.z,
-                              request->initial_pose.orientation.w),
-                          1.0f };
+            if (auto spawnPoints = GetSpawnPoints(); spawnPoints.contains(spawnPointName))
+            {
+                transform = spawnPoints.at(spawnPointName).pose;
+            }
+            else
+            {
+                transform = { AZ::Vector3(
+                                  request->initial_pose.position.x, request->initial_pose.position.y, request->initial_pose.position.z),
+                              AZ::Quaternion(
+                                  request->initial_pose.orientation.x,
+                                  request->initial_pose.orientation.y,
+                                  request->initial_pose.orientation.z,
+                                  request->initial_pose.orientation.w),
+                              1.0f };
+            }
         }
 
         optionalArgs.m_preInsertionCallback = [this, transform, spawnableName, spawnableNamespace](auto id, auto view)

Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.cpp

@@ -74,6 +74,7 @@ namespace ROS2
             serializeContext->Class<ROS2SpawnerComponentConfig, AZ::ComponentConfig>()
                 ->Version(1)
                 ->Field("Editor entity id", &ROS2SpawnerComponentConfig::m_editorEntityId)
+                ->Field("Support WGS", &ROS2SpawnerComponentConfig::m_supportWGS)
                 ->Field("Spawnables", &ROS2SpawnerComponentConfig::m_spawnables)
                 ->Field("Default spawn pose", &ROS2SpawnerComponentConfig::m_defaultSpawnPose)
                 ->Field("Service names", &ROS2SpawnerComponentConfig::m_serviceNames)
@@ -84,6 +85,7 @@ namespace ROS2
             {
                 editContext->Class<ROS2SpawnerComponentConfig>("ROS2SpawnerComponentConfig", "Config for ROS2SpawnerComponent")
                     ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->DataElement(AZ::Edit::UIHandlers::Default, &ROS2SpawnerComponentConfig::m_supportWGS, "Support WGS", "Support for spawning entities using WGS84 coordinate system")
                     ->DataElement(AZ::Edit::UIHandlers::Default, &ROS2SpawnerComponentConfig::m_spawnables, "Spawnables", "Spawnables")
                     ->DataElement(
                         AZ::Edit::UIHandlers::Default,
@@ -116,6 +118,11 @@ namespace ROS2
         return GetSpawnPoints();
     }
 
+    bool ROS2SpawnerComponentController::GetSupportWGS() const
+    {
+        return m_config.m_supportWGS;
+    }
+
     void ROS2SpawnerComponentController::Reflect(AZ::ReflectContext* context)
     {
         ROS2SpawnerComponentConfig::Reflect(context);

Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.h

@@ -47,6 +47,7 @@ namespace ROS2
 
         ROS2SpawnerServiceNames m_serviceNames;
         AZStd::unordered_map<AZStd::string, AZ::Data::Asset<AzFramework::Spawnable>> m_spawnables;
+        bool m_supportWGS{ true };
     };
 
     class ROS2SpawnerComponentController : public SpawnerRequestsBus::Handler
@@ -77,6 +78,7 @@ namespace ROS2
         SpawnPointInfoMap GetSpawnPoints() const;
         AZ::EntityId GetEditorEntityId() const;
         AZStd::unordered_map<AZStd::string, AZ::Data::Asset<AzFramework::Spawnable>> GetSpawnables() const;
+        bool GetSupportWGS() const;
 
     private:
         ROS2SpawnerComponentConfig m_config;