Georeference Component (o3de#604)

Added GeoreferenceBus and GeoreferenceLevelComponent.

Signed-off-by: Michał Pełka <[email protected]>

Author: michalpelka

Gems/ROS2/Code/Include/ROS2/Georeference/GeoreferenceBus.h

@@ -0,0 +1,48 @@
+/*
+ * 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 "GeoreferenceStructures.h"
+#include <AzCore/Component/EntityId.h>
+#include <AzCore/EBus/EBus.h>
+#include <AzCore/Math/Vector3.h>
+#include <AzCore/Math/Quaternion.h>
+
+namespace ROS2
+{
+    //! Interface that allows to convert between level and WSG84 coordinates.
+    class GeoreferenceRequests
+    {
+    public:
+
+        //! Function converts from Level's coordinate system to WSG84.
+        //! @param xyz Vector3 in Level's coordinate system.
+        //! @return Vector3 in WSG84 coordinate system as @class WGS::WGS84Coordinate.
+        virtual WGS::WGS84Coordinate ConvertFromLevelToWSG84(const AZ::Vector3& xyz) = 0;
+
+        //! Function converts from WSG84 coordinate system to Level's.
+        //!  @param latLon Vector3 in WSG84 coordinate system, where x is latitude, y is longitude and z is altitude.
+        //!  @return Vector3 in Level's coordinate system.
+        virtual AZ::Vector3 ConvertFromWSG84ToLevel(const WGS::WGS84Coordinate& latLon) = 0;
+
+        //! Function returns rotation from Level's frame to ENU's (East-North-Up) rotation.
+        //! Function is useful to fin georeference rotation of the level.
+        //! @return Quaternion in ENU coordinate system.
+        virtual AZ::Quaternion GetRotationFromLevelToENU() = 0;
+    };
+
+    class GeoreferenceRequestsTraits : public AZ::EBusTraits
+    {
+    public:
+        // EBusTraits overrides ...
+        static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Single;
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::Single;
+    };
+
+    using GeoreferenceRequestsBus = AZ::EBus<GeoreferenceRequests, GeoreferenceRequestsTraits>;
+} // namespace ROS2

Gems/ROS2/Code/Include/ROS2/Georeference/GeoreferenceStructures.h

@@ -0,0 +1,51 @@
+/*
+* 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 <AzCore/RTTI/RTTI.h>
+#include <AzCore/Math/Vector3.h>
+
+namespace ROS2::WGS
+{
+
+    //! WGS84Coordinate is a 3D vector with double precision.
+    //! It is used to represent coordinates in WSG84 coordinate system.
+    struct WGS84Coordinate
+    {
+        AZ_RTTI(WGS84Coordinate, "{577a5637-b31a-44c5-a33f-50df2922af2a}");
+        static void Reflect(AZ::ReflectContext* context);
+
+        WGS84Coordinate();
+        virtual ~WGS84Coordinate() = default;
+
+        WGS84Coordinate(double latitude, double longitude, double altitude);
+        explicit WGS84Coordinate(const AZ::Vector3& latLonAlt);
+
+        [[nodiscard]] AZ::Vector3 ToVector3f() const;
+
+        double m_latitude = 0.0; //!< Latitude in degrees.
+        double m_longitude = 0.0; //!< Longitude in degrees.
+        double m_altitude = 0.0; //!< Altitude in meters.
+    };
+
+    //! Vector3d is a 3D vector with double precision.
+    //! It is used to represent coordinates in ECEF or ENU coordinate systems.
+    struct Vector3d{
+        Vector3d() = default;
+        Vector3d(double x, double y, double z);
+        explicit Vector3d(const AZ::Vector3& xyz);
+        [[nodiscard]] AZ::Vector3 ToVector3f() const;
+
+        Vector3d operator+(Vector3d const& v) const;
+        Vector3d operator-(Vector3d const& v) const;
+
+        double m_x = 0.0; //!< X coordinate in meters.
+        double m_y = 0.0; //! Y coordinate in meters.
+        double m_z = 0.0; //! Z coordinate in meters.
+    };
+}

Gems/ROS2/Code/Source/GNSS/GNSSFormatConversions.cpp

@@ -6,13 +6,14 @@
  *
  */
 
-#include <AzCore/Math/Matrix4x4.h>
 #include "ROS2GNSSSensorComponent.h"
+#include <AzCore/Math/Matrix4x4.h>
 #include <ROS2/Frame/ROS2FrameComponent.h>
 #include <ROS2/ROS2GemUtilities.h>
 #include <ROS2/Utilities/ROS2Names.h>
 
-#include "GNSSFormatConversions.h"
+#include "Georeference/GNSSFormatConversions.h"
+#include <ROS2/Georeference/GeoreferenceBus.h>
 
 namespace ROS2
 {
@@ -23,12 +24,9 @@ namespace ROS2
 
     void ROS2GNSSSensorComponent::Reflect(AZ::ReflectContext* context)
     {
-        GNSSSensorConfiguration::Reflect(context);
-
         if (auto* serialize = azrtti_cast<AZ::SerializeContext*>(context))
         {
-            serialize->Class<ROS2GNSSSensorComponent, SensorBaseType>()->Version(3)->Field(
-                "gnssSensorConfiguration", &ROS2GNSSSensorComponent::m_gnssConfiguration);
+            serialize->Class<ROS2GNSSSensorComponent, SensorBaseType>()->Version(4);
 
             if (auto* editContext = serialize->GetEditContext())
             {
@@ -38,11 +36,6 @@ namespace ROS2
                     ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"))
                     ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/ROS2GNSSSensor.svg")
                     ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/ROS2GNSSSensor.svg")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &ROS2GNSSSensorComponent::m_gnssConfiguration,
-                        "GNSS sensor configuration",
-                        "GNSS sensor configuration")
                     ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly);
             }
         }
@@ -57,9 +50,7 @@ namespace ROS2
         m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(GNSSMsgType, pc));
     }
 
