map component wip

Signed-off-by: Piotr Jaroszek <[email protected]>

Code/Source/GNSS/ROS2GNSSSensorComponent.cpp

@@ -9,15 +9,13 @@
 #include "GNSS/ROS2GNSSSensorComponent.h"
 #include "Frame/ROS2FrameComponent.h"
 #include "ROS2/ROS2Bus.h"
-#include "Utilities/ROS2Conversions.h"
 #include "Utilities/ROS2Names.h"
 #include "AzCore/Math/Matrix4x4.h"
+#include "Map/MapBus.h"
 
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/EditContextConstants.inl>
 
-#include "GNSSFormatConversions.h"
-
 namespace ROS2
 {
     namespace Internal
@@ -31,22 +29,13 @@ namespace ROS2
         {
             serialize->Class<ROS2GNSSSensorComponent, ROS2SensorComponent>()
                     ->Version(1)
-                    ->Field("gnssOriginLatitude", &ROS2GNSSSensorComponent::m_gnssOriginLatitudeDeg)
-                    ->Field("gnssOriginLongitude", &ROS2GNSSSensorComponent::m_gnssOriginLongitudeDeg)
-                    ->Field("gnssOriginAltitude", &ROS2GNSSSensorComponent::m_gnssOriginAltitude)
                     ;
 
             if (AZ::EditContext* ec = serialize->GetEditContext())
             {
                 ec->Class<ROS2GNSSSensorComponent>("ROS2 GNSS Sensor", "GNSS sensor component")
                         ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
                         ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
-                        ->DataElement(AZ::Edit::UIHandlers::Default, &ROS2GNSSSensorComponent::m_gnssOriginLatitudeDeg,
-                                      "Latitude offset", "GNSS latitude position offset in degrees")
-                        ->DataElement(AZ::Edit::UIHandlers::Default, &ROS2GNSSSensorComponent::m_gnssOriginLongitudeDeg,
-                                      "Longitude offset", "GNSS longitude position offset in degrees")
-                        ->DataElement(AZ::Edit::UIHandlers::Default, &ROS2GNSSSensorComponent::m_gnssOriginAltitude,
-                                      "Altitude offset", "GNSS altitude position offset in meters")
                         ;
             }
         }
@@ -83,10 +72,9 @@ namespace ROS2
     void ROS2GNSSSensorComponent::FrequencyTick()
     {
         const AZ::Vector3 currentPosition = GetCurrentPose().GetTranslation();
-        const AZ::Vector3 currentPositionECEF =
-                GNSS::ENUToECEF({m_gnssOriginLatitudeDeg, m_gnssOriginLongitudeDeg, m_gnssOriginAltitude},
-                                 currentPosition);
-        const AZ::Vector3 currentPositionWGS84 = GNSS::ECEFToWGS84(currentPositionECEF);
+
+        AZ::Vector3 currentPositionWGS84;
+        MapRequestBus::BroadcastResult(currentPositionWGS84, &MapRequests::LocalToLatLon, currentPosition);
 
         m_gnssMsg.latitude = currentPositionWGS84.GetX();
         m_gnssMsg.longitude = currentPositionWGS84.GetY();

Code/Source/GNSS/ROS2GNSSSensorComponent.h

@@ -31,10 +31,6 @@ namespace ROS2
         void Deactivate() override;
 
     private:
-        float m_gnssOriginLatitudeDeg = 0.0f;
-        float m_gnssOriginLongitudeDeg = 0.0f;
-        float m_gnssOriginAltitude = 0.0f;
-
         void FrequencyTick() override;
 
         AZ::Transform GetCurrentPose() const;

Code/Source/GNSS/GNSSFormatConversions.cpp → Code/Source/Map/GeodeticTransforms.cpp

@@ -6,7 +6,7 @@
  *
  */
 
-#include "GNSSFormatConversions.h"
+#include "GeodeticTransforms.h"
 
 constexpr double earthSemimajorAxis = 6378137.0f;
 constexpr double reciprocalFlattening = 1 / 298.257223563f;
@@ -15,7 +15,7 @@ constexpr double firstEccentricitySquared = 2 * reciprocalFlattening - reciproca
 constexpr double secondEccentrictySquared = reciprocalFlattening * (2.0f - reciprocalFlattening) / ((1.0f - reciprocalFlattening) * (1.0f - reciprocalFlattening));
 
 // Based on http://wiki.gis.com/wiki/index.php/Geodetic_system
-namespace ROS2::GNSS {
+namespace ROS2::Utilities::GeodeticTransforms {
     float Rad2Deg(float rad) {
         return rad * 180.0f / AZ::Constants::Pi;
     }
@@ -97,4 +97,4 @@ namespace ROS2::GNSS {
         return {Rad2Deg(static_cast<float>(latitude)), Rad2Deg(static_cast<float>(longitude)), static_cast<float>(altitude)};
     }
 
-} // namespace ROS2::GNSS
+} // namespace ROS2::Utilities::GeodeticTransforms

Code/Source/GNSS/GNSSFormatConversions.h → Code/Source/Map/GeodeticTransforms.h

@@ -10,7 +10,7 @@
 
 #include "AzCore/Math/Matrix4x4.h"
 
-namespace ROS2::GNSS {
+namespace ROS2::Utilities::GeodeticTransforms {
     //! Converts radians to degrees
     float Rad2Deg(float rad);
 
@@ -46,4 +46,4 @@ namespace ROS2::GNSS {
     //!     latitude and longitude are in decimal degrees
     //!     altitude is in meters
     AZ::Vector3 ECEFToWGS84(const AZ::Vector3& ECFEPoint);
-} // namespace ROS2::GNSS
+} // namespace ROS2::Map

Code/Source/Map/MapBus.h

@@ -0,0 +1,39 @@
+/*
+* 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/EBus/EBus.h>
+#include <AzCore/Interface/Interface.h>
+
+namespace ROS2
+{
+    class MapRequests
+    {
+    public:
+        AZ_RTTI(MapRequests, "{a95c07bd-d8d6-4e24-b089-e96d34d79ebb}");
+        virtual ~MapRequests() = default;
+
+        // Put your public methods here
+        virtual AZ::Vector3 LocalToLatLon(const AZ::Vector3 &local) = 0;
+    };
+
+    class MapBusTraits
+            : public AZ::EBusTraits
+    {
+    public:
+        //////////////////////////////////////////////////////////////////////////
+        // EBusTraits overrides
+        static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Single;
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::Single;
+        //////////////////////////////////////////////////////////////////////////
+    };
+
+    using MapRequestBus = AZ::EBus<MapRequests, MapBusTraits>;
+
+} // namespace ROS2

Code/Source/Map/MapConfiguration.cpp

@@ -0,0 +1,82 @@
+/*
+ * 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 <AzCore/Component/ComponentApplicationBus.h>
+#include <AzCore/StringFunc/StringFunc.h>
+#include <AzCore/Serialization/EditContext.h>
+
+#include "Map/MapConfiguration.h"
+
+namespace ROS2
+{
+    AZ::Outcome<void, AZStd::string> MapConfiguration::ValidateHandle(void* newValue, [[maybe_unused]] const AZ::Uuid& valueType)
+    {
+        // Check if the object type is valid
+        if (azrtti_typeid<AZ::EntityId>() != valueType)
+        {
+            AZ_Assert(false, "Unexpected value type");
+            return AZ::Failure(AZStd::string("Trying to set an entity ID to something that isn't an entity ID."));
+        }
+
+        // Check if entity id is valid
+        AZ::EntityId actualEntityId = *reinterpret_cast<AZ::EntityId*>(newValue);
+        if (!actualEntityId.IsValid())
+        {
+            return AZ::Failure(AZStd::string("Invalid entity ID."));
+        }
+
+        // Check if entity exists
+        AZ::Entity* entity = nullptr;
+        AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationBus::Events::FindEntity, actualEntityId);
+        if (entity == nullptr)
+        {
+            return AZ::Failure(AZStd::string("Can't find entity."));
+        }
+
+        return AZ::Success();
+    }
+
+    bool MapConfiguration::IsMapHookUsed() const
+    {
+        return m_useMapHook;
+    }
+
+    void MapConfiguration::Reflect(AZ::ReflectContext* context)
+    {
+        if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serializeContext->Class<MapConfiguration>()
+                    ->Version(1)
+                    ->Field("useMapHook", &MapConfiguration::m_useMapHook)
+                    ->Field("mapHook", &MapConfiguration::m_mapHook)
+                    ->Field("originLatitude", &MapConfiguration::m_originLatitudeDeg)
+                    ->Field("originLongitude", &MapConfiguration::m_originLongitudeDeg)
+                    ->Field("originAltitude", &MapConfiguration::m_originAltitude)
+
+                    ;
+
+            if (AZ::EditContext* ec = serializeContext->GetEditContext())
+            {
+                ec->Class<MapConfiguration>("Map configuration", "Map origin configuration")
+                        ->DataElement(AZ::Edit::UIHandlers::Default, &MapConfiguration::m_useMapHook,
+                                      "Use map hook", "Should a map hook be used to position the scene in the world.")
+                            ->Attribute(AZ::Edit::Attributes::ChangeNotify, AZ::Edit::PropertyRefreshLevels::EntireTree)
+                        ->DataElement(AZ::Edit::UIHandlers::Default, &MapConfiguration::m_mapHook, "Map hook", "Map origin handle.")
+                            ->Attribute(AZ::Edit::Attributes::ChangeValidate, &MapConfiguration::ValidateHandle)
+                            ->Attribute(AZ::Edit::Attributes::Visibility, &MapConfiguration::IsMapHookUsed)
+                        ->DataElement(AZ::Edit::UIHandlers::Default, &MapConfiguration::m_originLatitudeDeg,
+                                      "Origin latitude", "Latitude position offset in degrees")
+                        ->DataElement(AZ::Edit::UIHandlers::Default, &MapConfiguration::m_originLongitudeDeg,
+                                      "Origin longitude", "Longitude position offset in degrees")
+                        ->DataElement(AZ::Edit::UIHandlers::Default, &MapConfiguration::m_originAltitude,
+                                      "Origin altitude", "Altitude position offset in meters")
+                        ;
+            }
+        }
+    }
+}

Code/Source/Map/MapConfiguration.h

@@ -0,0 +1,37 @@
+/*
+ * 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/Math/Transform.h>
+#include <AzCore/Component/Entity.h>
+#include <AzCore/std/string/string.h>
+
+namespace ROS2
+{
+class MapConfiguration
+{
+public:
+    AZ_TYPE_INFO(MapConfiguration, "{094e5059-698a-42ad-ae2d-36be20868004}");
+
+    static void Reflect(AZ::ReflectContext* context);
+
+    bool m_useMapHook = false;
+    bool m_isMapHookSet = false;
+    AZ::EntityId m_mapHook = AZ::EntityId{};
+    AZ::Transform m_mapHookTransform = AZ::Transform::CreateIdentity();
+
+    float m_originLatitudeDeg = 0.0f;
+    float m_originLongitudeDeg = 0.0f;
+    float m_originAltitude = 0.0f;
+
+private:
+    AZ::Outcome<void, AZStd::string> ValidateHandle(void* newValue, [[maybe_unused]] const AZ::Uuid& valueType);
+    bool IsMapHookUsed() const;
+};
+}

Code/Source/Map/MapManagerComponent.cpp

@@ -0,0 +1,84 @@
+/*
+ * 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 "Map/MapManagerComponent.h"
+#include "Map/GeodeticTransforms.h"
+
+#include <AzCore/Component/TransformBus.h>
+#include <AzCore/Serialization/EditContext.h>
+
+#include <AzCore/Debug/Trace.h>
+#include <AzCore/Math/MathStringConversions.h>
+
+namespace ROS2
+{
+
+    void MapManagerComponent::Activate()
+    {
+        MapRequestBus::Handler::BusConnect();
+    }
+
+    void MapManagerComponent::Deactivate()
+    {
+        MapRequestBus::Handler::BusDisconnect();
+    }
+
+    MapManagerComponent::MapManagerComponent()
+    {
+    }
+
+    MapManagerComponent::~MapManagerComponent()
+    {
+    }
+
+    void MapManagerComponent::Reflect(AZ::ReflectContext *context)
+    {
+        MapConfiguration::Reflect(context);
+        if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serializeContext->Class<MapManagerComponent, AZ::Component>()
+                    ->Version(1)
+                    ->Field("Map configuration", &MapManagerComponent::m_mapConfiguration)
+                    ;
+
+            if (AZ::EditContext* ec = serializeContext->GetEditContext())
+            {
+                ec->Class<MapManagerComponent>("ROS2 Map", "Map configuration")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                        ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
+                    ->DataElement(AZ::Edit::UIHandlers::Default, &MapManagerComponent::m_mapConfiguration, "Map configuration", "Map configuration")
+                        ;
+            }
+        }
+    }
+
+    AZ::Vector3 MapManagerComponent::LocalToLatLon(const AZ::Vector3 &local) {
+        if(m_mapConfiguration.m_useMapHook && !m_mapConfiguration.m_isMapHookSet) {
+            if (!m_mapConfiguration.m_mapHook.IsValid()) {
+                AZ_Warning("MapManager", false, "Map hook not set. Using identity on (0,0,0).");
+            } else {
+                AZ::TransformBus::EventResult(
+                        m_mapConfiguration.m_mapHookTransform,
+                        m_mapConfiguration.m_mapHook,
+                        &AZ::TransformBus::Events::GetWorldTM);
+            }
+            m_mapConfiguration.m_isMapHookSet = true;
+        }
+
+        AZ::Transform localInMapHookFrame =
+                m_mapConfiguration.m_mapHookTransform.GetInverse() * AZ::Transform(local, AZ::Quaternion::CreateIdentity(), 1.0);
+
+        const AZ::Vector3 currentPositionECEF =
+            Utilities::GeodeticTransforms::ENUToECEF(
+                    {m_mapConfiguration.m_originLatitudeDeg,
+                     m_mapConfiguration.m_originLongitudeDeg,
+                     m_mapConfiguration.m_originAltitude},
+                    localInMapHookFrame.GetTranslation());
+        return Utilities::GeodeticTransforms::ECEFToWGS84(currentPositionECEF);
+    }
+}

Code/Source/Map/MapManagerComponent.h

@@ -0,0 +1,38 @@
+/*
+ * 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 <AzCore/Component/Component.h>
+#include <AzCore/Math/Vector3.h>
+
+#include <Map/MapBus.h>
+#include "Map/MapConfiguration.h"
+
+#pragma once
+namespace ROS2
+{
+class MapManagerComponent
+        : public AZ::Component
+        , protected MapRequestBus::Handler
+{
+public:
+    AZ_COMPONENT(MapManagerComponent, "{043ea1d0-5751-4ee9-af73-cca4ae11c475}");
+
+    void Activate() override;
+    void Deactivate() override;
+
+    static void Reflect(AZ::ReflectContext* context);
+
+    [[nodiscard]] AZ::Vector3 LocalToLatLon(const AZ::Vector3 &local) override;
+
+    MapManagerComponent();
+    ~MapManagerComponent();
+
+private:
+    MapConfiguration m_mapConfiguration;
+};
+
+}