Map component

Code/Source/Frame/ROS2FrameComponent.cpp

@@ -7,7 +7,7 @@
  */
 
 #include "Frame/ROS2FrameComponent.h"
-#include "ROS2/ROS2Bus.h"
+#include "Map/MapBus.h"
 #include "Utilities/ROS2Names.h"
 #include <AzCore/Component/Entity.h>
 #include <AzCore/Serialization/EditContext.h>
@@ -52,8 +52,8 @@ namespace ROS2
 
     const char* ROS2FrameComponent::GetGlobalFrameName()
     {
-        // TODO - parametrize this (typically: "odom", "world" and sometimes "map")
-        return "odom";
+        AZStd::string odomFrame = MapRequestInterface::Get()->GetOdomFrameId();
+        return odomFrame.c_str();
     }
 
     bool ROS2FrameComponent::IsTopLevel() const

Code/Source/GNSS/ROS2GNSSSensorComponent.cpp

@@ -10,14 +10,13 @@
 #include "AzCore/Math/Matrix4x4.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
@@ -30,31 +29,15 @@ namespace ROS2
         if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
         {
             serialize->Class<ROS2GNSSSensorComponent, ROS2SensorComponent>()
-                ->Version(1)
-                ->Field("gnssOriginLatitude", &ROS2GNSSSensorComponent::m_gnssOriginLatitudeDeg)
-                ->Field("gnssOriginLongitude", &ROS2GNSSSensorComponent::m_gnssOriginLongitudeDeg)
-                ->Field("gnssOriginAltitude", &ROS2GNSSSensorComponent::m_gnssOriginAltitude);
+                    ->Version(1)
+                    ;
 
             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");
+                        ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                        ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
+                        ;
             }
         }
     }
@@ -90,9 +73,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::WorldPositionToLatLon, currentPosition);
 
         m_gnssMsg.latitude = currentPositionWGS84.GetX();
         m_gnssMsg.longitude = currentPositionWGS84.GetY();

Code/Source/GNSS/ROS2GNSSSensorComponent.h

@@ -30,10 +30,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/Map/GeodeticConfiguration.cpp