-    ROS2GNSSSensorComponent::ROS2GNSSSensorComponent(
-        const SensorConfiguration& sensorConfiguration, const GNSSSensorConfiguration& gnssConfiguration)
-        : m_gnssConfiguration(gnssConfiguration)
+    ROS2GNSSSensorComponent::ROS2GNSSSensorComponent(const SensorConfiguration& sensorConfiguration)
     {
         m_sensorConfiguration = sensorConfiguration;
     }
@@ -95,25 +86,23 @@ namespace ROS2
 
     void ROS2GNSSSensorComponent::FrequencyTick()
     {
-        const AZ::Vector3 currentPosition = GetCurrentPose().GetTranslation();
-        const AZ::Vector3 currentPositionECEF = GNSS::ENUToECEF(
-            { m_gnssConfiguration.m_originLatitudeDeg, m_gnssConfiguration.m_originLongitudeDeg, m_gnssConfiguration.m_originAltitude },
-            currentPosition);
-        const AZ::Vector3 currentPositionWGS84 = GNSS::ECEFToWGS84(currentPositionECEF);
 
-        m_gnssMsg.latitude = currentPositionWGS84.GetX();
-        m_gnssMsg.longitude = currentPositionWGS84.GetY();
-        m_gnssMsg.altitude = currentPositionWGS84.GetZ();
+        AZ::Vector3 currentPosition{ 0.0f };
+        AZ::TransformBus::EventResult(currentPosition, GetEntityId(), &AZ::TransformBus::Events::GetWorldTranslation);
+
+        WGS::WGS84Coordinate currentPositionWGS84;
+        ROS2::GeoreferenceRequestsBus::BroadcastResult(
+            currentPositionWGS84, &GeoreferenceRequests::ConvertFromLevelToWSG84, currentPosition);
+
+        m_gnssMsg.latitude = currentPositionWGS84.m_latitude;
+        m_gnssMsg.longitude = currentPositionWGS84.m_longitude;
+        m_gnssMsg.altitude = currentPositionWGS84.m_altitude;
 
         m_gnssMsg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_SBAS_FIX;
-        m_gnssMsg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GALILEO;
+        m_gnssMsg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS;
 
         m_gnssPublisher->publish(m_gnssMsg);
     }
 
-    AZ::Transform ROS2GNSSSensorComponent::GetCurrentPose() const
-    {
-        auto* ros2Frame = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(GetEntity());
-        return ros2Frame->GetFrameTransform();
-    }
+
 } // namespace ROS2