@@ -0,0 +1,105 @@
+/*
+ * 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/Component/TransformBus.h>
+#include <AzCore/StringFunc/StringFunc.h>
+#include <AzCore/Serialization/EditContext.h>
+
+#include "Map/GeodeticConfiguration.h"
+
+namespace ROS2::Map
+{
+    AZ::Outcome<void, AZStd::string> GeodeticConfiguration::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."));
+        }
+
+        // Check if entity has transform
+        AZ::TransformInterface* transformInterface = entity->GetTransform();
+        if (transformInterface == nullptr)
+        {
+            return AZ::Failure(AZStd::string("Entity doesn't have transform component."));
+        }
+
+        return AZ::Success();
+    }
+
+    void GeodeticConfiguration::SetHook()
+    {
+        if(m_useMapHook && !m_isMapHookSet) {
+            if (!m_mapHook.IsValid()) {
+                AZ_Warning("GeodeticConfiguration", false, "Map hook not set. Using identity on (0,0,0).");
+            } else {
+                AZ::TransformBus::EventResult(
+                        m_mapHookTransform,
+                        m_mapHook,
+                        &AZ::TransformBus::Events::GetWorldTM);
+            }
+            m_isMapHookSet = true;
+        }
+    }
+
+    bool GeodeticConfiguration::IsMapHookUsed() const
+    {
+        return m_useMapHook;
+    }
+
+    void GeodeticConfiguration::Reflect(AZ::ReflectContext* context)
+    {
+        if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serializeContext->Class<GeodeticConfiguration>()
+                    ->Version(1)
+                    ->Field("useMapHook", &GeodeticConfiguration::m_useMapHook)
+                    ->Field("mapHook", &GeodeticConfiguration::m_mapHook)
+                    ->Field("originLatitude", &GeodeticConfiguration::m_originLatitudeDeg)
+                    ->Field("originLongitude", &GeodeticConfiguration::m_originLongitudeDeg)
+                    ->Field("originAltitude", &GeodeticConfiguration::m_originAltitude)
+
+                    ;
+
+            if (AZ::EditContext* ec = serializeContext->GetEditContext())
+            {
+                ec->Class<GeodeticConfiguration>("Map configuration", "Map origin configuration")
+                        ->DataElement(AZ::Edit::UIHandlers::Default, &GeodeticConfiguration::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, &GeodeticConfiguration::m_mapHook, "Map hook", "Map origin handle.")
+                            ->Attribute(AZ::Edit::Attributes::ChangeValidate, &GeodeticConfiguration::ValidateHandle)
+                            ->Attribute(AZ::Edit::Attributes::Visibility, &GeodeticConfiguration::IsMapHookUsed)
+                        ->DataElement(AZ::Edit::UIHandlers::Default, &GeodeticConfiguration::m_originLatitudeDeg,
+                                      "Origin latitude", "Latitude position offset in degrees")
+                        ->DataElement(AZ::Edit::UIHandlers::Default, &GeodeticConfiguration::m_originLongitudeDeg,
+                                      "Origin longitude", "Longitude position offset in degrees")
+                        ->DataElement(AZ::Edit::UIHandlers::Default, &GeodeticConfiguration::m_originAltitude,
+                                      "Origin altitude", "Altitude position offset in meters")
+                        ;
+            }
+        }
+    }
+}

Code/Source/Map/GeodeticConfiguration.h

@@ -0,0 +1,43 @@
+/*
+ * 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::Map
+{
+//! Configuration for handling geodetic map info.
+//! This configuration allows setting a geographic coordinate system hook to a simulation scene.
+//! System hook points to the real-world location in latitude, longitude, and altitude offsets.
+class GeodeticConfiguration
+{
+public:
+    AZ_TYPE_INFO(GeodeticConfiguration, "{094e5059-698a-42ad-ae2d-36be20868004}");
+
+    static void Reflect(AZ::ReflectContext* context);
+
+    //! Updates the hook Transform
+    void SetHook();
+
+    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/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;
@@ -16,10 +16,9 @@ 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::Map::Utilities 
 {
-    float Rad2Deg(float rad)
-    {
+    float Rad2Deg(float rad) {
         return rad * 180.0f / AZ::Constants::Pi;
     }
 
@@ -113,4 +112,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::Map::Utilities 
 {
     //! Converts radians to degrees
     float Rad2Deg(float rad);
@@ -47,4 +47,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,69 @@
+/*
+* 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/std/string/string.h>
+#include <AzCore/EBus/EBus.h>
+#include <AzCore/Math/Vector3.h>
+#include <AzCore/Math/Transform.h>
+#include <AzCore/Interface/Interface.h>
+
+namespace ROS2
+{
+    class MapRequests
+    {
+    public:
+        AZ_RTTI(MapRequests, "{a95c07bd-d8d6-4e24-b089-e96d34d79ebb}");
+        virtual ~MapRequests() = default;
+
+        //! Convert Transform to the map coordinate system.
+        //! @param transform - Transform to be converted.
+        //! @return Copy of a Transform in the map coordinate system.
+        virtual AZ::Transform ConvertToMapCoordinateSystem(AZ::Transform transform) = 0;
+
+        //! Convert Transform from the map coordinate system.
+        //! @param transform - Transform to be converted.
+        //! @return Copy of a Transform in the world coordinate system.
+        virtual AZ::Transform ConvertFromMapCoordinateSystem(AZ::Transform transform) = 0;
+
+        //! Convert the position vector in WorldTM to longitude, latitude, and altitude in the
+        //! geographic coordinate system relative to the map frame.
+        //! @param worldPosition - position vector to be converted.
+        //! @return Vector where x: longitude, y: latitude, z: altitude.
+        virtual AZ::Vector3 WorldPositionToLatLon(const AZ::Vector3 &worldPosition) = 0;
+
+        //! Convert the geographic position relative to the map frame to the global o3de coordinate system.
+        //! @param latLonAlt - geographic position in degrees, x: latitude, y: longitude, z: altitude.
+        //! @return Vector where x: x position, y: y position, z: z position in o3de WorldTM.
+        virtual AZ::Vector3 LatLonToWorldPosition(const AZ::Vector3 &latLonAlt) = 0;
+
+
+        //! Get map frame id.
+        //! @return Map frame id.
+        virtual AZStd::string GetMapFrameId() = 0;
+
+        //! Get odometry frame id.
+        //! @return Odometry frame id.
+        virtual AZStd::string GetOdomFrameId() = 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>;
+    using MapRequestInterface = AZ::Interface<MapRequests>;
+
+} // namespace ROS2