Merging latest development

Signed-off-by: kberg-amzn <[email protected]>

.gitattributes

@@ -1,4 +1,3 @@
-
 #
 # Git LFS (see https://git-lfs.github.com/)
 #
@@ -130,3 +129,4 @@ Templates/Ros2FleetRobotTemplate/docs/images/*.png filter= diff= merge= -text
 Gems/ROS2/docs/**/*.png -filter -diff -merge
 Templates/Ros2ProjectTemplate/Screenshots/*.png filter= diff= merge= -text
 Templates/Ros2FleetRobotTemplate/Screenshots/*.png filter= diff= merge= -text
+*.pgm filter=lfs diff=lfs merge=lfs -text

Gems/OpenXRVk/Code/Include/OpenXRVk/OpenXRVkDevice.h

@@ -27,6 +27,7 @@ namespace OpenXRVk
 
         //////////////////////////////////////////////////////////////////////////
         // XR::Device overrides
+        
         // Create the xr specific native device object and populate the XRDeviceDescriptor with it.
         AZ::RHI::ResultCode InitDeviceInternal(AZ::RHI::XRDeviceDescriptor* instanceDescriptor) override;
         //! Get the Fov data  of the view specified by view index
@@ -41,8 +42,8 @@ namespace OpenXRVk
         //! Get the native device
         VkDevice GetNativeDevice() const;
 
-        //! Get glad vulkan context.
-        const GladVulkanContext& GetContext() const;
+        //! Get the native physical device
+        VkPhysicalDevice GetNativePhysicalDevice() const;
 
         //! Reserve space for appropriate number of views 
         void InitXrViews(uint32_t numViews);
@@ -67,12 +68,12 @@ namespace OpenXRVk
         //////////////////////////////////////////////////////////////////////////
 
         VkDevice m_xrVkDevice = VK_NULL_HANDLE;
+        VkPhysicalDevice m_xrVkPhysicalDevice = VK_NULL_HANDLE;
         XrFrameState m_frameState{ XR_TYPE_FRAME_STATE };
         AZStd::vector<XrCompositionLayerBaseHeader*> m_xrLayers;
         XrCompositionLayerProjection m_xrLayer{ XR_TYPE_COMPOSITION_LAYER_PROJECTION };
         AZStd::vector<XrCompositionLayerProjectionView> m_projectionLayerViews;
         AZStd::vector<XrView> m_views;
         uint32_t m_viewCountOutput = 0;
-        GladVulkanContext m_context = {};
     };
 }

Gems/OpenXRVk/Code/Include/OpenXRVk/OpenXRVkInstance.h

@@ -8,11 +8,12 @@
 
 #pragma once
 
+
 #include <AzCore/std/containers/vector.h>
-#include <Atom/RHI/ValidationLayer.h>
-#include <Atom/RHI.Loader/FunctionLoader.h>
 #include <OpenXRVk_Platform.h>
 #include <OpenXRVk/OpenXRVkPhysicalDevice.h>
+#include <Atom/RHI/ValidationLayer.h>
+#include <Atom/RHI.Reflect/Vulkan/VulkanBus.h>
 #include <XR/XRInstance.h>
 
 namespace OpenXRVk
@@ -20,7 +21,10 @@ namespace OpenXRVk
     //! Vulkan specific XR instance back-end class that will help manage
     //! XR specific vulkan native objects
     class Instance final
-    : public XR::Instance
+        : public XR::Instance
+        , public AZ::Vulkan::InstanceRequirementBus::Handler
+        , public AZ::Vulkan::DeviceRequirementBus::Handler
+        , public AZ::Vulkan::InstanceNotificationBus::Handler
     {
     public:
         AZ_CLASS_ALLOCATOR(Instance, AZ::SystemAllocator);
@@ -30,7 +34,6 @@ namespace OpenXRVk
 
         //////////////////////////////////////////////////////////////////////////
         // XR::Instance overrides
-        AZ::RHI::ResultCode InitNativeInstance(AZ::RHI::XRInstanceDescriptor* instanceDescriptor) override;
         AZ::u32 GetNumPhysicalDevices() const override;
         AZ::RHI::ResultCode GetXRPhysicalDevice(AZ::RHI::XRPhysicalDeviceDescriptor* physicalDeviceDescriptor, int32_t index) override;
         //////////////////////////////////////////////////////////////////////////
@@ -56,26 +59,29 @@ namespace OpenXRVk
         //! Get native VkInstance.
         VkInstance GetNativeInstance() const;
 
-        //! Get glad vulkan context.
-        GladVulkanContext& GetContext();
-
-        //! Get function loader.
-        AZ::Vulkan::FunctionLoader& GetFunctionLoader();
-
         //! Get XR environment blend mode.
         XrEnvironmentBlendMode GetEnvironmentBlendMode() const;
 
         //! Get XR configuration type.
         XrViewConfigurationType GetViewConfigType() const;
 
-        //! Ge the active VkPhysicalDevice.
-        VkPhysicalDevice GetActivePhysicalDevice() const;
-
     protected:
         // XR::Instance overrides...
         AZ::RHI::ResultCode InitInstanceInternal() override;
         void ShutdownInternal() override;
 
+        // AZ::Vulkan::InstanceRequirementBus::Handler overrides..
+        void CollectAdditionalRequiredInstanceExtensions(AZStd::vector<AZStd::string>& extensions) override;
+        void CollectMinMaxVulkanAPIVersions(AZStd::vector<uint32_t>& min, AZStd::vector<uint32_t>& max) override;
+
+        // AZ::Vulkan::DeviceRequirementBus::Handler overrides...
+        void CollectAdditionalRequiredDeviceExtensions(AZStd::vector<AZStd::string>& extensions) override;
+        void FilterSupportedDevices(AZStd::vector<VkPhysicalDevice>& supportedDevices) override;
+
+        // AZ::Vulkan::InstanceNotificationBus::Handler overrides...
+        virtual void OnInstanceCreated([[maybe_unused]] VkInstance instance);
+        virtual void OnInstanceDestroyed();
+
     private:
         XrInstance m_xrInstance = XR_NULL_HANDLE;
         VkInstance m_xrVkInstance = VK_NULL_HANDLE;
@@ -85,9 +91,10 @@ namespace OpenXRVk
         XrSystemId m_xrSystemId = XR_NULL_SYSTEM_ID;
         XR::RawStringList m_requiredLayers;
         XR::RawStringList m_requiredExtensions;
-        GladVulkanContext m_context = {};
-        AZStd::unique_ptr<AZ::Vulkan::FunctionLoader> m_functionLoader;
+        XR::StringList m_requireVulkanInstanceExtensions;
+        XR::StringList m_requireVulkanDeviceExtensions;
         AZStd::vector<VkPhysicalDevice> m_supportedXRDevices;
-        AZ::u32 m_physicalDeviceActiveIndex = 0;
+        uint32_t m_minVulkanAPIVersion = 0;
+        uint32_t m_maxVulkanAPIVersion = 0;
     };
 }

Gems/OpenXRVk/Code/Include/OpenXRVk/OpenXRVkSystemComponent.h

@@ -24,8 +24,8 @@ namespace OpenXRVk
         static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided);
         static void Reflect(AZ::ReflectContext* context);
 
-        SystemComponent();
-        ~SystemComponent();
+        SystemComponent() = default;
+        ~SystemComponent() = default;
 
         //////////////////////////////////////////////////////////////////////////
         // Component
@@ -59,5 +59,8 @@ namespace OpenXRVk
         //! Create XR::Swapchain::Image object.
         XR::Ptr<XR::SwapChain::Image> CreateSwapChainImage() override;
         ///////////////////////////////////////////////////////////////////
+
+    private:
+        XR::Ptr<XR::Instance> m_instance;
     };
 }

Gems/OpenXRVk/Code/Include/OpenXRVk/OpenXRVkUtils.h

@@ -63,7 +63,4 @@ namespace OpenXRVk
     //! Iterate through the characters while caching the starting pointer to a string
     //! and every time ' ' is encountered replace it with '\0' to indicate the end of a string.
     AZStd::vector<const char*> ParseExtensionString(char* names);
-
-    //! Disable certain extensions because function pointers didn't load correctly.
-    void FilterAvailableExtensions(GladVulkanContext& context);
 }

Gems/OpenXRVk/Code/Source/OpenXRVkDevice.cpp

@@ -27,90 +27,8 @@ namespace OpenXRVk
     AZ::RHI::ResultCode Device::InitDeviceInternal(AZ::RHI::XRDeviceDescriptor* deviceDescriptor)
     {
         AZ::Vulkan::XRDeviceDescriptor* xrDeviceDescriptor = static_cast<AZ::Vulkan::XRDeviceDescriptor*>(deviceDescriptor);
-        Instance* xrVkInstance = static_cast<Instance*>(GetDescriptor().m_instance.get());
-        XrVulkanDeviceCreateInfoKHR xrDeviceCreateInfo{ XR_TYPE_VULKAN_DEVICE_CREATE_INFO_KHR };
-        xrDeviceCreateInfo.systemId = xrVkInstance->GetXRSystemId();
-        xrDeviceCreateInfo.pfnGetInstanceProcAddr = xrVkInstance->GetContext().GetInstanceProcAddr;
-        xrDeviceCreateInfo.vulkanCreateInfo = xrDeviceDescriptor->m_inputData.m_deviceCreateInfo;
-        xrDeviceCreateInfo.vulkanPhysicalDevice = xrVkInstance->GetActivePhysicalDevice();
-        xrDeviceCreateInfo.vulkanAllocator = AZ::Vulkan::VkSystemAllocator::Get();
-
-        PFN_xrGetVulkanDeviceExtensionsKHR pfnGetVulkanDeviceExtensionsKHR = nullptr;
-        XrResult result = xrGetInstanceProcAddr(
-            xrVkInstance->GetXRInstance(), "xrGetVulkanDeviceExtensionsKHR", reinterpret_cast<PFN_xrVoidFunction*>(&pfnGetVulkanDeviceExtensionsKHR));
-        ASSERT_IF_UNSUCCESSFUL(result);
-
-        AZ::u32 deviceExtensionNamesSize = 0;
-        result = pfnGetVulkanDeviceExtensionsKHR(xrVkInstance->GetXRInstance(), xrDeviceCreateInfo.systemId, 0, &deviceExtensionNamesSize, nullptr);
-        ASSERT_IF_UNSUCCESSFUL(result);
-
-        AZStd::vector<char> deviceExtensionNames(deviceExtensionNamesSize);
-        result = pfnGetVulkanDeviceExtensionsKHR(
-            xrVkInstance->GetXRInstance(), xrDeviceCreateInfo.systemId, deviceExtensionNamesSize, &deviceExtensionNamesSize, &deviceExtensionNames[0]);
-        ASSERT_IF_UNSUCCESSFUL(result);
-
-        AZStd::vector<const char*> extensions = ParseExtensionString(&deviceExtensionNames[0]);
-        for (uint32_t i = 0; i < xrDeviceCreateInfo.vulkanCreateInfo->enabledExtensionCount; ++i)
-        {
-            extensions.push_back(xrDeviceCreateInfo.vulkanCreateInfo->ppEnabledExtensionNames[i]);
-        }
-
-        if (GetDescriptor().m_validationMode == AZ::RHI::ValidationMode::Enabled)
-        {
-            AZ_Printf("OpenXRVk", "Vulkan device extensions to enable: (%i)\n", extensions.size());
-            for (const AZStd::string& extension : extensions)
-            {
-                AZ_Printf("OpenXRVk", "Name=%s\n", extension.c_str());
-            }
-        }
-
-        VkPhysicalDeviceFeatures features{};
-        memcpy(&features, xrDeviceCreateInfo.vulkanCreateInfo->pEnabledFeatures, sizeof(features));
-
-        VkPhysicalDeviceFeatures availableFeatures{};
-        xrVkInstance->GetContext().GetPhysicalDeviceFeatures(xrVkInstance->GetActivePhysicalDevice(), &availableFeatures);
-        if (availableFeatures.shaderStorageImageMultisample == VK_TRUE)
-        {
-            // Setting this quiets down a validation error triggered by the Oculus runtime
-            features.shaderStorageImageMultisample = VK_TRUE;
-        }
-
-        VkDeviceCreateInfo deviceInfo{ VK_STRUCTURE_TYPE_DEVICE_CREATE_INFO };
-        memcpy(&deviceInfo, xrDeviceCreateInfo.vulkanCreateInfo, sizeof(deviceInfo));
-        deviceInfo.pEnabledFeatures = &features;
-        deviceInfo.enabledExtensionCount = aznumeric_cast<AZ::u32>(extensions.size());
-        deviceInfo.ppEnabledExtensionNames = extensions.empty() ? nullptr : extensions.data();
-
-        //Create VkDevice
-        auto pfnCreateDevice = (PFN_vkCreateDevice)xrDeviceCreateInfo.pfnGetInstanceProcAddr(xrVkInstance->GetNativeInstance(), "vkCreateDevice");
-        VkResult vulkanResult = pfnCreateDevice(xrDeviceCreateInfo.vulkanPhysicalDevice, &deviceInfo, xrDeviceCreateInfo.vulkanAllocator, &m_xrVkDevice);
-        if (vulkanResult != VK_SUCCESS)
-        {
-            ShutdownInternal();
-            AZ_Error("OpenXRVk", false, "Failed to create the device.");
-            return AZ::RHI::ResultCode::Fail;
-        }
-
-        {
-            VkPhysicalDevice xrVkPhysicalDevice = xrVkInstance->GetActivePhysicalDevice();
-            // Now that we have created the device, load the function pointers for it.
-            const bool functionsLoaded = xrVkInstance->GetFunctionLoader().LoadProcAddresses(
-                &m_context, xrVkInstance->GetNativeInstance(), xrVkPhysicalDevice, m_xrVkDevice);
-
-            FilterAvailableExtensions(m_context);
-
-            if (!functionsLoaded)
-            {
-                ShutdownInternal();
-                AZ_Error("OpenXRVk", false, "Failed to initialize function loader for the device.");
-                return AZ::RHI::ResultCode::Fail;
-            }
-        }
-
-        //Populate the output data of the descriptor
-        xrDeviceDescriptor->m_outputData.m_xrVkDevice = m_xrVkDevice;
-        xrDeviceDescriptor->m_outputData.m_context = m_context;
-
+        m_xrVkDevice = xrDeviceDescriptor->m_inputData.m_xrVkDevice;
+        m_xrVkPhysicalDevice = xrDeviceDescriptor->m_inputData.m_xrVkPhysicalDevice;
         return AZ::RHI::ResultCode::Success;
     }
 
@@ -259,9 +177,9 @@ namespace OpenXRVk
         return m_xrVkDevice;
     }
 
-    const GladVulkanContext& Device::GetContext() const
+    VkPhysicalDevice Device::GetNativePhysicalDevice() const
     {
-        return m_context;
+        return m_xrVkPhysicalDevice;
     }
 
     AZ::RHI::ResultCode Device::GetViewFov(AZ::u32 viewIndex, AZ::RPI::FovData& outFovData) const
@@ -305,10 +223,7 @@ namespace OpenXRVk
         m_projectionLayerViews.clear();
         m_views.clear();
         m_xrLayers.clear();
-        if (m_xrVkDevice != VK_NULL_HANDLE)
-        {
-            m_context.DestroyDevice(m_xrVkDevice, AZ::Vulkan::VkSystemAllocator::Get());
-            m_xrVkDevice = VK_NULL_HANDLE;
-        }
+        m_xrVkDevice = VK_NULL_HANDLE;
+        m_xrVkPhysicalDevice = VK_NULL_HANDLE;
     }
 }

Gems/OpenXRVk/Code/Source/OpenXRVkInstance.cpp

@@ -191,6 +191,18 @@ namespace OpenXRVk
         graphicsRequirements.maxApiVersionSupported = legacyRequirements.maxApiVersionSupported;
         graphicsRequirements.minApiVersionSupported = legacyRequirements.minApiVersionSupported;
 
+        m_minVulkanAPIVersion = VK_MAKE_API_VERSION(
+            0,
+            XR_VERSION_MAJOR(graphicsRequirements.minApiVersionSupported),
+            XR_VERSION_MINOR(graphicsRequirements.minApiVersionSupported),
+            XR_VERSION_PATCH(graphicsRequirements.minApiVersionSupported));
+
+        m_maxVulkanAPIVersion = VK_MAKE_API_VERSION(
+            0,
+            XR_VERSION_MAJOR(graphicsRequirements.maxApiVersionSupported),
+            XR_VERSION_MINOR(graphicsRequirements.maxApiVersionSupported),
+            XR_VERSION_PATCH(graphicsRequirements.maxApiVersionSupported));
+
         if (m_validationMode == AZ::RHI::ValidationMode::Enabled)
         {
             AZ_Printf("OpenXRVk", "graphicsRequirements.maxApiVersionSupported %d.%d.%d\n",
@@ -206,7 +218,62 @@ namespace OpenXRVk
             AZ_Printf("OpenXRVk", "Using system %d for form factor %s\n", m_xrSystemId, to_string(m_formFactor));
             LogViewConfigurations();
         }
-		
+
+        PFN_xrGetVulkanInstanceExtensionsKHR pfnGetVulkanInstanceExtensionsKHR = nullptr;
+        result = xrGetInstanceProcAddr(m_xrInstance, "xrGetVulkanInstanceExtensionsKHR", reinterpret_cast<PFN_xrVoidFunction*>(&pfnGetVulkanInstanceExtensionsKHR));
+        ASSERT_IF_UNSUCCESSFUL(result);
+
+        AZ::u32 extensionNamesSize = 0;
+        result = pfnGetVulkanInstanceExtensionsKHR(m_xrInstance, m_xrSystemId, 0, &extensionNamesSize, nullptr);
+        ASSERT_IF_UNSUCCESSFUL(result);
+
+        AZStd::vector<char> extensionNames(extensionNamesSize);
+        result = pfnGetVulkanInstanceExtensionsKHR(m_xrInstance, m_xrSystemId, extensionNamesSize, &extensionNamesSize, &extensionNames[0]);
+        ASSERT_IF_UNSUCCESSFUL(result);
+
+        AZStd::vector<const char*> requiredInstanceExtensions = ParseExtensionString(&extensionNames[0]);
+        m_requireVulkanInstanceExtensions.insert(m_requireVulkanInstanceExtensions.end(), requiredInstanceExtensions.begin(), requiredInstanceExtensions.end());
+        if (m_validationMode == AZ::RHI::ValidationMode::Enabled)
+        {
+            AZ_Printf("OpenXRVk", "Vulkan instance extensions to enable: (%i)\n", requiredInstanceExtensions.size());
+            for (const AZStd::string& extension : requiredInstanceExtensions)
+            {
+                AZ_Printf("OpenXRVk", "Name=%s\n", extension.c_str());
+            }
+        }
+
+        AZ::Vulkan::InstanceRequirementBus::Handler::BusConnect();
+
+        PFN_xrGetVulkanDeviceExtensionsKHR pfnGetVulkanDeviceExtensionsKHR = nullptr;
+        result = xrGetInstanceProcAddr(
+            m_xrInstance,
+            "xrGetVulkanDeviceExtensionsKHR",
+            reinterpret_cast<PFN_xrVoidFunction*>(&pfnGetVulkanDeviceExtensionsKHR));
+        ASSERT_IF_UNSUCCESSFUL(result);
+
+        AZ::u32 deviceExtensionNamesSize = 0;
+        result = pfnGetVulkanDeviceExtensionsKHR(m_xrInstance, m_xrSystemId, 0, &deviceExtensionNamesSize, nullptr);
+        ASSERT_IF_UNSUCCESSFUL(result);
+
+        AZStd::vector<char> deviceExtensionNames(deviceExtensionNamesSize);
+        result = pfnGetVulkanDeviceExtensionsKHR(
+            m_xrInstance, m_xrSystemId, deviceExtensionNamesSize, &deviceExtensionNamesSize, &deviceExtensionNames[0]);
+        ASSERT_IF_UNSUCCESSFUL(result);
+
+        AZStd::vector<const char*> requiredDeviceExtensions = ParseExtensionString(&deviceExtensionNames[0]);
+        m_requireVulkanDeviceExtensions.insert(m_requireVulkanDeviceExtensions.end(), requiredDeviceExtensions.begin(), requiredDeviceExtensions.end());
+        if (m_validationMode == AZ::RHI::ValidationMode::Enabled)
+        {
+            AZ_Printf("OpenXRVk", "Vulkan device extensions to enable: (%i)\n", requiredDeviceExtensions.size());
+            for (const AZStd::string& extension : requiredDeviceExtensions)
+            {
+                AZ_Printf("OpenXRVk", "Name=%s\n", extension.c_str());
+            }
+        }
+
+        AZ::Vulkan::DeviceRequirementBus::Handler::BusConnect();
+        AZ::Vulkan::InstanceNotificationBus::Handler::BusConnect();
+
         return AZ::RHI::ResultCode::Success;
     }
 
@@ -289,109 +356,50 @@ namespace OpenXRVk
         }
     }
 
-    AZ::RHI::ResultCode Instance::InitNativeInstance(AZ::RHI::XRInstanceDescriptor* instanceDescriptor)
+    void Instance::ShutdownInternal()
     {
-        m_functionLoader = AZ::Vulkan::FunctionLoader::Create();
-        if (!m_functionLoader->Init() ||
-            !m_functionLoader->LoadProcAddresses(&m_context, VK_NULL_HANDLE, VK_NULL_HANDLE, VK_NULL_HANDLE))
-        {
-            m_functionLoader.reset();
-            AZ_Error("OpenXRVk", false, "Could not initialize function loader.");
-            return AZ::RHI::ResultCode::Fail;
-        }
-
-        AZ::Vulkan::XRInstanceDescriptor* xrInstanceDescriptor = static_cast<AZ::Vulkan::XRInstanceDescriptor*>(instanceDescriptor);
-        XrVulkanInstanceCreateInfoKHR createInfo{ XR_TYPE_VULKAN_INSTANCE_CREATE_INFO_KHR };
-        createInfo.systemId = m_xrSystemId;
-        createInfo.pfnGetInstanceProcAddr = m_context.GetInstanceProcAddr;
-        createInfo.vulkanCreateInfo = xrInstanceDescriptor->m_inputData.m_createInfo;
-        createInfo.vulkanAllocator = AZ::Vulkan::VkSystemAllocator::Get();
+        AZ::Vulkan::InstanceNotificationBus::Handler::BusDisconnect();
+        AZ::Vulkan::DeviceRequirementBus::Handler::BusDisconnect();
+        AZ::Vulkan::InstanceRequirementBus::Handler::BusDisconnect();
 
-        PFN_xrGetVulkanInstanceExtensionsKHR pfnGetVulkanInstanceExtensionsKHR = nullptr;
-        XrResult result = xrGetInstanceProcAddr(m_xrInstance, "xrGetVulkanInstanceExtensionsKHR", reinterpret_cast<PFN_xrVoidFunction*>(&pfnGetVulkanInstanceExtensionsKHR));
-        ASSERT_IF_UNSUCCESSFUL(result);
-
-        AZ::u32 extensionNamesSize = 0;
-        result = pfnGetVulkanInstanceExtensionsKHR(m_xrInstance, m_xrSystemId, 0, &extensionNamesSize, nullptr);
-        ASSERT_IF_UNSUCCESSFUL(result);
-
-        AZStd::vector<char> extensionNames(extensionNamesSize);
-        result = pfnGetVulkanInstanceExtensionsKHR(m_xrInstance, m_xrSystemId, extensionNamesSize, &extensionNamesSize, &extensionNames[0]);
-        ASSERT_IF_UNSUCCESSFUL(result);
-
-        AZStd::vector<const char*> extensions = ParseExtensionString(&extensionNames[0]);
-        for (uint32_t i = 0; i < createInfo.vulkanCreateInfo->enabledExtensionCount; ++i)
-        {
-            extensions.push_back(createInfo.vulkanCreateInfo->ppEnabledExtensionNames[i]);
-        }
+        m_supportedXRDevices.clear();
+        m_xrVkInstance = VK_NULL_HANDLE;
+    }
 
-        if (m_validationMode == AZ::RHI::ValidationMode::Enabled)
-        {
-            AZ_Printf("OpenXRVk", "Vulkan instance extensions to enable: (%i)\n", extensions.size());
-            for (const AZStd::string& extension : extensions)
-            {
-                AZ_Printf("OpenXRVk", "Name=%s\n", extension.c_str());
-            }
-        }
+    void Instance::CollectAdditionalRequiredInstanceExtensions(AZStd::vector<AZStd::string>& extensions)
+    {
+        extensions.insert(extensions.end(), m_requireVulkanInstanceExtensions.begin(), m_requireVulkanInstanceExtensions.end());
+    }
 
-        VkInstanceCreateInfo instInfo{ VK_STRUCTURE_TYPE_INSTANCE_CREATE_INFO };
-        memcpy(&instInfo, createInfo.vulkanCreateInfo, sizeof(instInfo));
-        instInfo.enabledExtensionCount = aznumeric_cast<AZ::u32>(extensions.size());
-        instInfo.ppEnabledExtensionNames = extensions.empty() ? nullptr : extensions.data();
+    void Instance::CollectMinMaxVulkanAPIVersions(AZStd::vector<uint32_t>& min, AZStd::vector<uint32_t>& max)
+    {
+        min.push_back(m_minVulkanAPIVersion);
+        max.push_back(m_maxVulkanAPIVersion);
+    }
 
-        auto pfnCreateInstance = (PFN_vkCreateInstance)createInfo.pfnGetInstanceProcAddr(nullptr, "vkCreateInstance");
-        VkResult vkResult = pfnCreateInstance(&instInfo, AZ::Vulkan::VkSystemAllocator::Get(), &m_xrVkInstance);
-        if (vkResult != VK_SUCCESS)
-        {
-            ShutdownInternal();
-            AZ_Error("OpenXRVk", false, "Failed to create the instance.");
-            return AZ::RHI::ResultCode::Fail;
-        }
+    void Instance::CollectAdditionalRequiredDeviceExtensions(AZStd::vector<AZStd::string>& extensions)
+    {
+        extensions.insert(extensions.end(), m_requireVulkanDeviceExtensions.begin(), m_requireVulkanDeviceExtensions.end());
+    }
 
-        // Now that we have created the instance, load the function pointers for it.
-        if (!m_functionLoader->LoadProcAddresses(&m_context, m_xrVkInstance, VK_NULL_HANDLE, VK_NULL_HANDLE))
+    void Instance::FilterSupportedDevices(AZStd::vector<VkPhysicalDevice>& supportedDevices)
+    {
+        AZStd::erase_if(supportedDevices, [&](const auto& physicalDevice)
         {
-            ShutdownInternal();
-            AZ_Warning("OpenXRVk", false, "Failed to initialize function loader for the instance.");
-            return AZ::RHI::ResultCode::Fail;
-        }
-
-        FilterAvailableExtensions(m_context);
-
-        //Populate the instance descriptor with the correct VkInstance
-        xrInstanceDescriptor->m_outputData.m_xrVkInstance = m_xrVkInstance;
-        xrInstanceDescriptor->m_outputData.m_context = m_context;
+            return AZStd::find(m_supportedXRDevices.begin(), m_supportedXRDevices.end(), physicalDevice) == m_supportedXRDevices.end();
+        });
+    }
 
-        //Get the list of Physical devices
+    void Instance::OnInstanceCreated(VkInstance instance)
+    {
+        m_xrVkInstance = instance;
         m_supportedXRDevices = PhysicalDevice::EnumerateDeviceList(m_xrSystemId, m_xrInstance, m_xrVkInstance);
-        if (m_supportedXRDevices.empty())
-        {
-            ShutdownInternal();
-            AZ_Error("OpenXRVk", false, "No physical devices found.");
-            return AZ::RHI::ResultCode::Fail;
-        }
-
-        //Just use the first device at the moment.
-        m_physicalDeviceActiveIndex = 0;
-
-        return AZ::RHI::ResultCode::Success;
     }
 
-    void Instance::ShutdownInternal()
+    void Instance::OnInstanceDestroyed()
     {
-        if (m_xrVkInstance != VK_NULL_HANDLE)
-        {
-            m_supportedXRDevices.clear();
-
-            m_context.DestroyInstance(m_xrVkInstance, AZ::Vulkan::VkSystemAllocator::Get());
-            m_xrVkInstance = VK_NULL_HANDLE;
-        }
-
-        if (m_functionLoader)
-        {
-            m_functionLoader->Shutdown();
-        }
-        m_functionLoader = nullptr;
+        m_xrVkInstance = XR_NULL_HANDLE;
+        m_supportedXRDevices.clear();
     }
 
     AZ::RHI::ResultCode Instance::GetXRPhysicalDevice(AZ::RHI::XRPhysicalDeviceDescriptor* physicalDeviceDescriptor, int32_t index)
@@ -410,12 +418,6 @@ namespace OpenXRVk
         return aznumeric_cast<AZ::u32>(m_supportedXRDevices.size());
     }
 
-    VkPhysicalDevice Instance::GetActivePhysicalDevice() const
-    {
-        AZ_Assert(m_physicalDeviceActiveIndex < m_supportedXRDevices.size(), "Index out of range");
-        return m_supportedXRDevices[m_physicalDeviceActiveIndex];
-    }
-
     XrInstance Instance::GetXRInstance() const
     {
         return m_xrInstance;
@@ -431,16 +433,6 @@ namespace OpenXRVk
         return m_xrVkInstance;
     }
 
-    GladVulkanContext& Instance::GetContext()
-    {
-        return m_context;
-    }
-
-    AZ::Vulkan::FunctionLoader& Instance::GetFunctionLoader()
-    {
-        return *m_functionLoader;
-    }
-
     XrEnvironmentBlendMode Instance::GetEnvironmentBlendMode() const
     {
         return m_environmentBlendMode;

Gems/OpenXRVk/Code/Source/OpenXRVkSession.cpp

@@ -33,7 +33,7 @@ namespace OpenXRVk
         m_xrInstance = xrVkInstance->GetXRInstance();
         AZ_Printf("OpenXRVk", "Creating session...\n");
         m_graphicsBinding.instance = xrVkInstance->GetNativeInstance();
-        m_graphicsBinding.physicalDevice = xrVkInstance->GetActivePhysicalDevice();
+        m_graphicsBinding.physicalDevice = xrVkDevice->GetNativePhysicalDevice();
         m_graphicsBinding.device = xrVkDevice->GetNativeDevice();
         m_graphicsBinding.queueFamilyIndex = sessionDescriptor->m_inputData.m_graphicsBinding.m_queueFamilyIndex;
         m_graphicsBinding.queueIndex = sessionDescriptor->m_inputData.m_graphicsBinding.m_queueIndex;

Gems/OpenXRVk/Code/Source/OpenXRVkSystemComponent.cpp

@@ -9,6 +9,8 @@
 
 #include <AzCore/Serialization/SerializeContext.h>
 
+#include <Atom/RHI/FactoryManagerBus.h>
+
 #include <OpenXRVk/OpenXRVkDevice.h>
 #include <OpenXRVk/OpenXRVkInput.h>
 #include <OpenXRVk/OpenXRVkInstance.h>
@@ -17,11 +19,14 @@
 #include <OpenXRVk/OpenXRVkSwapChain.h>
 #include <OpenXRVk/OpenXRVkSystemComponent.h>
 
+#include <XR/XRUtils.h>
+
 namespace OpenXRVk
 {
     void SystemComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
     {
         provided.push_back(XR::Factory::GetPlatformService());
+        provided.push_back(AZ_CRC_CE("VulkanRequirementsService"));
     }
 
     void SystemComponent::Reflect(AZ::ReflectContext* context)
@@ -35,17 +40,6 @@ namespace OpenXRVk
         AzFramework::InputDeviceXRController::Reflect(context);
     }
 
-    SystemComponent::SystemComponent()
-    {
-        // Only have Vulkan back-end implementation for XR at the moment so register it. 
-        XR::Factory::Register(this);
-    }
-
-    SystemComponent::~SystemComponent()
-    {
-        XR::Factory::Unregister(this);
-    }
-
     XR::Ptr<XR::Instance> SystemComponent::CreateInstance()
     {
         return Instance::Create();
@@ -88,9 +82,33 @@ namespace OpenXRVk
 
     void SystemComponent::Activate()
     {
+        if (XR::IsOpenXREnabled())
+        {
+            m_instance = CreateInstance();
+            //Get the validation mode
+            AZ::RHI::ValidationMode validationMode = AZ::RHI::ValidationMode::Disabled;
+            AZ::RHI::FactoryManagerBus::BroadcastResult(validationMode, &AZ::RHI::FactoryManagerRequest::DetermineValidationMode);
+
+            if (m_instance->Init(validationMode) == AZ::RHI::ResultCode::Success)
+            {
+                XR::Factory::Register(this);
+                AZ::Interface<XR::Instance>::Register(m_instance.get());
+            }
+            else
+            {
+                AZ_Warning("OpenXRVK", false, "OpenXRVK is not supported on this platform");
+                m_instance = nullptr;
+            }
+        }
     }
 
     void SystemComponent::Deactivate()
     {
+        if (m_instance)
+        {
+            XR::Factory::Unregister(this);
+            AZ::Interface<XR::Instance>::Unregister(m_instance.get());
+            m_instance = nullptr;
+        }
     }
 }

Gems/OpenXRVk/Code/Source/OpenXRVkUtils.cpp

@@ -74,14 +74,4 @@ namespace OpenXRVk
         }
         return list;
     }
-
-    void FilterAvailableExtensions(GladVulkanContext& context)
-    {
-        // In some cases (like when running with the GPU profiler on Quest2) the extension is reported as available
-        // but the function pointers do not load. Disable the extension if that's the case.
-        if (context.EXT_debug_utils && !context.CmdBeginDebugUtilsLabelEXT)
-        {
-            context.EXT_debug_utils = 0;
-        }
-    }
 }

Gems/ProteusRobot/gem.json

@@ -22,7 +22,10 @@
     "documentation_url": "https://www.o3de.org/docs/user-guide/interactivity/robotics/project-configuration/#ros-2-project-templates",
     "dependencies": [],
     "repo_uri": "https://raw.githubusercontent.com/o3de/o3de-extras/development",
-    "compatible_engines": [],
+     "compatible_engines": [
+        "o3de-sdk>=1.2.0",
+        "o3de>=1.2.0"
+    ],
     "engine_api_dependencies": [],
     "restricted": "ProteusRobot",
     "download_source_uri": "https://github.com/o3de/o3de-extras/releases/download/2.0/proteusrobot-1.0.0-gem.zip"

Gems/ROS2/Code/CMakeLists.txt

@@ -37,7 +37,7 @@ endif()
 
 # Add a custom target to always check during build if sourced and cached distributions match.
 add_custom_target(
-    ROS2DistributionCheck ALL 
+    ROS2DistributionCheck ALL
     COMMENT "Checking if sourced ROS 2 distribution matches with the configuration"
     COMMAND ${CMAKE_COMMAND} -DROS_DISTRO=${ROS_DISTRO} -P ${CMAKE_CURRENT_SOURCE_DIR}/checkROS2Distribution.cmake
 )
@@ -64,8 +64,10 @@ ly_add_target(
             Gem::Atom_RPI.Public
             Gem::Atom_Feature_Common.Static
             Gem::Atom_Component_DebugCamera.Static
+            Gem::AtomLyIntegration_CommonFeatures.Static
             Gem::StartingPointInput
             Gem::PhysX.Static
+            Gem::LmbrCentral.API
 )
 
 target_depends_on_ros2_packages(${gem_name}.Static rclcpp builtin_interfaces std_msgs sensor_msgs nav_msgs tf2_ros ackermann_msgs gazebo_msgs)
@@ -133,11 +135,11 @@ if(PAL_TRAIT_BUILD_HOST_TOOLS)
                 Gem::LmbrCentral.API
                 Gem::PhysX.Editor.Static
                 Gem::${gem_name}.Static
+            PRIVATE
+                AZ::AssetBuilderSDK
+                3rdParty::sdformat
     )
 
-    find_package(urdfdom)
-    target_link_libraries(${gem_name}.Editor.Static PUBLIC urdfdom::urdfdom_model)
-
     ly_add_target(
         NAME ${gem_name}.Editor GEM_MODULE
         NAMESPACE Gem
@@ -160,8 +162,8 @@ if(PAL_TRAIT_BUILD_HOST_TOOLS)
     )
 
     # By default, we will specify that the above target ROS2 would be used by
-    # Tool and Builder type targets when this gem is enabled.  If you don't want it
-    # active in Tools or Builders by default, delete one of both of the following lines:
+    # Tool and Builder type targets when this gem is enabled. If you don't want it
+    # active in Tools or Builders by default, delete one or both of the following lines:
     ly_create_alias(NAME ${gem_name}.Tools    NAMESPACE Gem TARGETS Gem::${gem_name}.Editor)
     ly_create_alias(NAME ${gem_name}.Builders NAMESPACE Gem TARGETS Gem::${gem_name}.Editor)
 endif()
@@ -213,6 +215,7 @@ if(PAL_TRAIT_BUILD_TESTS_SUPPORTED)
                     PRIVATE
                         AZ::AzTest
                         Gem::${gem_name}.Editor
+                        3rdParty::sdformat
             )
 
             # Add ROS2.Editor.Tests to googletest
@@ -222,3 +225,5 @@ if(PAL_TRAIT_BUILD_TESTS_SUPPORTED)
         endif()
     endif()
 endif()
+
+add_subdirectory(PythonTests)

Gems/ROS2/Code/FindROS2.cmake

@@ -5,9 +5,10 @@
 
 # Note that this does not find any ros2 package in particular, but determines whether a distro is sourced properly
 # Can be extended to handle supported / unsupported distros
+message(STATUS "Ros Distro is \"$ENV{ROS_DISTRO}\"")
 if (NOT DEFINED ENV{ROS_DISTRO} OR NOT DEFINED ENV{AMENT_PREFIX_PATH})
     message(WARNING, "To build ROS2 Gem a ROS distribution needs to be sourced, but none detected")
     set(ROS2_FOUND FALSE)
     return()
 endif()
-set(ROS2_FOUND TRUE)
+set(ROS2_FOUND TRUE)

Gems/ROS2/Code/Include/ROS2/FactorySimulation/ConveyorBeltRequestBus.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/Component/EntityId.h>
+#include <AzCore/EBus/EBus.h>
+
+namespace ROS2
+{
+    //! Interface class that allows to add post-processing to the pipeline
+    class ConveyorBeltRequests : public AZ::EBusTraits
+    {
+    public:
+        using BusIdType = AZ::EntityId;
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
+        static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Single;
+
+        //! Start a particular belt.
+        virtual void StartBelt() = 0;
+
+        //! Stop a particular belt.
+        virtual void StopBelt() = 0;
+
+        //! Query whether a particular conveyor belt is stopped.
+        virtual bool IsBeltStopped() = 0;
+
+    protected:
+        ~ConveyorBeltRequests() = default;
+    };
+
+    using ConveyorBeltRequestBus = AZ::EBus<ConveyorBeltRequests>;
+} // namespace ROS2

Gems/ROS2/Code/Include/ROS2/Frame/NamespaceConfiguration.h

@@ -41,6 +41,11 @@ namespace ROS2
         void PopulateNamespace(bool isRoot, const AZStd::string& entityName);
         AZStd::string GetNamespace(const AZStd::string& parentNamespace) const;
 
+        //! Update namespace and strategy.
+        //! @param ns Desired namespace.
+        //! @param strategy Namespace strategy.
+        void SetNamespace(const AZStd::string& ns, NamespaceStrategy strategy);
+
     private:
         AZStd::string m_namespace;
         NamespaceStrategy m_namespaceStrategy = NamespaceStrategy::Default;

Gems/ROS2/Code/Include/ROS2/Frame/ROS2FrameComponent.h

@@ -74,6 +74,11 @@ namespace ROS2
         //! @return The name of the global frame with namespace attached. It is typically "odom", "map", "world".
         AZStd::string GetGlobalFrameName() const;
 
+        //! Updates the namespace and namespace strategy of the underlying namespace configuration
+        //! @param ns Namespace to set.
+        //! @param strategy Namespace strategy to use.
+        void UpdateNamespaceConfiguration(const AZStd::string& ns, NamespaceConfiguration::NamespaceStrategy strategy);
+
     private:
         //////////////////////////////////////////////////////////////////////////
         // AZ::TickBus::Handler overrides

Gems/ROS2/Code/Include/ROS2/Gripper/GripperRequestBus.h

@@ -0,0 +1,56 @@
+/*
+ * 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/Component/EntityId.h>
+#include <AzCore/EBus/EBus.h>
+#include <AzCore/Interface/Interface.h>
+
+namespace ROS2
+{
+    //! The interface allows to control gripper components through GripperCommand actions.
+    //! It is a bus that allows communication between ROS2 GripperCommand Action server with the particular implementation of the gripper.
+    //! It encapsulates GripperCommand commands and getters that allow to build the feedback and result messages.
+    //! @see <a href="https://github.com/ros-controls/control_msgs/blob/humble/control_msgs/action/GripperCommand.action">GripperCommand</a>
+    class GripperRequests : public AZ::EBusTraits
+    {
+    public:
+        using BusIdType = AZ::EntityId;
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
+        static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Single;
+
+        //! Send new command to the gripper.
+        //! @param position position of the gripper (as a gap size in meters) to be set, for vacuum/electromagnetic gripper it is 0 or 1.
+        //! @param maxEffort maximum effort of the gripper to be set in Newtons.
+        //! @return Nothing on success, error message if failed.
+        virtual AZ::Outcome<void, AZStd::string> GripperCommand(float position, float maxEffort) = 0;
+
+        //! Cancel the current command to the gripper.
+        //! @return Nothing on success, error message if failed.
+        virtual AZ::Outcome<void, AZStd::string> CancelGripperCommand() = 0;
+
+        //! Get the current position of the gripper.
+        //! @return Position of the gripper.
+        virtual float GetGripperPosition() const = 0;
+
+        //! Get the current effort of the gripper.
+        //! @return Position (gap size in meters) of the gripper.
+        virtual float GetGripperEffort() const = 0;
+
+        //! Get if the gripper is not moving (stalled).
+        virtual bool IsGripperNotMoving() const = 0;
+
+        //! Get if the gripper reached the set with GripperCommand position.
+        virtual bool HasGripperReachedGoal() const = 0;
+
+        //! Get if the gripper command has been cancelled.
+        virtual bool HasGripperCommandBeenCancelled() const = 0;
+    };
+
+    using GripperRequestBus = AZ::EBus<GripperRequests>;
+} // namespace ROS2

Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h

@@ -11,6 +11,7 @@
 #include <AzCore/EBus/EBus.h>
 #include <AzCore/Math/Transform.h>
 #include <AzCore/Math/Vector3.h>
+#include <ROS2/Communication/QoS.h>
 
 namespace ROS2
 {
@@ -151,10 +152,9 @@ namespace ROS2
             AZ_Assert(false, "This Lidar Implementation does not support noise!");
         }
 
-        //! Configures Layer ignoring parameters
-        //! @param ignoreLayer Should a specified collision layer be ignored?
-        //! @param layerIndex Index of collision layer to be ignored.
-        virtual void ConfigureLayerIgnoring([[maybe_unused]] bool ignoreLayer, [[maybe_unused]] AZ::u32 layerIndex)
+        //! Configures which collision layers should be ignored.
+        //! @param layerIndices Indices of collision layers to be ignored.
+        virtual void ConfigureIgnoredCollisionLayers([[maybe_unused]] const AZStd::unordered_set<AZ::u32>& layerIndices)
         {
             AZ_Assert(false, "This Lidar Implementation does not support collision layer configurations!");
         }
@@ -173,6 +173,40 @@ namespace ROS2
             AZ_Assert(false, "This Lidar Implementation does not support Max range point addition configuration!");
         }
 
+        //! Enables and configures raycaster-side Point Cloud Publisher.
+        //! If not called, no publishing (raycaster-side) is performed. For some implementations it might be beneficial
+        //! to publish internally (e.g. for the RGL gem, published points can be transformed from global to sensor
+        //! coordinates on the GPU and published without unnecessary data copying or CPU manipulation) This API enables
+        //! raycaster implementations that also handle publishing and provides them with necessary publisher configuration.
+        //! @param topicName Name of the ROS 2 topic the pointcloud is published on.
+        //! @param frameId Id of the ROS 2 frame of the sensor.
+        //! @param qoSPolicy QoS policy of published pointcloud messages.
+        virtual void ConfigurePointCloudPublisher(
+            [[maybe_unused]] const AZStd::string& topicName,
+            [[maybe_unused]] const AZStd::string& frameId,
+            [[maybe_unused]] const QoS& qoSPolicy)
+        {
+            AZ_Assert(false, "This Lidar Implementation does not support PointCloud publishing!");
+        }
+
+        //! Updates the timestamp of the messages published by the raycaster.
+        //! @param timestampNanoseconds timestamp in nanoseconds
+        //! (Time.msg: sec = timestampNanoseconds / 10^9; nanosec = timestampNanoseconds mod 10^9).
+        virtual void UpdatePublisherTimestamp([[maybe_unused]] AZ::u64 timestampNanoseconds)
+        {
+            AZ_Assert(false, "This Lidar Implementation does not support PointCloud publishing!");
+        }
+
+        //! Can the raycaster handle publishing?
+        //! This function should be called after the raycaster has been configured.
+        //! The raycaster may not be able to handle point-cloud publishing in certain configurations (e.g. when the maxPointAddition
+        //! is selected) in which case publishing must be handled somewhere else (e.g. by the ROS2LidarComponent).
+        virtual bool CanHandlePublishing()
+        {
+            AZ_Assert(false, "This Lidar Implementation does not support PointCloud publishing!");
+            return false;
+        }
+
     protected:
         ~LidarRaycasterRequests() = default;
 

Gems/ROS2/Code/Include/ROS2/Lidar/LidarRegistrarBus.h

@@ -16,12 +16,13 @@ namespace ROS2
     //! Enum bitwise flags used to describe LidarSystem's feature support.
     enum LidarSystemFeatures : uint16_t
     {
-        None                = 0b0000000000000000,
-        Noise               = 0b0000000000000001,
-        CollisionLayers     = 0b0000000000000010,
-        EntityExclusion     = 0b0000000000000100,
-        MaxRangePoints      = 0b0000000000001000,
-        All                 = 0b1111111111111111
+        None                    = 0,
+        Noise                   = 1,
+        CollisionLayers         = 1 << 1,
+        EntityExclusion         = 1 << 2,
+        MaxRangePoints          = 1 << 3,
+        PointcloudPublishing    = 1 << 4,
+        All                     = 0b1111111111111111,
     };
 
     //! Structure used to hold LidarSystem's metadata.

Gems/ROS2/Code/Include/ROS2/Lidar/LidarSystemBus.h

@@ -24,6 +24,10 @@ namespace ROS2
         //! @return A unique Id of the newly created Lidar.
         virtual LidarId CreateLidar(AZ::EntityId lidarEntityId) = 0;
 
+        //! Destroys a no longer used Lidar.
+        //! @param lidarId Id of the lidar to be destroyed.
+        virtual void DestroyLidar(LidarId lidarId) = 0;
+
     protected:
         ~LidarSystemRequests() = default;
     };

Gems/ROS2/Code/Include/ROS2/Manipulation/JointInfo.h

@@ -17,6 +17,8 @@
 namespace ROS2
 {
     using JointPosition = float;
+    using JointVelocity = float;
+    using JointEffort = float;
     struct JointInfo
     {
         AZ_TYPE_INFO(JointInfo, "{2E33E4D0-78DD-436D-B3AB-F752E744F421}");

Gems/ROS2/Code/Include/ROS2/Manipulation/JointsManipulationRequests.h

@@ -24,6 +24,8 @@ namespace ROS2
         static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
 
         using JointsPositionsMap = AZStd::unordered_map<AZStd::string, JointPosition>;
+        using JointsVelocitiesMap = AZStd::unordered_map<AZStd::string, JointVelocity>;
+        using JointsEffortsMap = AZStd::unordered_map<AZStd::string, JointEffort>;
 
         //! Get all entity tree joints, including joint or articulation component hierarchy.
         //! @return An unordered map of joint names to joint info structure.
@@ -37,10 +39,32 @@ namespace ROS2
         //! If it does not exist or some other error happened, error message is returned.
         virtual AZ::Outcome<JointPosition, AZStd::string> GetJointPosition(const AZStd::string& jointName) = 0;
 
+        //! Get velocity of a joint by name.
+        //! Works with hinge joints and articulation links.
+        //! @param jointName name of the joint. Use names acquired from GetJoints() query.
+        //! @return outcome with velocity if joint exists.
+        //! If it does not exist or some other error happened, error message is returned.
+        virtual AZ::Outcome<JointVelocity, AZStd::string> GetJointVelocity(const AZStd::string& jointName) = 0;
+
         //! Return positions of all single DOF joints.
         //! @return a vector of all joints relative positions in degree of motion range or error message.
         virtual JointsPositionsMap GetAllJointsPositions() = 0;
 
+        //! Return velocities of all single DOF joints.
+        //! @return a vector of all joints velocities or error message.
+        virtual JointsVelocitiesMap GetAllJointsVelocities() = 0;
+
+        //! Get effort of a force-driven articulation link by name.
+        //! If the joint is not an articulation link, or it's acceleration-driven, returns 0.
+        //! @param jointName name of the joint. Use names acquired from GetJoints() query.
+        //! @return outcome with effort if joint exists.
+        //! If it does not exist or some other error happened, error message is returned.
+        virtual AZ::Outcome<JointEffort, AZStd::string> GetJointEffort(const AZStd::string& jointName) = 0;
+
+        //! Return efforts of all single DOF joints.
+        //! @return a vector of all joints efforts or error message.
+        virtual JointsEffortsMap GetAllJointsEfforts() = 0;
+
         //! Move specified joints into positions.
         //! @param new positions for each named joint. Use names queried through GetJoints().
         //! @return nothing on success, error message on failure.
@@ -54,6 +78,13 @@ namespace ROS2
         //! @note the movement is realized by a specific controller and not instant. The joints will then keep this position.
         virtual AZ::Outcome<void, AZStd::string> MoveJointToPosition(const AZStd::string& jointName, JointPosition position) = 0;
 
+        //! Set max effort of an articulation link by name.
+        //! If the joint is not an articulation link, doesn't do anything
+        //! @param jointName name of the joint. Use names acquired from GetJoints() query.
+        //! @return outcome with effort if joint exists.
+        //! If it does not exist or some other error happened, error message is returned.
+        virtual AZ::Outcome<void, AZStd::string> SetMaxJointEffort(const AZStd::string& jointName, JointEffort maxEffort) = 0;
+
         //! Stop the joints movement in progress. It will keep the position in which it stopped.
         virtual void Stop() = 0;
     };

Gems/ROS2/Code/Include/ROS2/Manipulation/JointsTrajectoryRequests.h

@@ -24,6 +24,7 @@ namespace ROS2
 
         using TrajectoryGoal = control_msgs::action::FollowJointTrajectory::Goal;
         using TrajectoryGoalPtr = std::shared_ptr<const TrajectoryGoal>;
+        using TrajectoryResult = control_msgs::action::FollowJointTrajectory::Result;
         using TrajectoryResultPtr = std::shared_ptr<control_msgs::action::FollowJointTrajectory::Result>;
 
         //! Status of trajectory action.
@@ -39,12 +40,12 @@ namespace ROS2
         //! @param trajectoryGoal Specified trajectory including points with timing and tolerances.
         //! @return Nothing on success, error message if failed.
         //! @note The call will return an error if the goal trajectory mismatches joints system.
-        virtual AZ::Outcome<void, AZStd::string> StartTrajectoryGoal(TrajectoryGoalPtr trajectoryGoal) = 0;
+        virtual AZ::Outcome<void, TrajectoryResult> StartTrajectoryGoal(TrajectoryGoalPtr trajectoryGoal) = 0;
 
         //! Cancel current trajectory goal.
         //! @param result Result of trajectory goal with explanation on why it was cancelled.
         //! @return nothing on success, error if the goal could not be cancelled.
-        virtual AZ::Outcome<void, AZStd::string> CancelTrajectoryGoal(TrajectoryResultPtr result) = 0;
+        virtual AZ::Outcome<void, AZStd::string> CancelTrajectoryGoal() = 0;
 
         //! Retrieve current trajectory goal status.
         //! @return Status of trajectory goal.

Gems/ROS2/Code/Include/ROS2/ProximitySensor/ProximitySensorNotificationBus.h

@@ -0,0 +1,34 @@
+/*
+ * 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/Component/EntityId.h>
+#include <AzCore/EBus/EBus.h>
+
+namespace ROS2
+{
+    //! Interface class that allows to add post-processing to the pipeline
+    class ProximitySensorNotifications : public AZ::EBusTraits
+    {
+    public:
+        using BusIdType = AZ::EntityId;
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
+        static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Single;
+
+        //! Notify that a particular sensor is detecting an object (notification published for each frequency tick).
+        virtual void OnObjectInRange() = 0;
+
+        //! Notify that a particular sensor is not detecting an object (notification published for each frequency tick).
+        virtual void OnObjectOutOfRange() = 0;
+
+    protected:
+        ~ProximitySensorNotifications() = default;
+    };
+
+    using ProximitySensorNotificationBus = AZ::EBus<ProximitySensorNotifications>;
+} // namespace ROS2

Gems/ROS2/Code/Include/ROS2/ProximitySensor/ProximitySensorNotificationBusHandler.h

@@ -0,0 +1,41 @@
+/*
+ * 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/BehaviorContext.h>
+#include <AzCore/RTTI/BehaviorInterfaceProxy.h>
+
+#include <ROS2/ProximitySensor/ProximitySensorNotificationBus.h>
+
+namespace ROS2
+{
+    class ProximitySensorNotificationBusHandler
+        : public ProximitySensorNotificationBus::Handler
+        , public AZ::BehaviorEBusHandler
+    {
+    public:
+        AZ_EBUS_BEHAVIOR_BINDER(
+            ProximitySensorNotificationBusHandler,
+            "{cc9c2e5a-318d-4212-abeb-95bd0575452d}",
+            AZ::SystemAllocator,
+            OnObjectInRange,
+            OnObjectOutOfRange);
+
+        // Sent when the object is detected.
+        void OnObjectInRange() override
+        {
+            Call(FN_OnObjectInRange);
+        }
+
+        // Sent when there are no objects detected.
+        void OnObjectOutOfRange() override
+        {
+            Call(FN_OnObjectOutOfRange);
+        }
+    };
+} // namespace ROS2

Gems/ROS2/Code/Include/ROS2/RobotImporter/SDFormatSensorImporterHook.h

@@ -0,0 +1,59 @@
+/*
+ * 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/Component/Entity.h>
+#include <AzCore/Outcome/Outcome.h>
+#include <AzCore/std/containers/unordered_set.h>
+#include <AzCore/std/function/function_template.h>
+#include <AzCore/std/string/string.h>
+
+namespace sdf
+{
+    inline namespace v13
+    {
+        enum class SensorType;
+        class Sensor;
+    } // namespace v13
+} // namespace sdf
+
+namespace ROS2::SDFormat
+{
+    //! Hook used in ROS2 Gem Robot Importer to create ROS2 sensor components based on SDFormat model description.
+    //! It implements the parameters mapping between the SDFormat sensor and the particular O3DE component.
+    //! It should be registered as a serialization attribute in the component's reflection to make the it visible in the SDFormat model
+    //! parser. See the sensor element at http://sdformat.org/spec?ver=1.10&elem=sensor for more details on SDFormat.
+    struct SensorImporterHook
+    {
+        AZ_TYPE_INFO(SensorImporterHook, "{23f189e3-8da4-4498-9b89-1ef6c900940a}");
+
+        //! Types of sensors in SDFormat description that can be mapped to the particular O3DE component.
+        //! Multiple SDFormat sensors can map to one O3DE component.
+        AZStd::unordered_set<sdf::SensorType> m_sensorTypes;
+
+        //! Names of the sensor's parameters in SDFormat description that are supported by the particular O3DE component.
+        AZStd::unordered_set<AZStd::string> m_supportedSensorParams;
+
+        //! Names of plugins associated with the sensor in SDFormat description that are supported by the particular O3DE component.
+        //! Multiple SDFormat plugins can map to one O3DE component.
+        AZStd::unordered_set<AZStd::string> m_pluginNames;
+
+        //! Names of the plugin's parameters associated with the sensor in SDFormat description that are supported
+        //! by the particular O3DE component.
+        AZStd::unordered_set<AZStd::string> m_supportedPluginParams;
+
+        //! Registered function that is invoked when a sensor of the specified type is processed by the SDFormat Importer.
+        //! @param AZ::Entity& a reference to the Entity in which one or more O3DE component is created by the importer
+        //! @param sdf::Sensor& a reference to the sensor data in SDFormat, which is used to configure O3DE component
+        using ErrorString = AZStd::string;
+        using ConvertSensorOutcome = AZ::Outcome<void, AZStd::string>;
+        using ConvertSDFSensorCallback = AZStd::function<ConvertSensorOutcome(AZ::Entity&, const sdf::Sensor&)>;
+        ConvertSDFSensorCallback m_sdfSensorToComponentCallback;
+    };
+} // namespace ROS2::SDFormat

Gems/ROS2/Code/Include/ROS2/Sensor/ROS2SensorComponent.h

@@ -18,6 +18,7 @@ namespace ROS2
     //! Captures common behavior of ROS2 sensor Components.
     //! Sensors acquire data from the simulation engine and publish it to ROS2 ecosystem.
     //! Derive this Component to implement a new ROS2 sensor. Each sensor Component requires ROS2FrameComponent.
+    //! For high frequency sensors also derive PhysicsCallbackHandler.
     class ROS2SensorComponent
         : public AZ::Component
         , public AZ::TickBus::Handler
@@ -59,10 +60,10 @@ namespace ROS2
         virtual void FrequencyTick(){};
 
     private:
-        //! Visualise sensor operation.
+        //! Visualize sensor operation.
         //! For example, draw points or rays for a lidar, viewport for a camera, etc.
-        //! Visualisation can be turned on or off in SensorConfiguration.
-        virtual void Visualise(){};
+        //! Visualization can be turned on or off in SensorConfiguration.
+        virtual void Visualize(){};
 
         //! The number of ticks that are expected to pass to trigger next measurement.
         AZ::s32 m_tickCountDown{ 0 };

Gems/ROS2/Code/Include/ROS2/Sensor/SensorConfiguration.h

@@ -17,7 +17,7 @@ namespace ROS2
 {
     //! General configuration for sensors.
     //! All sensors can be set to a certain frequency, have their data published or not,
-    //! and visualised or not.
+    //! and visualized or not.
     struct SensorConfiguration
     {
     public:
@@ -34,7 +34,7 @@ namespace ROS2
         float m_frequency = 10;
 
         bool m_publishingEnabled = true; //!< Determines whether the sensor is publishing (sending data to ROS 2 ecosystem).
-        bool m_visualise = true; //!< Determines whether the sensor is visualised in O3DE (for example, point cloud is drawn for LIDAR).
+        bool m_visualize = true; //!< Determines whether the sensor is visualized in O3DE (for example, point cloud is drawn for LIDAR).
     private:
         // Frequency limit is once per day.
         static constexpr float m_minFrequency = AZStd::numeric_limits<float>::epsilon();

Gems/ROS2/Code/Include/ROS2/Spawner/SpawnerBus.h

@@ -11,6 +11,7 @@
 #include <AzCore/EBus/EBus.h>
 #include <AzCore/Math/Transform.h>
 #include <AzCore/RTTI/BehaviorContext.h>
+#include <ROS2/Spawner/SpawnerInfo.h>
 
 namespace ROS2
 {
@@ -28,8 +29,9 @@ namespace ROS2
         //! @return default spawn point coordinates set by user in Editor (by default: translation: {0, 0, 0}, rotation: {0, 0, 0, 1},
         //! scale: 1.0)
         virtual const AZ::Transform& GetDefaultSpawnPose() const = 0;
+
+        virtual AZStd::unordered_map<AZStd::string, SpawnPointInfo> GetAllSpawnPointInfos() const = 0;
     };
 
     using SpawnerRequestsBus = AZ::EBus<SpawnerRequests>;
-    using SpawnerInterface = AZ::Interface<SpawnerRequests>;
 } // namespace ROS2

Gems/ROS2/Code/Include/ROS2/Spawner/SpawnerInfo.h

@@ -0,0 +1,26 @@
+/*
+ * 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/Memory/Memory.h>
+#include <AzCore/Memory/Memory_fwd.h>
+#include <AzCore/Memory/SystemAllocator.h>
+#include <AzCore/std/containers/unordered_map.h>
+#include <AzCore/std/string/string.h>
+
+namespace ROS2
+{
+    struct SpawnPointInfo
+    {
+        AZStd::string info;
+        AZ::Transform pose;
+    };
+
+    using SpawnPointInfoMap = AZStd::unordered_map<AZStd::string, SpawnPointInfo>;
+} // namespace ROS2

Gems/ROS2/Code/Source/Utilities/PhysicsCallbackHandler.h → Gems/ROS2/Code/Include/ROS2/Utilities/PhysicsCallbackHandler.h

@@ -13,31 +13,26 @@
 
 namespace ROS2::Utils
 {
-    //! Helper class that register OnSceneSimulationFinishHandler and retrieve handle to Simulated Body from the Default Scene.
+    //! A class that registers physics tick callbacks from the Default Scene.
     class PhysicsCallbackHandler
     {
     protected:
-        //! Install to default physics scene callbacks, when the scene is created, the @member m_bodyHandle is populated with the handle to
-        //! simulated body.
-        //! @param m_entityId Entity id to get m_bodyHandle to.
-        void InstallPhysicalCallback(AZ::EntityId m_entityId);
+        //! Install default physics scene callbacks
+        void InstallPhysicalCallback();
 
         //! Removes all attached callbacks
         void RemovePhysicalCallback();
 
         //! Called multiple times per frame after every inner loop of physics engine.
-        //! It virtual version of  callback AzPhysics::SceneEvents::OnSceneSimulationFinishHandler.
+        //! It's a virtual version of callback AzPhysics::SceneEvents::OnSceneSimulationFinishHandler.
         //! @param sceneHandle - scene handle, only handle to Default Scene is expected
         //! @param deltaTime - update of simulated time in seconds.
-        virtual void OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime) {};
+        virtual void OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime){};
 
-        //! Callback called on begging of the first physical simulation.
-        //! inner loop of physics engine.
+        //! Callback called on beginning of the first physical simulation inner loop of physics engine.
         //! @param sceneHandle - scene handle, only handle to Default Scene is expected
-        virtual void OnPhysicsInitialization(AzPhysics::SceneHandle sceneHandle) {};
+        virtual void OnPhysicsInitialization(AzPhysics::SceneHandle sceneHandle){};
 
-        //! Handler to simulated physical body
-        AzPhysics::SimulatedBodyHandle m_bodyHandle = AzPhysics::InvalidSimulatedBodyHandle;
     private:
         AzPhysics::SceneEvents::OnSceneSimulationFinishHandler m_onSceneSimulationEvent;
         AzPhysics::SceneEvents::OnSceneSimulationStartHandler m_onSceneSimulationStart;

Gems/ROS2/Code/Platform/Linux/PAL_linux.cmake

@@ -7,3 +7,13 @@
 #
 
 set(PAL_TRAIT_BUILD_ROS2_GEM_SUPPORTED TRUE)
+
+if(${CMAKE_SYSTEM_PROCESSOR} STREQUAL "x86_64")
+    ly_associate_package(PACKAGE_NAME sdformat-13.5.0-rev1-linux
+        TARGETS sdformat
+        PACKAGE_HASH 71a86e255cfc7a176f6087e069fb26c9837c98e277becf87f3a4b0e25fe90bd3)
+elseif(${CMAKE_SYSTEM_PROCESSOR} STREQUAL "aarch64")
+    ly_associate_package(PACKAGE_NAME sdformat-13.5.0-rev1-linux-aarch64
+        TARGETS sdformat
+        PACKAGE_HASH c8ca2ffad604162cbc8ed852c8ea8695851c4c69c5f39f2a9d8e423511ce78f9)
+endif()

Gems/ROS2/Code/PythonTests/CMakeLists.txt

@@ -0,0 +1,22 @@
+# 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
+
+if (PAL_TRAIT_BUILD_TESTS_SUPPORTED)
+    if (PAL_TRAIT_TEST_PYTEST_SUPPORTED)
+        # PAL_TRAIT_BUILD macros are used by platform detection.
+        ly_add_pytest(
+                NAME Gem::${gem_name}.PythonTests
+                TEST_SUITE smoke
+                TEST_SERIAL
+                PATH ${CMAKE_CURRENT_LIST_DIR}/SmokeTests_Periodic.py
+                RUNTIME_DEPENDENCIES
+                    Legacy::Editor
+                    AZ::AssetProcessor
+                    ${gem_name}.Editor
+                COMPONENT
+                    SmokeTests
+        )
+    endif ()
+endif ()

Gems/ROS2/Code/PythonTests/SmokeTests_Periodic.py

@@ -0,0 +1,19 @@
+#
+# 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
+#
+
+import pytest
+from ly_test_tools.o3de.editor_test import EditorTestSuite, EditorSingleTest
+
+
[email protected]_periodic  # Marks the test suite as being part of a Periodic test suite
[email protected]("launcher_platform", ['linux_editor'])  # This test works on Linux editor
[email protected]("project", ["AutomatedTesting"])  # Use the AutomatedTesting project
+class TestAutomation(EditorTestSuite):
+    # Declaring a class that extends from EditorSingleTest declares a single test.
+    class SmokeTests_EnterGameModeWorks(EditorSingleTest):
+        # This runs a Single Test in a single Editor. For further work check EditorSingleTest siblings
+        from .tests import SmokeTests_EnterGameModeWorks as test_module

Gems/ROS2/Code/PythonTests/__init__.py

@@ -0,0 +1,6 @@
+"""
+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
+"""

Gems/ROS2/Code/PythonTests/tests/SmokeTests_EnterGameModeWorks.py

@@ -0,0 +1,110 @@
+#
+# 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
+#
+
+# Test Case Title : Check that entering into game mode works
+
+# List of results that we want to check, this is not 100% necessary but its a good
+# practice to make it easier to debug tests.
+# Here we define a tuple of tests
+
+# Paths to ROS rclpy are added directly as pytest is not accessing PYTHONPATH environment variable
+import sys
+
+sys.path.append('/opt/ros/humble/lib/python3.10/site-packages')
+sys.path.append('/opt/ros/humble/local/lib/python3.10/dist-packages')
+
+
+class Tests:
+    enter_game_mode = ("Entered game mode", "Failed to enter game mode")
+    topics_published = ("Topics were published during game mode", "Failed to publish topics during game mode")
+    topics_not_published_outside_of_game_mode = (
+        "All simulation topics closed", "Failed to close publishers after simulation")
+
+
+def check_result(result, msg):
+    from editor_python_test_tools.utils import Report
+    if not result:
+        Report.result(msg, False)
+        raise Exception(msg + " : FAILED")
+
+
+def check_topics():
+    import rclpy
+    from rclpy.node import Node
+
+    def get_topic_list():
+        node_dummy = Node("_ros2cli_dummy_to_show_topic_list")
+        result = node_dummy.get_topic_names_and_types()
+        node_dummy.destroy_node()
+        return result
+
+    topics = []
+    rclpy.init()
+    topic_list = get_topic_list()
+
+    for info in topic_list:
+        topics.append(info[0])
+    rclpy.shutdown()
+    return topics
+
+
+def SmokeTest_EnterGameModeWorks():
+    # Description: This test checks that entering into game mode works by opening an empty level
+    # and entering into the game mode. The is in game mode state should be changed after doing it
+    # Next it checks if there are some additional ROS topics published during the game mode
+
+    # Import report and test helper utilities
+    from editor_python_test_tools.utils import Report
+    from editor_python_test_tools.utils import TestHelper as helper
+    # All exposed python bindings are in azlmbr
+    import azlmbr.legacy.general as general
+
+    # Required for automated tests
+    helper.init_idle()
+
+    # Open the level called "Warehouse".
+    # We use a warehouse level for a smoke test - it already has a robot prefab present
+    helper.open_level(level="Warehouse", directory='')
+
+    topics_before_game_mode = check_topics()
+
+    # Using the exposed Python API from editor in CryEditPy.py we can enter into game mode this way
+    general.enter_game_mode()
+
+    # The script drives the execution of the test, to return the flow back to the editor,
+    # we will tick it one time
+    general.idle_wait_frames(1)
+
+    # Now we can use the Report.result() to report the state of a result
+    # if the second argument is false, it will mark this test as failed, however it will keep going.
+    Report.result(Tests.enter_game_mode, general.is_in_game_mode())
+
+    topics_in_game_mode = check_topics()
+
+    Report.result(Tests.topics_published, len(topics_in_game_mode) > len(topics_before_game_mode))
+
+    # Instead of using Report.result(), you can also use:
+    # assert is_in_game_mode, "Didn't enter into game mode"
+    # However this would stop the test at this point and not report anything when it succeeds
+
+    # The test will end at this point, is good practice to exit game mode or reset any changed stated
+    # *DO NOT* close the editor, the editor will close automatically and report the error code
+    general.exit_game_mode()
+
+    # this line is needed to update the simulation state
+    general.idle_wait_frames(1)
+
+    topics_after_game_mode = check_topics()
+    Report.result(Tests.topics_not_published_outside_of_game_mode,
+                  len(topics_after_game_mode) == len(topics_before_game_mode))
+
+
+if __name__ == "__main__":
+    # This utility starts up the test and sets up the state for knowing what test is currently being run
+    from editor_python_test_tools.utils import Report
+
+    Report.start_test(SmokeTest_EnterGameModeWorks)

Gems/ROS2/Code/Source/Camera/CameraSensorConfiguration.cpp

@@ -17,12 +17,14 @@ namespace ROS2
         if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
         {
             serializeContext->Class<CameraSensorConfiguration>()
-                ->Version(1)
+                ->Version(2)
                 ->Field("VerticalFieldOfViewDeg", &CameraSensorConfiguration::m_verticalFieldOfViewDeg)
                 ->Field("Width", &CameraSensorConfiguration::m_width)
                 ->Field("Height", &CameraSensorConfiguration::m_height)
                 ->Field("Depth", &CameraSensorConfiguration::m_depthCamera)
-                ->Field("Color", &CameraSensorConfiguration::m_colorCamera);
+                ->Field("Color", &CameraSensorConfiguration::m_colorCamera)
+                ->Field("ClipNear", &CameraSensorConfiguration::m_nearClipDistance)
+                ->Field("ClipFar", &CameraSensorConfiguration::m_farClipDistance);
 
             if (AZ::EditContext* ec = serializeContext->GetEditContext())
             {
@@ -39,7 +41,17 @@ namespace ROS2
                     ->DataElement(AZ::Edit::UIHandlers::Default, &CameraSensorConfiguration::m_height, "Image height", "Image height")
                     ->Attribute(AZ::Edit::Attributes::Min, CameraSensorConfiguration::m_minHeight)
                     ->DataElement(AZ::Edit::UIHandlers::Default, &CameraSensorConfiguration::m_colorCamera, "Color Camera", "Color Camera")
-                    ->DataElement(AZ::Edit::UIHandlers::Default, &CameraSensorConfiguration::m_depthCamera, "Depth Camera", "Depth Camera");
+                    ->DataElement(AZ::Edit::UIHandlers::Default, &CameraSensorConfiguration::m_depthCamera, "Depth Camera", "Depth Camera")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &CameraSensorConfiguration::m_nearClipDistance,
+                        "Near clip distance",
+                        "Minimum distance to detect objects")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &CameraSensorConfiguration::m_farClipDistance,
+                        "Far clip distance",
+                        "Maximum distance to detect objects");
             }
         }
     }

Gems/ROS2/Code/Source/Camera/CameraSensorConfiguration.h

@@ -29,5 +29,7 @@ namespace ROS2
         int m_height = 480; //!< Camera image height in pixels.
         bool m_colorCamera = true; //!< Use color camera?
         bool m_depthCamera = true; //!< Use depth camera?
+        float m_nearClipDistance = 0.1f; //!< Near clip distance of the camera.
+        float m_farClipDistance = 100.0f; //!< Far clip distance of the camera.
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/Camera/CameraSensorDescription.cpp

@@ -22,7 +22,11 @@ namespace ROS2
         , m_cameraName(cameraName)
         , m_cameraNamespace(effectiveNamespace)
         , m_viewToClipMatrix(CameraUtils::MakeClipMatrix(
-              m_cameraConfiguration.m_width, m_cameraConfiguration.m_height, m_cameraConfiguration.m_verticalFieldOfViewDeg))
+              m_cameraConfiguration.m_width,
+              m_cameraConfiguration.m_height,
+              m_cameraConfiguration.m_verticalFieldOfViewDeg,
+              m_cameraConfiguration.m_nearClipDistance,
+              m_cameraConfiguration.m_farClipDistance))
         , m_cameraIntrinsics(CameraUtils::MakeCameraIntrinsics(
               m_cameraConfiguration.m_width, m_cameraConfiguration.m_height, m_cameraConfiguration.m_verticalFieldOfViewDeg))
     {
@@ -37,6 +41,11 @@ namespace ROS2
         AZ_Assert(
             m_cameraConfiguration.m_width > 0 && m_cameraConfiguration.m_height > 0, "Camera resolution dimensions should be above zero");
         AZ_Assert(!m_cameraName.empty(), "Camera name cannot be empty");
+        AZ_Assert(m_cameraConfiguration.m_nearClipDistance > 0.0f, "Near clip distance should be greater than zero");
+        AZ_Assert(m_cameraConfiguration.m_farClipDistance > m_cameraConfiguration.m_nearClipDistance , "Far clip distance should be greater than the near plane distance");
+        AZ_Assert(
+            m_cameraConfiguration.m_farClipDistance > m_cameraConfiguration.m_nearClipDistance,
+            "Far clip distance should be greater than near clip distance");
     }
 
 } // namespace ROS2

Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.cpp

@@ -29,6 +29,8 @@ namespace ROS2
 
     void ROS2CameraSensorComponent::Reflect(AZ::ReflectContext* context)
     {
+        CameraSensorConfiguration::Reflect(context);
+
         auto* serialize = azrtti_cast<AZ::SerializeContext*>(context);
         if (serialize)
         {

Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp

@@ -28,10 +28,15 @@ namespace ROS2
             MakeTopicConfigurationPair("depth_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig));
     }
 
-    void ROS2CameraSensorEditorComponent::Reflect(AZ::ReflectContext* context)
+    ROS2CameraSensorEditorComponent::ROS2CameraSensorEditorComponent(
+        const SensorConfiguration& sensorConfiguration, const CameraSensorConfiguration& cameraConfiguration)
+        : m_sensorConfiguration(sensorConfiguration)
+        , m_cameraSensorConfiguration(cameraConfiguration)
     {
-        CameraSensorConfiguration::Reflect(context);
+    }
 
+    void ROS2CameraSensorEditorComponent::Reflect(AZ::ReflectContext* context)
+    {
         if (auto* serialize = azrtti_cast<AZ::SerializeContext*>(context))
         {
             serialize->Class<ROS2CameraSensorEditorComponent, AzToolsFramework::Components::EditorComponentBase>()
@@ -119,64 +124,60 @@ namespace ROS2
     void ROS2CameraSensorEditorComponent::DisplayEntityViewport(
         [[maybe_unused]] const AzFramework::ViewportInfo& viewportInfo, AzFramework::DebugDisplayRequests& debugDisplay)
     {
-        if (!m_sensorConfiguration.m_visualise)
+        if (!m_sensorConfiguration.m_visualize)
         {
             return;
         }
         const AZ::u32 stateBefore = debugDisplay.GetState();
         AZ::Transform transform = GetEntity()->GetTransform()->GetWorldTM();
 
+        const float distance = m_cameraSensorConfiguration.m_farClipDistance * 0.1f;
+        const float tangent = static_cast<float>(tan(0.5f * AZ::DegToRad(m_cameraSensorConfiguration.m_verticalFieldOfViewDeg)));
+
+        float height = distance * tangent;
+        float width = height * m_cameraSensorConfiguration.m_width / m_cameraSensorConfiguration.m_height;
+
+        AZ::Vector3 farPoints[4];
+        farPoints[0] = AZ::Vector3(width, height, distance);
+        farPoints[1] = AZ::Vector3(-width, height, distance);
+        farPoints[2] = AZ::Vector3(-width, -height, distance);
+        farPoints[3] = AZ::Vector3(width, -height, distance);
+
+        AZ::Vector3 nearPoints[4];
+        nearPoints[0] = farPoints[0].GetNormalizedSafe() * m_cameraSensorConfiguration.m_nearClipDistance;
+        nearPoints[1] = farPoints[1].GetNormalizedSafe() * m_cameraSensorConfiguration.m_nearClipDistance;
+        nearPoints[2] = farPoints[2].GetNormalizedSafe() * m_cameraSensorConfiguration.m_nearClipDistance;
+        nearPoints[3] = farPoints[3].GetNormalizedSafe() * m_cameraSensorConfiguration.m_nearClipDistance;
+
         // dimension of drawing
-        const float arrowRise = 1.1f;
-        const float arrowSize = 0.5f;
-        const float frustumScale = 0.1f;
-
-        transform.SetUniformScale(frustumScale);
-        debugDisplay.DepthTestOff();
-        const float vertical = 0.5f * AZStd::tan((AZ::DegToRad(m_cameraSensorConfiguration.m_verticalFieldOfViewDeg * 0.5f)));
-        const float horizontal = m_cameraSensorConfiguration.m_height * vertical / m_cameraSensorConfiguration.m_width;
-
-        // frustum drawing
-        const AZ::Vector3 p1(-vertical, -horizontal, 1.f);
-        const AZ::Vector3 p2(vertical, -horizontal, 1.f);
-        const AZ::Vector3 p3(vertical, horizontal, 1.f);
-        const AZ::Vector3 p4(-vertical, horizontal, 1.f);
-        const AZ::Vector3 p0(0, 0, 0);
-        const AZ::Vector3 py(0.1, 0, 0);
-
-        const auto pt1 = transform.TransformPoint(p1);
-        const auto pt2 = transform.TransformPoint(p2);
-        const auto pt3 = transform.TransformPoint(p3);
-        const auto pt4 = transform.TransformPoint(p4);
-        const auto pt0 = transform.TransformPoint(p0);
-        const auto ptz = transform.TransformPoint(AZ::Vector3::CreateAxisZ(0.2f));
-        const auto pty = transform.TransformPoint(AZ::Vector3::CreateAxisY(0.2f));
-        const auto ptx = transform.TransformPoint(AZ::Vector3::CreateAxisX(0.2f));
+        const float arrowRise = 0.11f;
+        const float arrowSize = 0.05f;
+
+        const AZ::Vector3 pt0(0, 0, 0);
+        const auto ptz = AZ::Vector3::CreateAxisZ(0.2f);
+        const auto pty = AZ::Vector3::CreateAxisY(0.2f);
+        const auto ptx = AZ::Vector3::CreateAxisX(0.2f);
+
+        debugDisplay.PushMatrix(transform);
 
         debugDisplay.SetColor(AZ::Color(0.f, 1.f, 1.f, 1.f));
-        debugDisplay.SetLineWidth(1);
-        debugDisplay.DrawLine(pt1, pt2);
-        debugDisplay.DrawLine(pt2, pt3);
-        debugDisplay.DrawLine(pt3, pt4);
-        debugDisplay.DrawLine(pt4, pt1);
-        debugDisplay.DrawLine(pt1, pt0);
-        debugDisplay.DrawLine(pt2, pt0);
-        debugDisplay.DrawLine(pt3, pt0);
-        debugDisplay.DrawLine(pt4, pt0);
+        debugDisplay.DrawLine(nearPoints[0], farPoints[0]);
+        debugDisplay.DrawLine(nearPoints[1], farPoints[1]);
+        debugDisplay.DrawLine(nearPoints[2], farPoints[2]);
+        debugDisplay.DrawLine(nearPoints[3], farPoints[3]);
+        debugDisplay.DrawPolyLine(nearPoints, AZ_ARRAY_SIZE(nearPoints));
+        debugDisplay.DrawPolyLine(farPoints, AZ_ARRAY_SIZE(farPoints));
 
         // up-arrow drawing
-        const AZ::Vector3 pa1(-arrowSize * vertical, -arrowRise * horizontal, 1.f);
-        const AZ::Vector3 pa2(arrowSize * vertical, -arrowRise * horizontal, 1.f);
-        const AZ::Vector3 pa3(0, (-arrowRise - arrowSize) * horizontal, 1.f);
-        const auto pat1 = transform.TransformPoint(pa1);
-        const auto pat2 = transform.TransformPoint(pa2);
-        const auto pat3 = transform.TransformPoint(pa3);
+        const AZ::Vector3 pa1(-arrowSize * height, -arrowRise * width, 1.f);
+        const AZ::Vector3 pa2(arrowSize * height, -arrowRise * width, 1.f);
+        const AZ::Vector3 pa3(0, (-arrowRise - arrowSize) * width, 1.f);
 
         debugDisplay.SetColor(AZ::Color(0.f, 0.6f, 1.f, 1.f));
         debugDisplay.SetLineWidth(1);
-        debugDisplay.DrawLine(pat1, pat2);
-        debugDisplay.DrawLine(pat2, pat3);
-        debugDisplay.DrawLine(pat3, pat1);
+        debugDisplay.DrawLine(pa1, pa2);
+        debugDisplay.DrawLine(pa2, pa3);
+        debugDisplay.DrawLine(pa3, pa1);
 
         // coordinate system drawing
         debugDisplay.SetColor(AZ::Color(1.f, 0.f, 0.f, 1.f));
@@ -191,6 +192,8 @@ namespace ROS2
         debugDisplay.SetLineWidth(2);
         debugDisplay.DrawLine(ptz, pt0);
 
+        debugDisplay.PopMatrix();
+
         debugDisplay.SetState(stateBefore);
     }
 

Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h

@@ -22,7 +22,7 @@ namespace ROS2
 {
     //! ROS2 Camera Editor sensor component class
     //! Allows turning an entity into a camera sensor in Editor
-    //! Component draws camera frustrum in the Editor
+    //! Component draws camera frustum in the Editor
     class ROS2CameraSensorEditorComponent
         : public AzToolsFramework::Components::EditorComponentBase
         , public CameraCalibrationRequestBus::Handler
@@ -30,6 +30,8 @@ namespace ROS2
     {
     public:
         ROS2CameraSensorEditorComponent();
+        ROS2CameraSensorEditorComponent(
+            const SensorConfiguration& sensorConfiguration, const CameraSensorConfiguration& cameraConfiguration);
         ~ROS2CameraSensorEditorComponent() override = default;
         AZ_EDITOR_COMPONENT(ROS2CameraSensorEditorComponent, "{3C2A86B2-AD58-4BF1-A5EF-71E0F94A2B42}");
         static void Reflect(AZ::ReflectContext* context);
@@ -55,7 +57,8 @@ namespace ROS2
 
         AZStd::pair<AZStd::string, TopicConfiguration> MakeTopicConfigurationPair(
             const AZStd::string& topic, const AZStd::string& messageType, const AZStd::string& configName) const;
+
         SensorConfiguration m_sensorConfiguration;
         CameraSensorConfiguration m_cameraSensorConfiguration;
     };
-} // namespace ROS2
+} // namespace ROS2

Gems/ROS2/Code/Source/Communication/TopicConfiguration.cpp

@@ -27,6 +27,7 @@ namespace ROS2
                 ec->Class<TopicConfiguration>("Publisher configuration", "")
                     ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
                     ->DataElement(AZ::Edit::UIHandlers::Default, &TopicConfiguration::m_type, "Type", "Type of topic messages")
+                    ->Attribute(AZ::Edit::Attributes::ReadOnly, true)
                     ->DataElement(AZ::Edit::UIHandlers::Default, &TopicConfiguration::m_topic, "Topic", "Topic with no namespace")
                     ->Attribute(AZ::Edit::Attributes::ChangeValidate, &ROS2Names::ValidateTopicField)
                     ->DataElement(AZ::Edit::UIHandlers::Default, &TopicConfiguration::m_qos, "QoS", "Quality of Service");

Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.cpp

@@ -0,0 +1,197 @@
+/*
+ * 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 "ROS2ContactSensorComponent.h"
+#include <AzCore/Component/ComponentApplicationBus.h>
+#include <AzCore/Debug/Trace.h>
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/EditContextConstants.inl>
+#include <AzCore/std/parallel/lock.h>
+#include <AzCore/std/parallel/mutex.h>
+#include <AzCore/std/utility/move.h>
+#include <AzFramework/Physics/Collision/CollisionEvents.h>
+#include <AzFramework/Physics/Common/PhysicsSimulatedBody.h>
+#include <AzFramework/Physics/Common/PhysicsSimulatedBodyEvents.h>
+#include <AzFramework/Physics/Common/PhysicsTypes.h>
+#include <AzFramework/Physics/PhysicsSystem.h>
+#include <ROS2/Frame/ROS2FrameComponent.h>
+#include <ROS2/ROS2Bus.h>
+#include <ROS2/ROS2GemUtilities.h>
+#include <ROS2/Utilities/ROS2Conversions.h>
+#include <ROS2/Utilities/ROS2Names.h>
+#include <geometry_msgs/msg/wrench.hpp>
+
+namespace ROS2
+{
+    namespace Internal
+    {
+        constexpr float ContactMaximumSeparation = 0.0001f;
+    }
+
+    ROS2ContactSensorComponent::ROS2ContactSensorComponent()
+    {
+        TopicConfiguration tc;
+        AZStd::string type = "gazebo_msgs::msg::ContactsState";
+        tc.m_type = type;
+        tc.m_topic = "contact_sensor";
+        m_sensorConfiguration.m_frequency = 15;
+        m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(AZStd::move(type), AZStd::move(tc)));
+    }
+
+    void ROS2ContactSensorComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<ROS2ContactSensorComponent, ROS2SensorComponent>()->Version(1);
+
+            if (AZ::EditContext* editContext = serialize->GetEditContext())
+            {
+                editContext->Class<ROS2ContactSensorComponent>("ROS2 Contact Sensor", "Contact detection controller")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"));
+            }
+        }
+    }
+
+    void ROS2ContactSensorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
+    {
+        required.push_back(AZ_CRC_CE("PhysicsColliderService"));
+        required.push_back(AZ_CRC_CE("ROS2Frame"));
+    }
+
+    void ROS2ContactSensorComponent::Activate()
+    {
+        m_entityId = GetEntityId();
+        AZ::Entity* entity = nullptr;
+        AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, m_entityId);
+        m_entityName = entity->GetName();
+
+        auto ros2Node = ROS2Interface::Get()->GetNode();
+        AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for Contact sensor");
+        const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations["gazebo_msgs::msg::ContactsState"];
+        const auto fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic);
+        m_contactsPublisher = ros2Node->create_publisher<gazebo_msgs::msg::ContactsState>(fullTopic.data(), publisherConfig.GetQoS());
+
+        m_onCollisionBeginHandler = AzPhysics::SimulatedBodyEvents::OnCollisionBegin::Handler(
+            [this]([[maybe_unused]] AzPhysics::SimulatedBodyHandle bodyHandle, const AzPhysics::CollisionEvent& event)
+            {
+                AddNewContact(event);
+            });
+
+        m_onCollisionPersistHandler = AzPhysics::SimulatedBodyEvents::OnCollisionPersist::Handler(
+            [this]([[maybe_unused]] AzPhysics::SimulatedBodyHandle bodyHandle, const AzPhysics::CollisionEvent& event)
+            {
+                AddNewContact(event);
+            });
+
+        m_onCollisionEndHandler = AzPhysics::SimulatedBodyEvents::OnCollisionEnd::Handler(
+            [this]([[maybe_unused]] AzPhysics::SimulatedBodyHandle bodyHandle, const AzPhysics::CollisionEvent& event)
+            {
+                AZStd::lock_guard<AZStd::mutex> lock(m_activeContactsMutex);
+                m_activeContacts.erase(event.m_body2->GetEntityId());
+            });
+
+        ROS2SensorComponent::Activate();
+    }
+
+    void ROS2ContactSensorComponent::Deactivate()
+    {
+        m_activeContacts.clear();
+        m_contactsPublisher.reset();
+        m_onCollisionBeginHandler.Disconnect();
+        m_onCollisionPersistHandler.Disconnect();
+        m_onCollisionEndHandler.Disconnect();
+        ROS2SensorComponent::Deactivate();
+    }
+
+    void ROS2ContactSensorComponent::FrequencyTick()
+    {
+        // Connects the collision handlers if not already connected
+        AzPhysics::SystemInterface* physicsSystem = AZ::Interface<AzPhysics::SystemInterface>::Get();
+        if (!physicsSystem)
+        {
+            return;
+        }
+
+        if (!m_onCollisionBeginHandler.IsConnected() || !m_onCollisionPersistHandler.IsConnected() ||
+            !m_onCollisionEndHandler.IsConnected())
+        {
+            AZStd::pair<AzPhysics::SceneHandle, AzPhysics::SimulatedBodyHandle> foundBody =
+                physicsSystem->FindAttachedBodyHandleFromEntityId(GetEntityId());
+            AZ_Warning("Contact Sensor", foundBody.first != AzPhysics::InvalidSceneHandle, "Invalid scene handle");
+            if (foundBody.first != AzPhysics::InvalidSceneHandle)
+            {
+                AzPhysics::SimulatedBodyEvents::RegisterOnCollisionBeginHandler(
+                    foundBody.first, foundBody.second, m_onCollisionBeginHandler);
+                AzPhysics::SimulatedBodyEvents::RegisterOnCollisionPersistHandler(
+                    foundBody.first, foundBody.second, m_onCollisionPersistHandler);
+                AzPhysics::SimulatedBodyEvents::RegisterOnCollisionEndHandler(foundBody.first, foundBody.second, m_onCollisionEndHandler);
+            }
+        }
+
+        // Publishes all contacts
+        gazebo_msgs::msg::ContactsState msg;
+        const auto* ros2Frame = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(GetEntity());
+        AZ_Assert(ros2Frame, "Invalid component pointer value");
+        msg.header.frame_id = ros2Frame->GetFrameID().data();
+        msg.header.stamp = ROS2Interface::Get()->GetROSTimestamp();
+
+        {
+            // If there are no active collisions, then there is nothing to send
+            AZStd::lock_guard<AZStd::mutex> lock(m_activeContactsMutex);
+            if (!m_activeContacts.empty())
+            {
+                for (auto& [id, contact] : m_activeContacts)
+                {
+                    msg.states.push_back(AZStd::move(contact));
+                }
+                m_contactsPublisher->publish(AZStd::move(msg));
+                m_activeContacts.clear();
+            }
+        }
+    }
+
+    void ROS2ContactSensorComponent::AddNewContact(const AzPhysics::CollisionEvent& event)
+    {
+        AZ::Entity* contactedEntity = nullptr;
+        AZ::ComponentApplicationBus::BroadcastResult(
+            contactedEntity, &AZ::ComponentApplicationRequests::FindEntity, event.m_body2->GetEntityId());
+        gazebo_msgs::msg::ContactState state;
+        AZ_Assert(contactedEntity, "Invalid entity pointer value");
+        state.collision1_name = ("ID: " + m_entityId.ToString() + " Name:" + m_entityName).c_str();
+        state.collision2_name = ("ID: " + event.m_body2->GetEntityId().ToString() + " Name:" + contactedEntity->GetName()).c_str();
+        geometry_msgs::msg::Wrench totalWrench;
+        for (auto& contact : event.m_contacts)
+        {
+            if (contact.m_separation < Internal::ContactMaximumSeparation)
+            {
+                state.contact_positions.emplace_back(ROS2Conversions::ToROS2Vector3(contact.m_position));
+                state.contact_normals.emplace_back(ROS2Conversions::ToROS2Vector3(contact.m_normal));
+
+                geometry_msgs::msg::Wrench contactWrench;
+                contactWrench.force = ROS2Conversions::ToROS2Vector3(contact.m_impulse);
+                state.wrenches.push_back(AZStd::move(contactWrench));
+
+                totalWrench.force.x += contact.m_impulse.GetX();
+                totalWrench.force.y += contact.m_impulse.GetY();
+                totalWrench.force.z += contact.m_impulse.GetZ();
+
+                state.depths.emplace_back(contact.m_separation);
+            }
+        }
+
+        state.total_wrench = AZStd::move(totalWrench);
+
+        if (!state.contact_positions.empty())
+        {
+            AZStd::lock_guard<AZStd::mutex> lock(m_activeContactsMutex);
+            m_activeContacts[event.m_body2->GetEntityId()] = AZStd::move(state);
+        }
+    }
+} // namespace ROS2

Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.h

@@ -0,0 +1,63 @@
+/*
+ * 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/Component/EntityId.h>
+#include <AzCore/RTTI/ReflectContext.h>
+#include <AzCore/std/containers/unordered_map.h>
+#include <AzCore/std/parallel/mutex.h>
+#include <AzCore/std/string/string.h>
+#include <AzFramework/Physics/Common/PhysicsSimulatedBodyEvents.h>
+#include <ROS2/Sensor/ROS2SensorComponent.h>
+#include <gazebo_msgs/msg/contact_state.hpp>
+#include <gazebo_msgs/msg/contacts_state.hpp>
+#include <rclcpp/publisher.hpp>
+
+namespace ROS2
+{
+    //! Contact sensor detects collisions between two objects.
+    //! It reports the location of the contact associated forces.
+    //! This component publishes a contact_sensor topic.
+    //! It doesn't measure torque.
+    class ROS2ContactSensorComponent : public ROS2SensorComponent
+    {
+    public:
+        AZ_COMPONENT(ROS2ContactSensorComponent, "{91272e66-c9f1-4aa2-a9d5-98eaa4ef4e9a}", ROS2SensorComponent);
+        ROS2ContactSensorComponent();
+        ~ROS2ContactSensorComponent() = default;
+
+        static void Reflect(AZ::ReflectContext* context);
+        static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
+        //////////////////////////////////////////////////////////////////////////
+        // Component overrides
+        void Activate() override;
+        void Deactivate() override;
+        //////////////////////////////////////////////////////////////////////////
+
+    private:
+        //////////////////////////////////////////////////////////////////////////
+        // ROS2SensorComponent overrides
+        void FrequencyTick() override;
+        //////////////////////////////////////////////////////////////////////////
+
+        void AddNewContact(const AzPhysics::CollisionEvent& event);
+
+        AZ::EntityId m_entityId;
+        AZStd::string m_entityName = "";
+
+        AzPhysics::SimulatedBodyEvents::OnCollisionBegin::Handler m_onCollisionBeginHandler;
+        AzPhysics::SimulatedBodyEvents::OnCollisionPersist::Handler m_onCollisionPersistHandler;
+        AzPhysics::SimulatedBodyEvents::OnCollisionEnd::Handler m_onCollisionEndHandler;
+
+        std::shared_ptr<rclcpp::Publisher<gazebo_msgs::msg::ContactsState>> m_contactsPublisher;
+
+        AZStd::unordered_map<AZ::EntityId, gazebo_msgs::msg::ContactState> m_activeContacts;
+        AZStd::mutex m_activeContactsMutex;
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/FactorySimulation/ConveyorBeltComponent.cpp

@@ -0,0 +1,338 @@
+/*
+ * 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 "ConveyorBeltComponent.h"
+#include <AtomLyIntegration/CommonFeatures/Material/MaterialComponentBus.h>
+#include <AtomLyIntegration/CommonFeatures/Mesh/MeshComponentBus.h>
+#include <AzCore/Asset/AssetSerializer.h>
+#include <AzCore/Math/Matrix3x3.h>
+#include <AzCore/Math/Transform.h>
+#include <AzCore/Serialization/EditContext.h>
+#include <AzFramework/Physics/Common/PhysicsSimulatedBody.h>
+#include <AzFramework/Physics/Components/SimulatedBodyComponentBus.h>
+#include <AzFramework/Physics/Material/PhysicsMaterialSlots.h>
+#include <AzFramework/Physics/PhysicsSystem.h>
+#include <AzFramework/Physics/RigidBodyBus.h>
+#include <LmbrCentral/Shape/SplineComponentBus.h>
+#include <Source/RigidBodyComponent.h>
+
+namespace ROS2
+{
+    void ConveyorBeltComponent::Reflect(AZ::ReflectContext* context)
+    {
+        ConveyorBeltComponentConfiguration::Reflect(context);
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<ConveyorBeltComponent, AZ::Component>()->Version(1)->Field(
+                "Configuration", &ConveyorBeltComponent::m_configuration);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<ConveyorBeltComponent>("Conveyor Belt Component", "Conveyor Belt Component.")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::AutoExpand, true)
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"))
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &ConveyorBeltComponent::m_configuration,
+                        "Configuration",
+                        "Configuration of the conveyor belt");
+            }
+        }
+        if (AZ::BehaviorContext* behaviorContext = azrtti_cast<AZ::BehaviorContext*>(context))
+        {
+            behaviorContext->EBus<ConveyorBeltRequestBus>("ConveyorBeltKinematic", "ConveyorBeltRequestBus")
+                ->Attribute(AZ::Edit::Attributes::Category, "ROS2/FactorySimulation/ConveyorBelt")
+                ->Event("StartBelt", &ConveyorBeltRequestBus::Events::StartBelt)
+                ->Event("StopBelt", &ConveyorBeltRequestBus::Events::StopBelt)
+                ->Event("IsBeltStopped", &ConveyorBeltRequestBus::Events::IsBeltStopped);
+        }
+    }
+
+    void ConveyorBeltComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
+    {
+        required.push_back(AZ_CRC_CE("SplineService"));
+    }
+
+    void ConveyorBeltComponent::Activate()
+    {
+        AzPhysics::SystemInterface* physicsSystem = AZ::Interface<AzPhysics::SystemInterface>::Get();
+        AZ_Assert(physicsSystem, "No physics system");
+        AzPhysics::SceneInterface* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
+        AZ_Assert(sceneInterface, "No scene interface");
+        AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
+        AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle");
+        m_sceneHandle = defaultSceneHandle;
+
+        m_splineTransform = AZ::Transform::CreateIdentity();
+        AZ::TransformBus::EventResult(m_splineTransform, m_entity->GetId(), &AZ::TransformBus::Events::GetWorldTM);
+
+        AZ::ConstSplinePtr splinePtr{ nullptr };
+        LmbrCentral::SplineComponentRequestBus::EventResult(splinePtr, m_entity->GetId(), &LmbrCentral::SplineComponentRequests::GetSpline);
+        AZ_Assert(splinePtr, "Unable to get spline for entity id (%s)", m_entity->GetId().ToString().c_str());
+
+        if (splinePtr)
+        {
+            m_splineConsPtr = splinePtr;
+            m_sceneFinishSimHandler = AzPhysics::SceneEvents::OnSceneSimulationFinishHandler(
+                [this]([[maybe_unused]] AzPhysics::SceneHandle sceneHandle, float fixedDeltaTime)
+                {
+                    SpawnSegments(fixedDeltaTime);
+                    MoveSegmentsPhysically(fixedDeltaTime);
+                    DespawnSegments();
+                },
+                aznumeric_cast<int32_t>(AzPhysics::SceneEvents::PhysicsStartFinishSimulationPriority::Components));
+            sceneInterface->RegisterSceneSimulationFinishHandler(defaultSceneHandle, m_sceneFinishSimHandler);
+            const auto& [startPoint, endPoint] = GetStartAndEndPointOfBelt(splinePtr);
+            m_startPoint = startPoint;
+            m_endPoint = endPoint;
+            m_splineLength = GetSplineLength(splinePtr);
+
+            // initial segment population
+            AZ_Assert(m_splineLength != 0.0f, "m_splineLength must be non-zero");
+            const float normalizedDistanceStep = SegmentSeparation * m_configuration.m_segmentSize / m_splineLength;
+            for (float normalizedIndex = 0.f; normalizedIndex < 1.f; normalizedIndex += normalizedDistanceStep)
+            {
+                m_conveyorSegments.push_back(CreateSegment(splinePtr, normalizedIndex));
+            }
+            AZ_Printf("ConveyorBeltComponent", "Initial Number of segments: %d", m_conveyorSegments.size());
+            AZ::TickBus::Handler::BusConnect();
+        }
+        AZ::EntityBus::Handler::BusConnect(m_configuration.m_conveyorEntityId);
+        ConveyorBeltRequestBus::Handler::BusConnect(m_configuration.m_conveyorEntityId);
+    }
+
+    void ConveyorBeltComponent::Deactivate()
+    {
+        if (m_sceneFinishSimHandler.IsConnected())
+        {
+            m_sceneFinishSimHandler.Disconnect();
+        }
+        ConveyorBeltRequestBus::Handler::BusDisconnect();
+        AZ::EntityBus::Handler::BusDisconnect();
+        AZ::TickBus::Handler::BusDisconnect();
+    }
+
+    AZ::Vector3 ConveyorBeltComponent::GetLocationOfSegment(const AzPhysics::SimulatedBodyHandle handle)
+    {
+        AzPhysics::SceneInterface* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
+        AZ_Assert(sceneInterface, "No scene interface");
+        auto* body = azdynamic_cast<AzPhysics::RigidBody*>(sceneInterface->GetSimulatedBodyFromHandle(m_sceneHandle, handle));
+        AZ_Assert(body, "No valid body found");
+        if (body)
+        {
+            AZ::Vector3 beginOfSegments = body->GetPosition();
+            return beginOfSegments;
+        }
+        return AZ::Vector3::CreateZero();
+    }
+    void ConveyorBeltComponent::OnEntityActivated(const AZ::EntityId& entityId)
+    {
+        if (m_configuration.m_conveyorEntityId.IsValid() && entityId == m_configuration.m_conveyorEntityId)
+        {
+            AZ::Render::MaterialComponentRequestBus::EventResult(
+                m_graphhicalMaterialId,
+                m_configuration.m_conveyorEntityId,
+                &AZ::Render::MaterialComponentRequestBus::Events::FindMaterialAssignmentId,
+                -1,
+                m_configuration.m_graphicalMaterialSlot);
+
+            AZStd::string foundMaterialName;
+            AZ::Render::MaterialComponentRequestBus::EventResult(
+                foundMaterialName,
+                m_configuration.m_conveyorEntityId,
+                &AZ::Render::MaterialComponentRequestBus::Events::GetMaterialLabel,
+                m_graphhicalMaterialId);
+
+            AZ_Warning(
+                "ConveyorBeltComponent",
+                m_configuration.m_graphicalMaterialSlot == foundMaterialName,
+                "Material slot \"%s\" not found on entity %s, found material slot \"%s\" instead.",
+                m_configuration.m_graphicalMaterialSlot.c_str(),
+                m_configuration.m_conveyorEntityId.ToString().c_str(),
+                foundMaterialName.c_str());
+            AZ_Assert(m_graphhicalMaterialId.IsSlotIdOnly(), "m_graphhicalMaterialId should be a slot id only");
+        }
+    }
+
+    AZStd::pair<float, AzPhysics::SimulatedBodyHandle> ConveyorBeltComponent::CreateSegment(
+        AZ::ConstSplinePtr splinePtr, float normalizedLocation)
+    {
+        AzPhysics::SystemInterface* physicsSystem = AZ::Interface<AzPhysics::SystemInterface>::Get();
+        AZ_Assert(physicsSystem != nullptr, "Unable to get Physics System");
+
+        auto colliderConfiguration = AZStd::make_shared<Physics::ColliderConfiguration>();
+        colliderConfiguration->m_isInSceneQueries = false;
+        colliderConfiguration->m_materialSlots.SetMaterialAsset(0, m_configuration.m_materialAsset);
+        colliderConfiguration->m_rotation = AZ::Quaternion::CreateFromAxisAngle(AZ::Vector3::CreateAxisX(), AZ::DegToRad(90.0f));
+        auto shapeConfiguration =
+            AZStd::make_shared<Physics::CapsuleShapeConfiguration>(m_configuration.m_beltWidth, m_configuration.m_segmentSize / 2.0f);
+        const auto transform = GetTransformFromSpline(m_splineConsPtr, normalizedLocation);
+        AzPhysics::RigidBodyConfiguration conveyorSegmentRigidBodyConfig;
+        conveyorSegmentRigidBodyConfig.m_kinematic = true;
+        conveyorSegmentRigidBodyConfig.m_position = transform.GetTranslation();
+        conveyorSegmentRigidBodyConfig.m_orientation = transform.GetRotation();
+        conveyorSegmentRigidBodyConfig.m_colliderAndShapeData = AzPhysics::ShapeColliderPair(colliderConfiguration, shapeConfiguration);
+        conveyorSegmentRigidBodyConfig.m_computeCenterOfMass = true;
+        conveyorSegmentRigidBodyConfig.m_computeInertiaTensor = true;
+        conveyorSegmentRigidBodyConfig.m_startSimulationEnabled = true;
+        conveyorSegmentRigidBodyConfig.m_computeMass = false;
+        conveyorSegmentRigidBodyConfig.m_mass = 1.0f;
+        conveyorSegmentRigidBodyConfig.m_entityId = GetEntityId();
+        conveyorSegmentRigidBodyConfig.m_debugName = "ConveyorBeltSegment";
+        AzPhysics::SimulatedBodyHandle handle = physicsSystem->GetScene(m_sceneHandle)->AddSimulatedBody(&conveyorSegmentRigidBodyConfig);
+        AZ_Assert(handle == AzPhysics::InvalidSimulatedBodyHandle, "Body created with invalid handle");
+        return AZStd::make_pair(normalizedLocation, handle);
+    }
+
+    void ConveyorBeltComponent::OnTick(float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
+    {
+        MoveSegmentsGraphically(deltaTime);
+    }
+
+    void ConveyorBeltComponent::StartBelt()
+    {
+        m_beltStopped = false;
+    }
+
+    void ConveyorBeltComponent::StopBelt()
+    {
+        m_beltStopped = true;
+    }
+
+    bool ConveyorBeltComponent::IsBeltStopped()
+    {
+        return m_beltStopped;
+    }
+
+    AZStd::pair<AZ::Vector3, AZ::Vector3> ConveyorBeltComponent::GetStartAndEndPointOfBelt(AZ::ConstSplinePtr splinePtr)
+    {
+        AZ::Transform splineTransform = AZ::Transform::Identity();
+        AZ::TransformBus::EventResult(splineTransform, m_entity->GetId(), &AZ::TransformBus::Events::GetWorldTM);
+        const AZ::SplineAddress addressBegin = splinePtr->GetAddressByFraction(0.f);
+        const AZ::SplineAddress addressEnd = splinePtr->GetAddressByFraction(1.f);
+        const AZ::Vector3 posBegin = splinePtr->GetPosition(addressBegin);
+        const AZ::Vector3 posEnd = splinePtr->GetPosition(addressEnd);
+        return { m_splineTransform.TransformPoint(posBegin), m_splineTransform.TransformPoint(posEnd) };
+    }
+
+    AZ::Transform ConveyorBeltComponent::GetTransformFromSpline(AZ::ConstSplinePtr splinePtr, float distanceNormalized)
+    {
+        AZ_Assert(splinePtr, "Spline pointer is null");
+        AZ::SplineAddress address = splinePtr->GetAddressByFraction(distanceNormalized);
+        const AZ::Vector3& p = splinePtr->GetPosition(address);
+
+        // construct the rotation matrix from three orthogonal vectors.
+        const AZ::Vector3& v1 = splinePtr->GetTangent(address);
+        const AZ::Vector3& v2 = splinePtr->GetNormal(address);
+        const AZ::Vector3 v3 = v1.Cross(v2);
+
+        AZ::Matrix3x3 rotationMatrix = AZ::Matrix3x3::CreateFromColumns(v1, v2, v3);
+        AZ_Assert(rotationMatrix.IsOrthogonal(0.001f), "Rotation matrix is not orthogonal");
+        rotationMatrix.Orthogonalize();
+        AZ::Transform transform = AZ::Transform::CreateFromMatrix3x3AndTranslation(
+            rotationMatrix, p - AZ::Vector3::CreateAxisZ(m_configuration.m_segmentSize / 2.f));
+        return m_splineTransform * transform;
+    }
+
+    float ConveyorBeltComponent::GetSplineLength(AZ::ConstSplinePtr splinePtr)
+    {
+        AZ_Assert(splinePtr, "Spline pointer is null");
+        AZ::Transform splineTransform = AZ::Transform::Identity();
+        AZ::TransformBus::EventResult(splineTransform, m_entity->GetId(), &AZ::TransformBus::Events::GetWorldTM);
+        const AZ::SplineAddress addressEnd = splinePtr->GetAddressByFraction(1.f);
+        return splinePtr->GetLength(addressEnd);
+    }
+
+    void ConveyorBeltComponent::MoveSegmentsGraphically(float deltaTime)
+    {
+        if (m_beltStopped)
+        { // Do not move texture when stopped
+            return;
+        }
+
+        if (!m_configuration.m_conveyorEntityId.IsValid())
+        {
+            return;
+        }
+
+        // Animate texture
+
+        m_textureOffset += deltaTime * m_configuration.m_speed * m_configuration.m_textureScale;
+
+        AZ::Render::MaterialComponentRequestBus::Event(
+            m_configuration.m_conveyorEntityId,
+            &AZ::Render::MaterialComponentRequestBus::Events::SetPropertyValueT<float>,
+            m_graphhicalMaterialId,
+            "uv.offsetU",
+            m_textureOffset);
+    }
+
+    void ConveyorBeltComponent::DespawnSegments()
+    {
+        bool wasSegmentRemoved = false;
+        for (auto& [pos, handle] : m_conveyorSegments)
+        {
+            if (pos > 1.0f)
+            {
+                AZ::Interface<AzPhysics::SceneInterface>::Get()->RemoveSimulatedBody(m_sceneHandle, handle);
+                handle = AzPhysics::InvalidSimulatedBodyHandle;
+                wasSegmentRemoved = true;
+            }
+        }
+        if (wasSegmentRemoved)
+        {
+            const auto isInvalidHandle = [](const auto& pair)
+            {
+                return pair.second == AzPhysics::InvalidSimulatedBodyHandle;
+            };
+            // clear object handle from cache
+            m_conveyorSegments.erase(
+                AZStd::remove_if(m_conveyorSegments.begin(), m_conveyorSegments.end(), isInvalidHandle), m_conveyorSegments.end());
+        }
+    }
+
+    void ConveyorBeltComponent::MoveSegmentsPhysically(float fixedDeltaTime)
+    {
+        if (m_beltStopped)
+        { // Do not move segments when stopped
+            return;
+        }
+
+        AzPhysics::SceneInterface* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
+        AZ_Assert(sceneInterface != nullptr, "Unable to get Scene Interface");
+        // update positions of the segments
+        for (auto& [pos, handle] : m_conveyorSegments)
+        {
+            auto* body = azdynamic_cast<AzPhysics::RigidBody*>(sceneInterface->GetSimulatedBodyFromHandle(m_sceneHandle, handle));
+            if (body)
+            {
+                pos += m_configuration.m_speed * fixedDeltaTime / m_splineLength;
+                auto transform = GetTransformFromSpline(m_splineConsPtr, pos);
+                transform.SetTranslation(transform.GetTranslation());
+                body->SetKinematicTarget(transform);
+            }
+        }
+    }
+
+    void ConveyorBeltComponent::SpawnSegments(float deltaTime)
+    {
+        m_deltaTimeFromLastSpawn += deltaTime;
+        if (m_conveyorSegments.empty())
+        {
+            m_conveyorSegments.push_back(CreateSegment(m_splineConsPtr, 0.f));
+            return;
+        }
+        if (m_deltaTimeFromLastSpawn > SegmentSeparation * m_configuration.m_segmentSize / m_configuration.m_speed)
+        {
+            m_deltaTimeFromLastSpawn = 0.f;
+            m_conveyorSegments.push_back(CreateSegment(m_splineConsPtr, 0.f));
+        }
+    }
+
+} // namespace ROS2

Gems/ROS2/Code/Source/FactorySimulation/ConveyorBeltComponent.h

@@ -0,0 +1,120 @@
+/*
+ * 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 "ConveyorBeltComponentConfiguration.h"
+#include <AtomLyIntegration/CommonFeatures/Material/MaterialAssignmentId.h>
+#include <AzCore/Component/Component.h>
+#include <AzCore/Component/Entity.h>
+#include <AzCore/Component/EntityBus.h>
+#include <AzCore/Component/TickBus.h>
+#include <AzCore/Math/Spline.h>
+#include <AzCore/Math/Transform.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <AzCore/std/containers/deque.h>
+#include <AzFramework/Physics/Common/PhysicsEvents.h>
+#include <AzFramework/Physics/Common/PhysicsSimulatedBodyEvents.h>
+#include <AzFramework/Physics/Common/PhysicsTypes.h>
+#include <AzFramework/Physics/Material/PhysicsMaterialAsset.h>
+#include <AzFramework/Physics/PhysicsSystem.h>
+#include <ROS2/FactorySimulation/ConveyorBeltRequestBus.h>
+
+namespace ROS2
+{
+    //! Component that simulates a conveyor belt using kinematic physics.
+    //! The conveyor belt is simulated using a spline and number of kinematic rigid bodies.
+    //! The kinematic rigid bodies have their kinematic targets set to interpolate along the spline.
+    //! The component is updating kinematic targets every physic sub-step and creates and despawns rigid bodies as needed.
+    class ConveyorBeltComponent
+        : public AZ::Component
+        , public AZ::TickBus::Handler
+        , public AZ::EntityBus::Handler
+        , protected ROS2::ConveyorBeltRequestBus::Handler
+    {
+        static constexpr float SegmentSeparation = 1.0f; //!< Separation between segments of the belt (in normalized units)
+
+    public:
+        AZ_COMPONENT(ConveyorBeltComponent, "{B7F56411-01D4-48B0-8874-230C58A578BD}");
+        ConveyorBeltComponent() = default;
+        virtual ~ConveyorBeltComponent() = default;
+        static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
+        static void Reflect(AZ::ReflectContext* context);
+
+        // Component overrides
+        void Activate() override;
+        void Deactivate() override;
+
+    private:
+        // EntityBus::Handler overrides
+        void OnEntityActivated(const AZ::EntityId& entityId) override;
+
+        //! Obtains the start and end point of the simulated conveyor belt
+        //! @param splinePtr the spline to obtain the start and end point from
+        //! @return a pair of vectors, the first being the start point and the second being the end point
+        AZStd::pair<AZ::Vector3, AZ::Vector3> GetStartAndEndPointOfBelt(AZ::ConstSplinePtr splinePtr);
+
+        //! Obtain the length of the spline
+        //! @param splinePtr the spline to obtain the length from
+        float GetSplineLength(AZ::ConstSplinePtr splinePtr);
+
+        //! Obtains location of the segment of the belt.
+        //! @param handle the handle of the simulated body of the segment
+        //! @return the location of the segment in world space
+        AZ::Vector3 GetLocationOfSegment(const AzPhysics::SimulatedBodyHandle handle);
+
+        //! Obtains the transform of the pose on the spline at the given distance
+        //! @param splinePtr the spline to obtain the transform from
+        //! @param distanceNormalized the distance along the spline to obtain the transform from (normalized)
+        //! @return the transform of the pose on the spline at the given distance
+        AZ::Transform GetTransformFromSpline(AZ::ConstSplinePtr splinePtr, float distanceNormalized);
+
+        //! Spawn a rigid body at the given location
+        //! @param splinePtr the spline to spawn the rigid body on
+        //! @param location the location to spawn the rigid body at (normalized)
+        //! @return a pair of the normalized location and the handle of the simulated body
+        AZStd::pair<float, AzPhysics::SimulatedBodyHandle> CreateSegment(AZ::ConstSplinePtr splinePtr, float normalizedLocation);
+
+        // AZ::TickBus::Handler overrides...
+        void OnTick(float delta, AZ::ScriptTimePoint timePoint) override;
+
+        // ROS2::ConveyorBeltRequestBus::Handler overrides...
+        void StartBelt() override;
+        void StopBelt() override;
+        bool IsBeltStopped() override;
+
+        //! Update texture offset of the conveyor belt
+        //! @param deltaTime the time since the last update
+        void MoveSegmentsGraphically(float deltaTime);
+
+        //! Update location of segments in physics scene
+        //! @param deltaTime the time since the last update
+        void MoveSegmentsPhysically(float deltaTime);
+
+        //! Despawn segments that are at the end of the spline
+        void DespawnSegments();
+
+        //! Spawn segments with given rate of spawning
+        //! @param deltaTime the time since the last call of the function
+        void SpawnSegments(float deltaTime);
+
+        ConveyorBeltComponentConfiguration m_configuration; //!< Configuration of the component
+
+        AzPhysics::SceneEvents::OnSceneSimulationFinishHandler m_sceneFinishSimHandler; //!< Handler called after every physics sub-step
+        AZStd::deque<AZStd::pair<float, AzPhysics::SimulatedBodyHandle>> m_conveyorSegments; //!< Cache of created segments
+        float m_textureOffset = 0.0f; //!< Current offset of the texture during animation
+        AZ::ConstSplinePtr m_splineConsPtr{ nullptr }; //!< Pointer to the spline
+        float m_splineLength = -1.0f; //!< Non-normalized spline length
+        AZ::Transform m_splineTransform; //!< Transform from spline's local frame to world frame
+        AZ::Vector3 m_startPoint; //!< Start point of the belt
+        AZ::Vector3 m_endPoint; //!< End point of the belt
+        AzPhysics::SceneHandle m_sceneHandle; //!< Scene handle of the scene the belt is in
+        bool m_beltStopped = false; //!< State of the conveyor belt
+        float m_deltaTimeFromLastSpawn = 0.0f; //!< Time since the last spawn
+        AZ::Render::MaterialAssignmentId m_graphhicalMaterialId; //!< Material id of the animated belt
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/FactorySimulation/ConveyorBeltComponentConfiguration.cpp

@@ -0,0 +1,91 @@
+/*
+ * 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 "ConveyorBeltComponentConfiguration.h"
+#include <AzCore/Asset/AssetSerializer.h>
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/EditContextConstants.inl>
+#include <AzFramework/Physics/Material/PhysicsMaterialManager.h>
+
+namespace ROS2
+{
+    namespace Internal
+    {
+        AZ::Data::AssetId GetDefaultPhysicsMaterialAssetId()
+        {
+            // Used for Edit Context.
+            // When the physics material asset property doesn't have an asset assigned it
+            // will show "(default)" to indicate that the default material will be used.
+            if (auto* materialManager = AZ::Interface<Physics::MaterialManager>::Get())
+            {
+                if (AZStd::shared_ptr<Physics::Material> defaultMaterial = materialManager->GetDefaultMaterial())
+                {
+                    return defaultMaterial->GetMaterialAsset().GetId();
+                }
+            }
+            return {};
+        }
+    } // namespace Internal
+
+    void ConveyorBeltComponentConfiguration::Reflect(AZ::ReflectContext* context)
+    {
+        if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serializeContext->Class<ConveyorBeltComponentConfiguration>()
+                ->Version(1)
+                ->Field("BeltEntityId", &ConveyorBeltComponentConfiguration::m_conveyorEntityId)
+                ->Field("Speed", &ConveyorBeltComponentConfiguration::m_speed)
+                ->Field("BeltWidth", &ConveyorBeltComponentConfiguration::m_beltWidth)
+                ->Field("SegmentSize", &ConveyorBeltComponentConfiguration::m_segmentSize)
+                ->Field("TextureScale", &ConveyorBeltComponentConfiguration::m_textureScale)
+                ->Field("MaterialAsset", &ConveyorBeltComponentConfiguration::m_materialAsset)
+                ->Field("GraphicalMaterialSlot", &ConveyorBeltComponentConfiguration::m_graphicalMaterialSlot);
+
+            if (AZ::EditContext* ec = serializeContext->GetEditContext())
+            {
+                ec->Class<ConveyorBeltComponentConfiguration>("Conveyor Belt Component Configuration", "Conveyor Belt Component")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::EntityId,
+                        &ConveyorBeltComponentConfiguration::m_conveyorEntityId,
+                        "Conveyor Belt Entity",
+                        "Entity of the conveyor belt")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &ConveyorBeltComponentConfiguration::m_speed,
+                        "Initial Speed",
+                        "Initial speed of the conveyor belt")
+                    ->DataElement(AZ::Edit::UIHandlers::Default, &ConveyorBeltComponentConfiguration::m_beltWidth, "Width", "Belt width")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &ConveyorBeltComponentConfiguration::m_segmentSize,
+                        "Segment Size",
+                        "Size of simulated segments. Short segments might affect performance"
+                        "of the physics engine.")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &ConveyorBeltComponentConfiguration::m_textureScale,
+                        "Texture scale",
+                        "Scale of the texture on conveyor belt.")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &ConveyorBeltComponentConfiguration::m_materialAsset,
+                        "Physical Material",
+                        "Physical Material asset of the conveyor belt")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &ConveyorBeltComponentConfiguration::m_graphicalMaterialSlot,
+                        "Graphical Material slot",
+                        "The graphical material slot name to have its UV coordinates animated.")
+                    ->Attribute(AZ::Edit::Attributes::DefaultAsset, &Internal::GetDefaultPhysicsMaterialAssetId)
+                    ->Attribute(AZ_CRC_CE("EditButton"), "")
+                    ->Attribute(AZ_CRC_CE("EditDescription"), "Open in Asset Editor")
+                    ->Attribute(AZ_CRC_CE("DisableEditButtonWhenNoAssetSelected"), true);
+            }
+        }
+    }
+} // namespace ROS2

Gems/ROS2/Code/Source/FactorySimulation/ConveyorBeltComponentConfiguration.h

@@ -0,0 +1,31 @@
+/*
+ * 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/Component/Entity.h>
+#include <AzCore/RTTI/RTTI.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <AzCore/std/string/string.h>
+#include <AzFramework/Physics/Material/PhysicsMaterialAsset.h>
+
+namespace ROS2
+{
+    struct ConveyorBeltComponentConfiguration
+    {
+        AZ_TYPE_INFO(ConveyorBeltComponentConfiguration, "{ebaf61a0-20c5-11ee-be56-0242ac120002}");
+        static void Reflect(AZ::ReflectContext* context);
+
+        AZStd::string m_graphicalMaterialSlot{ "Belt" }; //!< Name of the material slot to change UVs of
+        float m_beltWidth = 1.0f; //!< Width of the conveyor belt
+        float m_segmentSize = 0.1f; //!< Length of individual segments of the conveyor belt
+        AZ::Data::Asset<Physics::MaterialAsset> m_materialAsset; //!< Material of individual segments of the conveyor belt
+        AZ::EntityId m_conveyorEntityId; //!< Conveyor belt entity (used for texture movement)
+        float m_textureScale = 1.0f; //!< Scaling factor of the texture
+        float m_speed = 1.0f; //!< Initial speed of the conveyor belt
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/Frame/NamespaceConfiguration.cpp

@@ -63,6 +63,13 @@ namespace ROS2
         return ROS2Names::GetNamespacedName(parentNamespace, m_namespace);
     }
 
+    void NamespaceConfiguration::SetNamespace(const AZStd::string& ns, NamespaceStrategy strategy)
+    {
+        m_namespace = ns;
+        m_namespaceStrategy = strategy;
+        UpdateNamespace();
+    }
+
     bool NamespaceConfiguration::IsNamespaceCustom() const
     {
         return m_namespaceStrategy == NamespaceConfiguration::NamespaceStrategy::Custom;

Gems/ROS2/Code/Source/Frame/ROS2FrameComponent.cpp

@@ -139,6 +139,11 @@ namespace ROS2
         return ROS2Names::GetNamespacedName(GetNamespace(), AZStd::string("odom"));
     }
 
+    void ROS2FrameComponent::UpdateNamespaceConfiguration(const AZStd::string& ns, NamespaceConfiguration::NamespaceStrategy strategy)
+    {
+        m_namespaceConfiguration.SetNamespace(ns, strategy);
+    }
+
     bool ROS2FrameComponent::IsTopLevel() const
     {
         return GetGlobalFrameName() == GetParentFrameID();

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

@@ -0,0 +1,47 @@
+/*
+ * 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 "GNSSSensorConfiguration.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/EditContextConstants.inl>
+
+namespace ROS2
+{
+    void GNSSSensorConfiguration::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<GNSSSensorConfiguration>()
+                ->Version(1)
+                ->Field("originLatitude", &GNSSSensorConfiguration::m_originLatitudeDeg)
+                ->Field("originLongitude", &GNSSSensorConfiguration::m_originLongitudeDeg)
+                ->Field("originAltitude", &GNSSSensorConfiguration::m_originAltitude);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<GNSSSensorConfiguration>("ROS2 GNSS Sensor", "GNSS sensor component")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &GNSSSensorConfiguration::m_originLatitudeDeg,
+                        "Latitude offset",
+                        "GNSS latitude position offset in degrees")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &GNSSSensorConfiguration::m_originLongitudeDeg,
+                        "Longitude offset",
+                        "GNSS longitude position offset in degrees")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &GNSSSensorConfiguration::m_originAltitude,
+                        "Altitude offset",
+                        "GNSS altitude position offset in meters");
+            }
+        }
+    }
+
+} // namespace ROS2

Gems/ROS2/Code/Source/GNSS/GNSSSensorConfiguration.h

@@ -0,0 +1,26 @@
+/*
+ * 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/Serialization/SerializeContext.h>
+#include <AzCore/std/string/string.h>
+
+namespace ROS2
+{
+    //! A structure capturing configuration of a Global Navigation Satellite Systems (GNSS) sensor.
+    struct GNSSSensorConfiguration
+    {
+        AZ_TYPE_INFO(GNSSSensorConfiguration, "{8bc39c6b-2f2c-4612-bcc7-0cc7033001d4}");
+        static void Reflect(AZ::ReflectContext* context);
+
+        float m_originLatitudeDeg = 0.0f;
+        float m_originLongitudeDeg = 0.0f;
+        float m_originAltitude = 0.0f;
+    };
+} // namespace ROS2

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

@@ -28,13 +28,12 @@ namespace ROS2
 
     void ROS2GNSSSensorComponent::Reflect(AZ::ReflectContext* context)
     {
+        GNSSSensorConfiguration::Reflect(context);
+
         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);
+            serialize->Class<ROS2GNSSSensorComponent, ROS2SensorComponent>()->Version(2)->Field(
+                "gnssSensorConfiguration", &ROS2GNSSSensorComponent::m_gnssConfiguration);
 
             if (AZ::EditContext* ec = serialize->GetEditContext())
             {
@@ -44,19 +43,10 @@ namespace ROS2
                     ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("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");
+                        &ROS2GNSSSensorComponent::m_gnssConfiguration,
+                        "GNSS sensor configuration",
+                        "GNSS sensor configuration")
+                    ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly);
             }
         }
     }
@@ -70,6 +60,13 @@ namespace ROS2
         m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(Internal::kGNSSMsgType, pc));
     }
 
+    ROS2GNSSSensorComponent::ROS2GNSSSensorComponent(
+        const SensorConfiguration& sensorConfiguration, const GNSSSensorConfiguration& gnssConfiguration)
+        : m_gnssConfiguration(gnssConfiguration)
+    {
+        m_sensorConfiguration = sensorConfiguration;
+    }
+
     void ROS2GNSSSensorComponent::Activate()
     {
         ROS2SensorComponent::Activate();
@@ -92,8 +89,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 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();

Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h

@@ -13,6 +13,8 @@
 #include <rclcpp/publisher.hpp>
 #include <sensor_msgs/msg/nav_sat_fix.hpp>
 
+#include "GNSSSensorConfiguration.h"
+
 namespace ROS2
 {
     //! Global Navigation Satellite Systems (GNSS) sensor component class
@@ -24,6 +26,7 @@ namespace ROS2
     public:
         AZ_COMPONENT(ROS2GNSSSensorComponent, "{55B4A299-7FA3-496A-88F0-764C75B0E9A7}", ROS2SensorComponent);
         ROS2GNSSSensorComponent();
+        ROS2GNSSSensorComponent(const SensorConfiguration& sensorConfiguration, const GNSSSensorConfiguration& gnssConfiguration);
         ~ROS2GNSSSensorComponent() = default;
         static void Reflect(AZ::ReflectContext* context);
         //////////////////////////////////////////////////////////////////////////
@@ -33,9 +36,7 @@ namespace ROS2
         //////////////////////////////////////////////////////////////////////////
 
     private:
-        float m_gnssOriginLatitudeDeg = 0.0f;
-        float m_gnssOriginLongitudeDeg = 0.0f;
-        float m_gnssOriginAltitude = 0.0f;
+        GNSSSensorConfiguration m_gnssConfiguration;
 
         //////////////////////////////////////////////////////////////////////////
         // ROS2SensorComponent overrides

Gems/ROS2/Code/Source/Gripper/FingerGripperComponent.cpp

@@ -0,0 +1,281 @@
+/*
+ * 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 "FingerGripperComponent.h"
+#include "Utils.h"
+
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/SerializeContext.h>
+
+#include <AzFramework/Physics/PhysicsSystem.h>
+#include <ROS2/Frame/ROS2FrameComponent.h>
+#include <ROS2/Manipulation/JointsManipulationRequests.h>
+#include <Utilities/JointUtilities.h>
+#include <imgui/imgui.h>
+
+namespace ROS2
+{
+    void FingerGripperComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
+    {
+        provided.push_back(AZ_CRC_CE("GripperService"));
+    }
+    void FingerGripperComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible)
+    {
+        incompatible.push_back(AZ_CRC_CE("GripperService"));
+    }
+
+    void FingerGripperComponent::Activate()
+    {
+        m_grippingInProgress = false;
+        m_initialised = false;
+        m_cancelled = false;
+        m_ImGuiPosition = 0.0f;
+        m_stallingFor = 0.0f;
+        AZ::TickBus::Handler::BusConnect();
+        ImGui::ImGuiUpdateListenerBus::Handler::BusConnect();
+        GripperRequestBus::Handler::BusConnect(GetEntityId());
+    }
+
+    void FingerGripperComponent::Deactivate()
+    {
+        AZ::TickBus::Handler::BusDisconnect();
+        ImGui::ImGuiUpdateListenerBus::Handler::BusDisconnect();
+        GripperRequestBus::Handler::BusDisconnect(GetEntityId());
+    }
+
+    void FingerGripperComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<FingerGripperComponent, AZ::Component>()
+                ->Field("VelocityEpsilon", &FingerGripperComponent::m_velocityEpsilon)
+                ->Field("DistanceEpsilon", &FingerGripperComponent::m_goalTolerance)
+                ->Field("StallTime", &FingerGripperComponent::m_stallTime)
+                ->Version(1);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<FingerGripperComponent>("FingerGripperComponent", "Component controlling a finger gripper.")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "FingerGripperComponent")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &FingerGripperComponent::m_velocityEpsilon,
+                        "Velocity Epsilon",
+                        "The maximum velocity to consider the gripper as stalled.")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &FingerGripperComponent::m_goalTolerance,
+                        "Goal tolerance",
+                        "Goal is considered reached if the gripper is at this distance or closer")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &FingerGripperComponent::m_stallTime,
+                        "Stall Time",
+                        "The time to wait before considering the gripper as stalled.");
+            }
+        }
+    }
+
+    ManipulationJoints& FingerGripperComponent::GetFingerJoints()
+    {
+        m_fingerJoints.clear();
+        m_rootOfArticulation = Utils::GetRootOfArticulation(GetEntityId());
+        AZ_Warning(
+            "FingerGripperComponent",
+            m_rootOfArticulation.IsValid(),
+            "Entity %s is not part of an articulation.",
+            GetEntity()->GetName().c_str());
+        ManipulationJoints allJoints;
+        if (m_rootOfArticulation.IsValid())
+        {
+            JointsManipulationRequestBus::EventResult(allJoints, m_rootOfArticulation, &JointsManipulationRequests::GetJoints);
+        }
+        AZStd::vector<AZ::EntityId> descendantIds;
+        AZ::TransformBus::EventResult(descendantIds, GetEntityId(), &AZ::TransformBus::Events::GetAllDescendants);
+
+        for (AZ::EntityId descendant : descendantIds)
+        {
+            AZStd::string jointName = ROS2::Utils::GetJointName(descendant);
+            if (!jointName.empty())
+            {
+                AZ_Printf("FingerGripperComponent", "Adding finger joint %s", jointName.c_str());
+                m_fingerJoints[jointName] = allJoints[jointName];
+            }
+        }
+
+        return m_fingerJoints;
+    }
+
+    void FingerGripperComponent::SetPosition(float position, float maxEffort)
+    {
+        if (m_fingerJoints.empty())
+        {
+            return;
+        }
+
+        float targetPosition = position;
+        for (auto& [jointName, jointInfo] : m_fingerJoints)
+        {
+            AZ::Outcome<void, AZStd::string> result;
+            JointsManipulationRequestBus::EventResult(
+                result, m_rootOfArticulation, &JointsManipulationRequests::MoveJointToPosition, jointName, targetPosition);
+            if (!result.IsSuccess())
+            {
+                AZ_Warning(
+                    "FingerGripperComponent",
+                    result,
+                    "Joint move cannot be realized: %s for %s ",
+                    result.GetError().c_str(),
+                    jointName.c_str());
+            }
+        }
+
+        float oneMaxEffort = maxEffort / m_fingerJoints.size();
+        for (auto& [jointName, jointInfo] : m_fingerJoints)
+        {
+            AZ::Outcome<void, AZStd::string> result;
+            JointsManipulationRequestBus::EventResult(
+                result, m_rootOfArticulation, &JointsManipulationRequests::SetMaxJointEffort, jointName, oneMaxEffort);
+            if (!result.IsSuccess())
+            {
+                AZ_Warning(
+                    "FingerGripperComponent",
+                    result,
+                    "Setting a max force for a joint cannot be realized: %s for %s ",
+                    result.GetError().c_str(),
+                    jointName.c_str());
+            }
+        }
+    }
+
+    AZ::Outcome<void, AZStd::string> FingerGripperComponent::GripperCommand(float position, float maxEffort)
+    {
+        if (maxEffort == 0.0f)
+        { // The moveit panda demo fills the max effort fields with 0s, but we want to exert effort
+            maxEffort = AZStd::numeric_limits<float>::infinity();
+        }
+
+        m_grippingInProgress = true;
+        m_desiredPosition = position;
+        m_stallingFor = 0.0f;
+        m_cancelled = false;
+
+        SetPosition(position, maxEffort);
+
+        return AZ::Success();
+    }
+
+    AZ::Outcome<void, AZStd::string> FingerGripperComponent::CancelGripperCommand()
+    {
+        m_grippingInProgress = false;
+        m_cancelled = true;
+        SetPosition(0.0f, AZStd::numeric_limits<float>::infinity());
+        return AZ::Success();
+    }
+
+    bool FingerGripperComponent::HasGripperCommandBeenCancelled() const
+    {
+        return m_cancelled;
+    }
+
+    float FingerGripperComponent::GetGripperPosition() const
+    {
+        float gripperPosition = 0.0f;
+        if (m_fingerJoints.empty())
+        {
+            return gripperPosition;
+        }
+
+        for (const auto& [jointName, _] : m_fingerJoints)
+        {
+            AZ::Outcome<JointPosition, AZStd::string> result;
+            JointsManipulationRequestBus::EventResult(
+                result, m_rootOfArticulation, &JointsManipulationRequests::GetJointPosition, jointName);
+            gripperPosition += result.GetValueOr(0.f);
+        }
+
+        return gripperPosition / m_fingerJoints.size();
+    }
+
+    float FingerGripperComponent::GetGripperEffort() const
+    {
+        float gripperEffort = 0.0f;
+        for (const auto& [jointName, _] : m_fingerJoints)
+        {
+            AZ::Outcome<JointEffort, AZStd::string> result;
+            JointsManipulationRequestBus::EventResult(result, m_rootOfArticulation, &JointsManipulationRequests::GetJointEffort, jointName);
+            if (result)
+            {
+                gripperEffort += result.GetValue();
+            }
+        }
+        return gripperEffort;
+    }
+
+    bool FingerGripperComponent::IsGripperVelocity0() const
+    {
+        for (const auto& [jointName, _] : m_fingerJoints)
+        {
+            AZ::Outcome<JointEffort, AZStd::string> velocityResult;
+            JointsManipulationRequestBus::EventResult(
+                velocityResult, m_rootOfArticulation, &JointsManipulationRequests::GetJointVelocity, jointName);
+            if (velocityResult && AZStd::abs(velocityResult.GetValue()) > m_velocityEpsilon)
+            {
+                return false;
+            }
+        }
+
+        return true;
+    }
+
+    bool FingerGripperComponent::IsGripperNotMoving() const
+    {
+        return m_stallingFor > m_stallTime;
+    }
+
+    bool FingerGripperComponent::HasGripperReachedGoal() const
+    {
+        return !m_grippingInProgress || AZStd::abs(GetGripperPosition() - m_desiredPosition) < m_goalTolerance;
+    }
+
+    void FingerGripperComponent::OnImGuiUpdate()
+    {
+        ImGui::Begin("FingerGripperDebugger");
+
+        ImGui::SliderFloat("Target Position", &m_ImGuiPosition, 0.0f, 0.1f);
+
+        if (ImGui::Button("Execute Command"))
+        {
+            GripperCommand(m_ImGuiPosition, AZStd::numeric_limits<float>::infinity());
+        }
+
+        ImGui::End();
+    }
+
+    void FingerGripperComponent::OnTick([[maybe_unused]] float delta, [[maybe_unused]] AZ::ScriptTimePoint timePoint)
+    {
+        if (!m_initialised)
+        {
+            m_initialised = true;
+            GetFingerJoints();
+            SetPosition(0.0f, AZStd::numeric_limits<float>::infinity());
+        }
+
+        if (IsGripperVelocity0())
+        {
+            m_stallingFor += delta;
+        }
+        else
+        {
+            m_stallingFor = 0.0f;
+        }
+    }
+} // namespace ROS2

Gems/ROS2/Code/Source/Gripper/FingerGripperComponent.h

@@ -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
+ *
+ */
+#pragma once
+
+#include <AzCore/Component/Component.h>
+#include <AzCore/Component/TickBus.h>
+#include <AzFramework/Physics/Common/PhysicsEvents.h>
+#include <ImGuiBus.h>
+#include <ROS2/Gripper/GripperRequestBus.h>
+#include <ROS2/Manipulation/JointsManipulationRequests.h>
+#include <Utilities/ArticulationsUtilities.h>
+
+namespace ROS2
+{
+    //! This component implements finger gripper functionality.
+    class FingerGripperComponent
+        : public AZ::Component
+        , public GripperRequestBus::Handler
+        , public ImGui::ImGuiUpdateListenerBus::Handler
+        , public AZ::TickBus::Handler
+    {
+    public:
+        AZ_COMPONENT(FingerGripperComponent, "{ae5f8ec2-26ee-11ee-be56-0242ac120002}", AZ::Component);
+        FingerGripperComponent() = default;
+        ~FingerGripperComponent() = default;
+
+        // AZ::Component overrides...
+        void Activate() override;
+        void Deactivate() override;
+
+        static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided);
+        static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible);
+
+        static void Reflect(AZ::ReflectContext* context);
+
+    private:
+        // GripperRequestBus::Handler overrides...
+        AZ::Outcome<void, AZStd::string> GripperCommand(float position, float maxEffort) override;
+        AZ::Outcome<void, AZStd::string> CancelGripperCommand() override;
+        bool HasGripperCommandBeenCancelled() const override;
+
+        // Sum of all joint positions
+        float GetGripperPosition() const override;
+        // Sum of all efforts exerted by fingers
+        float GetGripperEffort() const override;
+        // Non-articulation fingers return 0 effort!
+        bool IsGripperNotMoving() const override;
+        // Doesn't check if the max force is applied, only checks speed
+        bool HasGripperReachedGoal() const override;
+
+        // ImGui::ImGuiUpdateListenerBus::Handler overrides...
+        void OnImGuiUpdate() override;
+
+        // AZ::TickBus::Handler overrides...
+        void OnTick(float delta, AZ::ScriptTimePoint timePoint) override;
+
+        ManipulationJoints& GetFingerJoints();
+
+        AZ::EntityId m_rootOfArticulation; //!< The root of the articulation chain
+
+        float GetDefaultPosition();
+        void SetPosition(float position, float maxEffort);
+        bool IsGripperVelocity0() const;
+        void PublishFeedback() const;
+
+        ManipulationJoints m_fingerJoints;
+        bool m_grippingInProgress{ false };
+        bool m_cancelled{ false };
+        bool m_initialised{ false };
+        float m_desiredPosition{ false };
+        float m_stallingFor{ 0.f };
+        float m_ImGuiPosition{ 0.1f };
+
+        float m_velocityEpsilon{ 0.01f }; //!< The epsilon value used to determine whether the gripper is moving
+        float m_goalTolerance{ 0.001f }; //!< The epsilon value used to determine whether the gripper reached it's goal
+        float m_stallTime{ 0.1f }; //!< The time in seconds to wait before determining the gripper is stalled
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/Gripper/GripperActionServer.cpp

@@ -0,0 +1,131 @@
+/*
+ * 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 "Utils.h"
+
+#include "GripperActionServer.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <AzFramework/Components/TransformComponent.h>
+#include <ROS2/ROS2Bus.h>
+#include <ROS2/ROS2GemUtilities.h>
+
+namespace ROS2
+{
+    GripperActionServer::GripperActionServer(const AZStd::string& actionName, const AZ::EntityId& entityId)
+        : m_entityId(entityId)
+    {
+        auto ros2Node = ROS2Interface::Get()->GetNode();
+        actionServer = rclcpp_action::create_server<GripperCommand>(
+            ros2Node,
+            actionName.data(),
+            AZStd::bind(&GripperActionServer::GoalReceivedCallback, this, AZStd::placeholders::_1, AZStd::placeholders::_2),
+            AZStd::bind(&GripperActionServer::GoalCancelledCallback, this, AZStd::placeholders::_1),
+            AZStd::bind(&GripperActionServer::GoalAcceptedCallback, this, AZStd::placeholders::_1));
+    }
+
+    bool GripperActionServer::IsGoalActiveState() const
+    {
+        if (!m_goalHandle)
+        {
+            return false;
+        }
+        return m_goalHandle->is_active() || m_goalHandle->is_executing() || m_goalHandle->is_canceling();
+    }
+
+    bool GripperActionServer::IsReadyForExecution() const
+    {
+        // Has no goal handle yet - can be accepted.
+        if (!m_goalHandle)
+        {
+            return true;
+        }
+        // Accept if the previous one is in a terminal state.
+        return IsGoalActiveState() == false;
+    }
+
+    rclcpp_action::GoalResponse GripperActionServer::GoalReceivedCallback(
+        [[maybe_unused]] const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const GripperCommand::Goal> goal)
+    {
+        if (!IsReadyForExecution())
+        {
+            AZ_Printf("GripperActionServer", "GripperActionServer::handleGoal: Rejected goal, already executing\n");
+            if (m_goalHandle)
+            {
+                AZ_Trace(
+                    "GripperActionServer",
+                    " is_active: %d,  is_executing %d, is_canceling : %d\n",
+                    m_goalHandle->is_active(),
+                    m_goalHandle->is_executing(),
+                    m_goalHandle->is_canceling());
+            }
+            return rclcpp_action::GoalResponse::REJECT;
+        }
+
+        AZ::Outcome<void, AZStd::string> commandOutcome = AZ::Failure(AZStd::string("No gripper component found!"));
+        GripperRequestBus::EventResult(
+            commandOutcome, m_entityId, &GripperRequestBus::Events::GripperCommand, goal->command.position, goal->command.max_effort);
+        if (!commandOutcome.IsSuccess())
+        {
+            AZ_Trace("GripperActionServer", "GripperCommand could not be accepted: %s\n", commandOutcome.GetError().c_str());
+            return rclcpp_action::GoalResponse::REJECT;
+        }
+        AZ_Trace("GripperActionServer", "GripperActionServer::handleGoal: GripperCommand accepted!\n");
+        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    }
+
+    rclcpp_action::CancelResponse GripperActionServer::GoalCancelledCallback(const std::shared_ptr<GoalHandleGripperCommand> goal_handle)
+    {
+        AZ::Outcome<void, AZStd::string> cancelOutcome = AZ::Failure(AZStd::string("No gripper component found!"));
+        GripperRequestBus::EventResult(cancelOutcome, m_entityId, &GripperRequestBus::Events::CancelGripperCommand);
+
+        if (!cancelOutcome)
+        { // This will not happen in a simulation unless intentionally done for behavior validation
+            AZ_Trace("GripperActionServer", "Cancelling could not be accepted: %s\n", cancelOutcome.GetError().c_str());
+            return rclcpp_action::CancelResponse::REJECT;
+        }
+        return rclcpp_action::CancelResponse::ACCEPT;
+    }
+
+    void GripperActionServer::GoalAcceptedCallback(const std::shared_ptr<GoalHandleGripperCommand> goal_handle)
+    {
+        AZ_Trace("GripperActionServer", "Goal accepted!\n");
+        m_goalHandle = goal_handle;
+    }
+
+    void GripperActionServer::CancelGoal(std::shared_ptr<GripperCommand::Result> result)
+    {
+        AZ_Assert(m_goalHandle, "Invalid goal handle!");
+        if (m_goalHandle && m_goalHandle->is_canceling())
+        {
+            AZ_Trace("GripperActionServer", "Cancelling goal\n");
+            m_goalHandle->canceled(result);
+        }
+    }
+
+    void GripperActionServer::GoalSuccess(std::shared_ptr<GripperCommand::Result> result)
+    {
+        AZ_Assert(m_goalHandle, "Invalid goal handle!");
+        if (m_goalHandle && (m_goalHandle->is_executing() || m_goalHandle->is_canceling()))
+        {
+            AZ_Trace("GripperActionServer", "Goal succeeded\n");
+            m_goalHandle->succeed(result);
+        }
+    }
+
+    void GripperActionServer::PublishFeedback(std::shared_ptr<GripperCommand::Feedback> feedback)
+    {
+        AZ_Assert(m_goalHandle, "Invalid goal handle!");
+        if (m_goalHandle && m_goalHandle->is_executing())
+        {
+            m_goalHandle->publish_feedback(feedback);
+        }
+    }
+
+} // namespace ROS2

Gems/ROS2/Code/Source/Gripper/GripperActionServer.h

@@ -0,0 +1,63 @@
+/*
+ * 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/Component/Component.h>
+#include <AzCore/Component/TickBus.h>
+#include <ROS2/Gripper/GripperRequestBus.h>
+#include <control_msgs/action/gripper_command.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+
+namespace ROS2
+{
+    //! GripperActionServer is a class responsible for managing an action server controlling a gripper
+    //! @see <a href="https://docs.ros.org/en/humble/p/rclcpp_action/generated/classrclcpp__action_1_1Server.html"> ROS 2 action
+    //! server documentation </a>
+    //! @see <a href="https://docs.ros.org/en/api/control_msgs/html/msg/GripperCommand.html"> GripperCommand documentation </a>
+    class GripperActionServer
+    {
+    public:
+        using GripperCommand = control_msgs::action::GripperCommand;
+        using GoalHandleGripperCommand = rclcpp_action::ServerGoalHandle<control_msgs::action::GripperCommand>;
+
+        //! Create an action server for GripperAction action and bind Goal callbacks.
+        //! @param actionName Name of the action, similar to topic or service name.
+        //! @param entityId entity which will execute callbacks through GripperRequestBus.
+        GripperActionServer(const AZStd::string& actionName, const AZ::EntityId& entityId);
+
+        //! Cancel the current goal.
+        //! @param result Result to be passed to through action server to the client.
+        void CancelGoal(std::shared_ptr<GripperCommand::Result> result);
+
+        //! Report goal success to the action server.
+        //! @param result Result which contains success code.
+        void GoalSuccess(std::shared_ptr<GripperCommand::Result> result);
+
+        //! Publish feedback during an active action.
+        //! @param feedback An action feedback message informing about the progress.
+        void PublishFeedback(std::shared_ptr<GripperCommand::Feedback> feedback);
+
+        //! Check if the goal is in an active state
+        bool IsGoalActiveState() const;
+
+        //! Check if the goal is in a pending state
+        bool IsReadyForExecution() const;
+
+    private:
+        rclcpp_action::GoalResponse GoalReceivedCallback(
+            const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const GripperCommand::Goal> goal);
+        rclcpp_action::CancelResponse GoalCancelledCallback(const std::shared_ptr<GoalHandleGripperCommand> goal_handle);
+        void GoalAcceptedCallback(const std::shared_ptr<GoalHandleGripperCommand> goal_handle);
+
+        rclcpp_action::Server<GripperCommand>::SharedPtr actionServer;
+        std::shared_ptr<GoalHandleGripperCommand> m_goalHandle;
+        AZ::EntityId m_entityId; //! Entity that has target gripper component
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/Gripper/GripperActionServerComponent.cpp

@@ -0,0 +1,135 @@
+/*
+ * 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 "Utils.h"
+
+#include "GripperActionServerComponent.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <AzFramework/Components/TransformComponent.h>
+#include <ROS2/Frame/ROS2FrameComponent.h>
+#include <ROS2/ROS2GemUtilities.h>
+#include <ROS2/Utilities/ROS2Names.h>
+
+namespace ROS2
+{
+    void GripperActionServerComponent::Activate()
+    {
+        auto* ros2Frame = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(GetEntity());
+        AZ_Assert(ros2Frame, "Missing Frame Component!");
+        AZStd::string namespacedAction = ROS2Names::GetNamespacedName(ros2Frame->GetNamespace(), m_gripperActionServerName);
+        AZ_Printf("GripperActionServerComponent", "Creating Gripper Action Server: %s\n", namespacedAction.c_str());
+        m_gripperActionServer = AZStd::make_unique<GripperActionServer>(namespacedAction, GetEntityId());
+        AZ::TickBus::Handler::BusConnect();
+    }
+
+    void GripperActionServerComponent::Deactivate()
+    {
+        AZ::TickBus::Handler::BusDisconnect();
+        m_gripperActionServer.reset();
+    }
+
+    void GripperActionServerComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<GripperActionServerComponent, AZ::Component>()
+                ->Field("ActionServerName", &GripperActionServerComponent::m_gripperActionServerName)
+                ->Version(1);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<GripperActionServerComponent>("GripperActionServerComponent", "Component for the gripper action server")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "GripperActionServer")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &GripperActionServerComponent::m_gripperActionServerName,
+                        "Gripper Action Server",
+                        "Action name for the gripper server.");
+            }
+        }
+    }
+
+    void GripperActionServerComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
+    {
+        required.push_back(AZ_CRC_CE("ROS2Frame"));
+        required.push_back(AZ_CRC_CE("GripperService"));
+    }
+    void GripperActionServerComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
+    {
+        provided.push_back(AZ_CRC_CE("GripperServerService"));
+    }
+    void GripperActionServerComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible)
+    {
+        incompatible.push_back(AZ_CRC_CE("GripperServerService"));
+    }
+
+    std::shared_ptr<GripperActionServer::GripperCommand::Feedback> GripperActionServerComponent::ProduceFeedback() const
+    {
+        auto feedback = std::make_shared<GripperCommand::Feedback>();
+        float position = 0.0f;
+        float effort = 0.0f;
+        GripperRequestBus::EventResult(position, GetEntityId(), &GripperRequestBus::Events::GetGripperPosition);
+        GripperRequestBus::EventResult(effort, GetEntityId(), &GripperRequestBus::Events::GetGripperEffort);
+        feedback->position = position;
+        feedback->effort = effort;
+        feedback->reached_goal = false;
+        feedback->stalled = false;
+        return feedback;
+    }
+
+    std::shared_ptr<GripperActionServer::GripperCommand::Result> GripperActionServerComponent::ProduceResult() const
+    {
+        auto result = std::make_shared<GripperCommand::Result>();
+        float position = 0.0f;
+        float effort = 0.0f;
+        bool stalled = false;
+        bool reachedGoal = false;
+        GripperRequestBus::EventResult(position, GetEntityId(), &GripperRequestBus::Events::GetGripperPosition);
+        GripperRequestBus::EventResult(effort, GetEntityId(), &GripperRequestBus::Events::GetGripperEffort);
+        GripperRequestBus::EventResult(stalled, GetEntityId(), &GripperRequestBus::Events::IsGripperNotMoving);
+        GripperRequestBus::EventResult(reachedGoal, GetEntityId(), &GripperRequestBus::Events::HasGripperReachedGoal);
+        result->position = position;
+        result->position = effort;
+        result->reached_goal = reachedGoal;
+        result->stalled = stalled;
+        return result;
+    }
+
+    void GripperActionServerComponent::OnTick(float delta, AZ::ScriptTimePoint timePoint)
+    {
+        AZ_Assert(m_gripperActionServer, "GripperActionServer::OnTick: GripperActionServer is null!");
+        if (!m_gripperActionServer->IsGoalActiveState())
+        {
+            return;
+        }
+        bool isDone = false;
+        bool isStalled;
+        bool isCancelled = false;
+        GripperRequestBus::EventResult(isDone, GetEntityId(), &GripperRequestBus::Events::HasGripperReachedGoal);
+        GripperRequestBus::EventResult(isStalled, GetEntityId(), &GripperRequestBus::Events::IsGripperNotMoving);
+        GripperRequestBus::EventResult(isCancelled, GetEntityId(), &GripperRequestBus::Events::HasGripperCommandBeenCancelled);
+        if (isCancelled)
+        {
+            m_gripperActionServer->CancelGoal(ProduceResult());
+            return;
+        }
+        if (isDone || isStalled)
+        {
+            AZ_Printf("GripperActionServer::OnTick", "GripperActionServer::OnTick: Gripper reached goal!");
+            m_gripperActionServer->GoalSuccess(ProduceResult());
+            return;
+        }
+        m_gripperActionServer->PublishFeedback(ProduceFeedback());
+        return;
+    }
+
+} // namespace ROS2

Gems/ROS2/Code/Source/Gripper/GripperActionServerComponent.h

@@ -0,0 +1,50 @@
+/*
+ * 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 "GripperActionServer.h"
+#include <AzCore/Component/Component.h>
+#include <AzCore/Component/TickBus.h>
+#include <ROS2/Gripper/GripperRequestBus.h>
+#include <control_msgs/action/gripper_command.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+namespace ROS2
+{
+    //! GripperActionServerComponent is a component that encapsulates gripper action server.
+    //! It is responsible for creating and managing the action server, producing feedback and result.
+    class GripperActionServerComponent
+        : public AZ::Component
+        , private AZ::TickBus::Handler
+    {
+    public:
+        using GripperCommand = control_msgs::action::GripperCommand;
+        using GoalHandleGripperCommand = rclcpp_action::ServerGoalHandle<control_msgs::action::GripperCommand>;
+        AZ_COMPONENT(GripperActionServerComponent, "{6A4417AC-1D85-4AB0-A116-1E77D40FC816}", AZ::Component);
+        GripperActionServerComponent() = default;
+        ~GripperActionServerComponent() = default;
+        static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
+        static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided);
+        static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible);
+        static void Reflect(AZ::ReflectContext* context);
+
+    private:
+        // AZ::Component overrides...
+        void Activate() override;
+        void Deactivate() override;
+
+        // AZ::TickBus::Handler overrides
+        void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
+
+        std::shared_ptr<GripperActionServer::GripperCommand::Feedback> ProduceFeedback() const;
+        std::shared_ptr<GripperActionServer::GripperCommand::Result> ProduceResult() const;
+        AZStd::string m_gripperActionServerName{ "gripper_server" }; //! name of the GripperCommand action server
+        AZStd::unique_ptr<GripperActionServer> m_gripperActionServer; //! action server for GripperCommand
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/Gripper/VacuumGripperComponent.cpp

@@ -0,0 +1,342 @@
+/*
+ * 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 "VacuumGripperComponent.h"
+#include "Source/ArticulationLinkComponent.h"
+#include "Utils.h"
+#include <Utilities/ArticulationsUtilities.h>
+
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <AzFramework/Components/TransformComponent.h>
+#include <AzFramework/Physics/PhysicsSystem.h>
+#include <AzFramework/Physics/RigidBodyBus.h>
+#include <AzFramework/Physics/SimulatedBodies/RigidBody.h>
+#include <imgui/imgui.h>
+
+namespace ROS2
+{
+    void VacuumGripperComponent::Activate()
+    {
+        m_gripperEffectorBodyHandle = AzPhysics::InvalidSimulatedBodyHandle;
+        m_onTriggerEnterHandler = AzPhysics::SimulatedBodyEvents::OnTriggerEnter::Handler(
+            [&]([[maybe_unused]] AzPhysics::SimulatedBodyHandle bodyHandle, [[maybe_unused]] const AzPhysics::TriggerEvent& event)
+            {
+                const auto grippedEntityCandidateId = event.m_otherBody->GetEntityId();
+                const bool isGrippable = isObjectGrippable(grippedEntityCandidateId);
+                if (isGrippable)
+                {
+                    m_grippedObjectInEffector = grippedEntityCandidateId;
+                }
+            });
+
+        m_onTriggerExitHandler = AzPhysics::SimulatedBodyEvents::OnTriggerExit::Handler(
+            [&]([[maybe_unused]] AzPhysics::SimulatedBodyHandle bodyHandle, [[maybe_unused]] const AzPhysics::TriggerEvent& event)
+            {
+                const auto grippedEntityCandidateId = event.m_otherBody->GetEntityId();
+                if (m_grippedObjectInEffector == grippedEntityCandidateId)
+                {
+                    m_grippedObjectInEffector = AZ::EntityId(AZ::EntityId::InvalidEntityId);
+                }
+            });
+
+        m_vacuumJoint = AzPhysics::InvalidJointHandle;
+        m_grippedObjectInEffector = AZ::EntityId(AZ::EntityId::InvalidEntityId);
+        m_tryingToGrip = false;
+        m_cancelGripperCommand = false;
+        AZ::TickBus::Handler::BusConnect();
+        ImGui::ImGuiUpdateListenerBus::Handler::BusConnect();
+        GripperRequestBus::Handler::BusConnect(GetEntityId());
+    }
+
+    void VacuumGripperComponent::Deactivate()
+    {
+        m_onTriggerEnterHandler.Disconnect();
+        m_onTriggerExitHandler.Disconnect();
+        GripperRequestBus::Handler::BusDisconnect();
+        AZ::TickBus::Handler::BusDisconnect();
+        ImGui::ImGuiUpdateListenerBus::Handler::BusDisconnect();
+    }
+
+    void VacuumGripperComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
+    {
+        provided.push_back(AZ_CRC_CE("GripperService"));
+    }
+    void VacuumGripperComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible)
+    {
+        incompatible.push_back(AZ_CRC_CE("GripperService"));
+    }
+
+    void VacuumGripperComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<VacuumGripperComponent, AZ::Component>()
+                ->Field("EffectorCollider", &VacuumGripperComponent::m_gripperEffectorCollider)
+                ->Field("EffectorArticulation", &VacuumGripperComponent::m_gripperEffectorArticulationLink)
+                ->Version(1);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<VacuumGripperComponent>("VacuumGripperComponent", "Component for control of a vacuum gripper.")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "VacuumGripper")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::EntityId,
+                        &VacuumGripperComponent::m_gripperEffectorCollider,
+                        "Effector Trigger Collider",
+                        "The entity with trigger collider that will detect objects that can be successfully gripped.")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::EntityId,
+                        &VacuumGripperComponent::m_gripperEffectorArticulationLink,
+                        "Effector Articulation Link",
+                        "The entity that is the articulation link of the effector.");
+            }
+        }
+    }
+
+    void VacuumGripperComponent::OnTick(float delta, AZ::ScriptTimePoint timePoint)
+    {
+        AzPhysics::SystemInterface* physicsSystem = AZ::Interface<AzPhysics::SystemInterface>::Get();
+        AZ_Assert(physicsSystem, "No physics system.");
+
+        AzPhysics::SceneInterface* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
+        AZ_Assert(sceneInterface, "No scene intreface.");
+
+        AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
+        AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle.");
+
+        // Connect the trigger handlers if not already connected, it is circumventing the issue GH-16188, the
+        // RigidbodyNotificationBus should be used instead.
+        if (!m_onTriggerEnterHandler.IsConnected() || !m_onTriggerExitHandler.IsConnected())
+        {
+            if (auto* physicsSystem = AZ::Interface<AzPhysics::SystemInterface>::Get())
+            {
+                AZStd::pair<AzPhysics::SceneHandle, AzPhysics::SimulatedBodyHandle> foundBody =
+                    physicsSystem->FindAttachedBodyHandleFromEntityId(m_gripperEffectorCollider);
+                AZ_Warning(
+                    "VacuumGripper", foundBody.first != AzPhysics::InvalidSceneHandle, "No body found for m_gripperEffectorCollider.");
+                if (foundBody.first != AzPhysics::InvalidSceneHandle)
+                {
+                    AzPhysics::SimulatedBodyEvents::RegisterOnTriggerEnterHandler(
+                        foundBody.first, foundBody.second, m_onTriggerEnterHandler);
+                    AzPhysics::SimulatedBodyEvents::RegisterOnTriggerExitHandler(foundBody.first, foundBody.second, m_onTriggerExitHandler);
+                }
+            }
+
+            AZ::EntityId rootArticulationEntity = Utils::GetRootOfArticulation(m_gripperEffectorArticulationLink);
+            AZ::Entity* rootEntity = nullptr;
+            AZ::ComponentApplicationBus::BroadcastResult(rootEntity, &AZ::ComponentApplicationRequests::FindEntity, rootArticulationEntity);
+
+            AZ_Trace("VacuumGripper", "Root articulation entity name: %s\n", rootEntity->GetName().c_str());
+
+            PhysX::ArticulationLinkComponent* component = rootEntity->FindComponent<PhysX::ArticulationLinkComponent>();
+            AZStd::vector<AzPhysics::SimulatedBodyHandle> articulationHandles = component->GetSimulatedBodyHandles();
+
+            AZ_Assert(articulationHandles.size() > 1, "Expected more than one body handles in articulations");
+            for (AzPhysics::SimulatedBodyHandle handle : articulationHandles)
+            {
+                AzPhysics::SimulatedBody* body = sceneInterface->GetSimulatedBodyFromHandle(defaultSceneHandle, handle);
+                AZ_Assert(body, "Expected valid body pointer");
+                if (body->GetEntityId() == m_gripperEffectorArticulationLink)
+                {
+                    m_gripperEffectorBodyHandle = handle;
+                }
+            }
+        }
+        if (m_tryingToGrip)
+        {
+            TryToGripObject();
+        }
+    }
+
+    bool VacuumGripperComponent::isObjectGrippable(const AZ::EntityId entityId)
+    {
+        bool isGrippable = false;
+        LmbrCentral::TagComponentRequestBus::EventResult(isGrippable, entityId, &LmbrCentral::TagComponentRequests::HasTag, GrippableTag);
+        return isGrippable;
+    }
+
+    bool VacuumGripperComponent::TryToGripObject()
+    {
+        AZ_Warning(
+            "VacuumGripper",
+            m_gripperEffectorBodyHandle != AzPhysics::InvalidSimulatedBodyHandle,
+            "Invalid body handle for gripperEffectorBody");
+        if (m_gripperEffectorBodyHandle == AzPhysics::InvalidSimulatedBodyHandle)
+        {
+            // No articulation link found
+            return true;
+        }
+        if (m_vacuumJoint != AzPhysics::InvalidJointHandle)
+        {
+            // Object is already gripped
+            return true;
+        }
+
+        if (!m_grippedObjectInEffector.IsValid())
+        {
+            // No object to grip
+            return false;
+        }
+        PhysX::ArticulationLinkComponent* component = m_entity->FindComponent<PhysX::ArticulationLinkComponent>();
+        AZ_Assert(component, "No PhysX::ArticulationLinkComponent found on entity ");
+
+        auto* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
+        AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
+        AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle");
+
+        // Get gripped rigid body
+        AzPhysics::RigidBody* grippedRigidBody = nullptr;
+        Physics::RigidBodyRequestBus::EventResult(grippedRigidBody, m_grippedObjectInEffector, &Physics::RigidBodyRequests::GetRigidBody);
+        AZ_Assert(grippedRigidBody, "No RigidBody found on entity grippedRigidBody");
+
+        // Gripper is the end of the articulation chain
+        AzPhysics::SimulatedBody* gripperBody = nullptr;
+        gripperBody = sceneInterface->GetSimulatedBodyFromHandle(defaultSceneHandle, m_gripperEffectorBodyHandle);
+        AZ_Assert(gripperBody, "No gripper body found");
+
+        AttachToGripper(gripperBody, grippedRigidBody, sceneInterface);
+
+        return true;
+    }
+
+    void VacuumGripperComponent::AttachToGripper(
+        AzPhysics::SimulatedBody* gripperBody, AzPhysics::RigidBody* grippedRigidBody, AzPhysics::SceneInterface* sceneInterface)
+    {
+        AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
+        AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle");
+
+        // Find Transform of the child in parent's frame
+        AZ::Transform childTransformWorld = grippedRigidBody->GetTransform();
+        AZ::Transform parentsTranformWorld = gripperBody->GetTransform();
+        AZ::Transform childTransformParent = parentsTranformWorld.GetInverse() * childTransformWorld;
+        childTransformParent.Invert();
+
+        // Configure new joint
+        PhysX::FixedJointConfiguration jointConfig;
+        jointConfig.m_debugName = "VacuumJoint";
+        jointConfig.m_parentLocalRotation = AZ::Quaternion::CreateIdentity();
+        jointConfig.m_parentLocalPosition = AZ::Vector3::CreateZero();
+        jointConfig.m_childLocalRotation = childTransformParent.GetRotation();
+        jointConfig.m_childLocalPosition = childTransformParent.GetTranslation();
+        jointConfig.m_startSimulationEnabled = true;
+
+        // Create new joint
+        m_vacuumJoint =
+            sceneInterface->AddJoint(defaultSceneHandle, &jointConfig, m_gripperEffectorBodyHandle, grippedRigidBody->m_bodyHandle);
+    }
+
+    void VacuumGripperComponent::ReleaseGrippedObject()
+    {
+        m_tryingToGrip = false;
+        if (m_vacuumJoint == AzPhysics::InvalidJointHandle)
+        {
+            return;
+        }
+        auto* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
+        AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
+        AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle");
+        // Wake up the body to prevent it from not moving after release
+        AzPhysics::RigidBody* grippedRigidBody = nullptr;
+        Physics::RigidBodyRequestBus::EventResult(grippedRigidBody, m_grippedObjectInEffector, &Physics::RigidBodyRequests::GetRigidBody);
+        if (grippedRigidBody)
+        {
+            grippedRigidBody->ForceAwake();
+        }
+        sceneInterface->RemoveJoint(defaultSceneHandle, m_vacuumJoint);
+        m_vacuumJoint = AzPhysics::InvalidJointHandle;
+    }
+
+    void VacuumGripperComponent::OnImGuiUpdate()
+    {
+        ImGui::Begin("VacuumGripperDebugger");
+
+        AZStd::string grippedObjectInEffectorName;
+        AZ::ComponentApplicationBus::BroadcastResult(
+            grippedObjectInEffectorName, &AZ::ComponentApplicationRequests::GetEntityName, m_grippedObjectInEffector);
+
+        ImGui::Text("Grippable object : %s", grippedObjectInEffectorName.c_str());
+        ImGui::Text("Vacuum joint created : %d ", m_vacuumJoint != AzPhysics::InvalidJointHandle);
+
+        ImGui::Checkbox("Gripping", &m_tryingToGrip);
+
+        if (ImGui::Button("Grip Command "))
+        {
+            m_tryingToGrip = true;
+        }
+
+        if (ImGui::Button("Release Command"))
+        {
+            ReleaseGrippedObject();
+        }
+        ImGui::End();
+    }
+
+    AZ::Outcome<void, AZStd::string> VacuumGripperComponent::GripperCommand(float position, float maxEffort)
+    {
+        AZ_Trace("VacuumGripper", "GripperCommand %f\n", position);
+        m_cancelGripperCommand = false;
+        if (position == 0.0f)
+        {
+            m_tryingToGrip = true;
+        }
+        else
+        {
+            ReleaseGrippedObject();
+        }
+        return AZ::Success();
+    }
+
+    AZ::Outcome<void, AZStd::string> VacuumGripperComponent::CancelGripperCommand()
+    {
+        ReleaseGrippedObject();
+        m_cancelGripperCommand = true;
+        return AZ::Success();
+    }
+
+    bool VacuumGripperComponent::HasGripperCommandBeenCancelled() const
+    {
+        if (m_cancelGripperCommand && m_vacuumJoint == AzPhysics::InvalidJointHandle)
+        {
+            return true;
+        }
+        return false;
+    }
+
+    float VacuumGripperComponent::GetGripperPosition() const
+    {
+        return m_tryingToGrip ? 0.0f : 1.0f;
+    }
+
+    float VacuumGripperComponent::GetGripperEffort() const
+    {
+        return m_vacuumJoint == AzPhysics::InvalidJointHandle ? 0.0f : 1.0f;
+    }
+    bool VacuumGripperComponent::IsGripperNotMoving() const
+    {
+        return true;
+    }
+
+    bool VacuumGripperComponent::HasGripperReachedGoal() const
+    {
+        const bool isObjectAttached = (m_vacuumJoint != AzPhysics::InvalidJointHandle);
+        if (m_tryingToGrip && isObjectAttached)
+        {
+            return true;
+        }
+        if (!m_tryingToGrip && !isObjectAttached)
+        {
+            return true;
+        }
+        return false;
+    }
+
+} // namespace ROS2

Gems/ROS2/Code/Source/Gripper/VacuumGripperComponent.h

@@ -0,0 +1,99 @@
+/*
+ * 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/Component/Component.h>
+#include <AzCore/Component/TickBus.h>
+#include <AzFramework/Physics/Common/PhysicsEvents.h>
+#include <AzFramework/Physics/Common/PhysicsSimulatedBodyEvents.h>
+#include <AzFramework/Physics/PhysicsSystem.h>
+#include <AzFramework/Physics/RigidBodyBus.h>
+#include <ImGuiBus.h>
+#include <LmbrCentral/Scripting/TagComponentBus.h>
+#include <ROS2/Gripper/GripperRequestBus.h>
+
+namespace ROS2
+{
+    //! This component implements vacuum gripper functionality.
+    //! It allows to attach to gripper objects that are in inside designated trigger collider to objects that has "Grippable" tag.
+    //! To use component:
+    //!  - Attach component to root of robot articulation.
+    //!  - Assign to m_gripperEffectorCollider EntityId of the collider that will be used as the gripper effector.
+    //!  - Add tag "Grippable" to objects that can be gripped.
+    class VacuumGripperComponent
+        : public AZ::Component
+        , public GripperRequestBus::Handler
+        , public ImGui::ImGuiUpdateListenerBus::Handler
+        , public AZ::TickBus::Handler
+    {
+    public:
+        static constexpr AZ::Crc32 GrippableTag = AZ_CRC_CE("Grippable");
+        AZ_COMPONENT(VacuumGripperComponent, "{a29eb4fa-0f6f-11ee-be56-0242ac120002}", AZ::Component);
+        VacuumGripperComponent() = default;
+        ~VacuumGripperComponent() = default;
+
+        // AZ::Component overrides...
+        void Activate() override;
+        void Deactivate() override;
+
+        static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided);
+        static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible);
+        static void Reflect(AZ::ReflectContext* context);
+
+    private:
+        // GripperRequestBus::Handler overrides...
+        AZ::Outcome<void, AZStd::string> GripperCommand(float position, float maxEffort) override;
+        AZ::Outcome<void, AZStd::string> CancelGripperCommand() override;
+        float GetGripperPosition() const override;
+        float GetGripperEffort() const override;
+        bool IsGripperNotMoving() const override;
+        bool HasGripperReachedGoal() const override;
+        bool HasGripperCommandBeenCancelled() const override;
+        // AZ::TickBus::Handler overrides...
+        void OnTick(float delta, AZ::ScriptTimePoint timePoint) override;
+
+        // ImGui::ImGuiUpdateListenerBus::Handler overrides...
+        void OnImGuiUpdate() override;
+
+        //! Entity that contains the collider that will be used as the gripper
+        //! effector/ The collider must be a trigger collider.
+        AZ::EntityId m_gripperEffectorCollider;
+
+        //! Entity that contains the articulation link that will be used as the gripper
+        AZ::EntityId m_gripperEffectorArticulationLink;
+
+        //! The physics body handle to m_gripperEffectorArticulationLink.
+        AzPhysics::SimulatedBodyHandle m_gripperEffectorBodyHandle;
+
+        //! EntityId of the object that is currently gripped by the gripper effector.
+        AZ::EntityId m_grippedObjectInEffector;
+
+        //! Handle to joint created by vacuum gripper.
+        AzPhysics::JointHandle m_vacuumJoint;
+
+        bool m_tryingToGrip{ false };
+
+        bool m_cancelGripperCommand{ false };
+
+        AzPhysics::SimulatedBodyEvents::OnTriggerEnter::Handler m_onTriggerEnterHandler;
+        AzPhysics::SimulatedBodyEvents::OnTriggerExit::Handler m_onTriggerExitHandler;
+
+        //! Checks if object is grippable (has Tag).
+        bool isObjectGrippable(const AZ::EntityId entityId);
+
+        //! Checks if an object is in the gripper effector collider and creates a joint between gripper effector and object.
+        bool TryToGripObject();
+
+        //! Releases object from gripper effector.
+        void ReleaseGrippedObject();
+
+        void AttachToGripper(
+            AzPhysics::SimulatedBody* gripperBody, AzPhysics::RigidBody* grippedRigidBody, AzPhysics::SceneInterface* sceneInterface);
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/Imu/ImuSensorConfiguration.cpp

@@ -0,0 +1,67 @@
+/*
+ * 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 "ImuSensorConfiguration.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/EditContextConstants.inl>
+
+namespace ROS2
+{
+    void ImuSensorConfiguration::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<ImuSensorConfiguration>()
+                ->Version(1)
+                ->Field("FilterSize", &ImuSensorConfiguration::m_filterSize)
+                ->Field("IncludeGravity", &ImuSensorConfiguration::m_includeGravity)
+                ->Field("AbsoluteRotation", &ImuSensorConfiguration::m_absoluteRotation)
+                ->Field("AccelerationVariance", &ImuSensorConfiguration::m_linearAccelerationVariance)
+                ->Field("AngularVelocityVariance", &ImuSensorConfiguration::m_angularVelocityVariance)
+                ->Field("OrientationVariance", &ImuSensorConfiguration::m_orientationVariance);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<ImuSensorConfiguration>("ROS2 IMU sensor configuration", "IMU sensor configuration")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Slider,
+                        &ImuSensorConfiguration::m_filterSize,
+                        "Filter Length",
+                        "Filter Length, Large value reduce numeric noise but increase lag")
+                    ->Attribute(AZ::Edit::Attributes::Max, &ImuSensorConfiguration::m_maxFilterSize)
+                    ->Attribute(AZ::Edit::Attributes::Min, &ImuSensorConfiguration::m_minFilterSize)
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &ImuSensorConfiguration::m_includeGravity,
+                        "Include Gravitation",
+                        "Enable accelerometer to observe gravity force.")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &ImuSensorConfiguration::m_absoluteRotation,
+                        "Absolute Rotation",
+                        "Include Absolute rotation in message.")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &ImuSensorConfiguration::m_linearAccelerationVariance,
+                        "Linear Acceleration Variance",
+                        "Variance of linear acceleration.")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &ImuSensorConfiguration::m_angularVelocityVariance,
+                        "Angular Velocity Variance",
+                        "Variance of angular velocity.")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &ImuSensorConfiguration::m_orientationVariance,
+                        "Orientation Variance",
+                        "Variance of orientation.");
+            }
+        }
+    }
+
+} // namespace ROS2

Gems/ROS2/Code/Source/Imu/ImuSensorConfiguration.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
+ *
+ */
+#pragma once
+
+#include <AzCore/Math/Matrix3x3.h>
+#include <AzCore/RTTI/RTTI.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <AzCore/std/string/string.h>
+
+namespace ROS2
+{
+    //! A structure capturing configuration of a IMU sensor.
+    struct ImuSensorConfiguration
+    {
+        AZ_TYPE_INFO(ImuSensorConfiguration, "{6788e84f-b985-4413-8e2b-46fbfb667c95}");
+        static void Reflect(AZ::ReflectContext* context);
+
+        //! Length of filter that removes numerical noise
+        int m_filterSize = 10;
+        int m_minFilterSize = 1;
+        int m_maxFilterSize = 200;
+
+        //! Include gravity acceleration
+        bool m_includeGravity = true;
+
+        //! Measure also absolute rotation
+        bool m_absoluteRotation = true;
+
+        AZ::Vector3 m_orientationVariance = AZ::Vector3::CreateZero();
+        AZ::Vector3 m_angularVelocityVariance = AZ::Vector3::CreateZero();
+        AZ::Vector3 m_linearAccelerationVariance = AZ::Vector3::CreateZero();
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp

@@ -31,16 +31,12 @@ namespace ROS2
 
     void ROS2ImuSensorComponent::Reflect(AZ::ReflectContext* context)
     {
+        ImuSensorConfiguration::Reflect(context);
+
         if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
         {
-            serialize->Class<ROS2ImuSensorComponent, ROS2SensorComponent>()
-                ->Version(1)
-                ->Field("FilterSize", &ROS2ImuSensorComponent::m_filterSize)
-                ->Field("IncludeGravity", &ROS2ImuSensorComponent::m_includeGravity)
-                ->Field("AbsoluteRotation", &ROS2ImuSensorComponent::m_absoluteRotation)
-                ->Field("AccelerationVariance", &ROS2ImuSensorComponent::m_linearAccelerationVariance)
-                ->Field("AngularVelocityVariance", &ROS2ImuSensorComponent::m_angularVelocityVariance)
-                ->Field("OrientationVariance", &ROS2ImuSensorComponent::m_orientationVariance);
+            serialize->Class<ROS2ImuSensorComponent, ROS2SensorComponent>()->Version(2)->Field(
+                "imuSensorConfiguration", &ROS2ImuSensorComponent::m_imuConfiguration);
 
             if (AZ::EditContext* ec = serialize->GetEditContext())
             {
@@ -48,38 +44,12 @@ namespace ROS2
                     ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
                     ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
                     ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"))
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Slider,
-                        &ROS2ImuSensorComponent::m_filterSize,
-                        "Filter Length",
-                        "Filter Length, Large value reduce numeric noise but increase lag")
-                    ->Attribute(AZ::Edit::Attributes::Max, &ROS2ImuSensorComponent::m_maxFilterSize)
-                    ->Attribute(AZ::Edit::Attributes::Min, &ROS2ImuSensorComponent::m_minFilterSize)
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &ROS2ImuSensorComponent::m_includeGravity,
-                        "Include Gravitation",
-                        "Enable accelerometer to observe gravity force.")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &ROS2ImuSensorComponent::m_absoluteRotation,
-                        "Absolute Rotation",
-                        "Include Absolute rotation in message.")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &ROS2ImuSensorComponent::m_linearAccelerationVariance,
-                        "Linear Acceleration Variance",
-                        "Variance of linear acceleration.")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &ROS2ImuSensorComponent::m_angularVelocityVariance,
-                        "Angular Velocity Variance",
-                        "Variance of angular velocity.")
                     ->DataElement(
                         AZ::Edit::UIHandlers::Default,
-                        &ROS2ImuSensorComponent::m_orientationVariance,
-                        "Orientation Variance",
-                        "Variance of orientation.");
+                        &ROS2ImuSensorComponent::m_imuConfiguration,
+                        "Imu sensor configuration",
+                        "Imu sensor configuration")
+                    ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly);
             }
         }
     }
@@ -94,15 +64,32 @@ namespace ROS2
         m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(type, pc));
     }
 
+    ROS2ImuSensorComponent::ROS2ImuSensorComponent(
+        const SensorConfiguration& sensorConfiguration, const ImuSensorConfiguration& imuConfiguration)
+        : m_imuConfiguration(imuConfiguration)
+    {
+        m_sensorConfiguration = sensorConfiguration;
+    }
+
     void ROS2ImuSensorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
     {
-        required.push_back(AZ_CRC_CE("PhysicsRigidBodyService"));
+        required.push_back(AZ_CRC_CE("PhysicsDynamicRigidBodyService"));
         required.push_back(AZ_CRC_CE("ROS2Frame"));
     }
 
     void ROS2ImuSensorComponent::SetupRefreshLoop()
     {
-        InstallPhysicalCallback(m_entity->GetId());
+        InstallPhysicalCallback();
+    }
+
+    void ROS2ImuSensorComponent::OnPhysicsInitialization(AzPhysics::SceneHandle sceneHandle)
+    {
+        AzPhysics::RigidBody* rigidBody = nullptr;
+        AZ::EntityId entityId = GetEntityId();
+        Physics::RigidBodyRequestBus::EventResult(rigidBody, entityId, &Physics::RigidBodyRequests::GetRigidBody);
+        AZ_Assert(rigidBody, "Entity %s does not have rigid body.", entityId.ToString().c_str());
+
+        m_bodyHandle = rigidBody->m_bodyHandle;
     }
 
     void ROS2ImuSensorComponent::OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime)
@@ -117,7 +104,7 @@ namespace ROS2
         m_filterAcceleration.push_back(linearVelocity);
         const auto angularVelocity = inv.TransformVector(rigidbody->GetAngularVelocity());
         m_filterAngularVelocity.push_back(angularVelocity);
-        if (m_filterAcceleration.size() > m_filterSize)
+        if (m_filterAcceleration.size() > m_imuConfiguration.m_filterSize)
         {
             m_filterAcceleration.pop_front();
             m_filterAngularVelocity.pop_front();
@@ -135,16 +122,16 @@ namespace ROS2
 
             m_previousLinearVelocity = linearVelocityFilter;
             m_acceleration = -acc + angularRateFiltered.Cross(linearVelocityFilter);
-            if (m_includeGravity)
+            if (m_imuConfiguration.m_includeGravity)
             {
                 m_acceleration += inv.TransformVector(gravity);
             }
             m_imuMsg.linear_acceleration = ROS2Conversions::ToROS2Vector3(m_acceleration);
-            m_imuMsg.linear_acceleration_covariance =  ROS2Conversions::ToROS2Covariance(m_linearAccelerationCovariance);
+            m_imuMsg.linear_acceleration_covariance = ROS2Conversions::ToROS2Covariance(m_linearAccelerationCovariance);
             m_imuMsg.angular_velocity = ROS2Conversions::ToROS2Vector3(angularRateFiltered);
             m_imuMsg.angular_velocity_covariance = ROS2Conversions::ToROS2Covariance(m_angularVelocityCovariance);
 
-            if (m_absoluteRotation)
+            if (m_imuConfiguration.m_absoluteRotation)
             {
                 m_imuMsg.orientation = ROS2Conversions::ToROS2Quaternion(rigidbody->GetTransform().GetRotation());
                 m_imuMsg.orientation_covariance = ROS2Conversions::ToROS2Covariance(m_orientationCovariance);
@@ -162,11 +149,11 @@ namespace ROS2
         const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kImuMsgType];
         const auto fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic);
         m_imuPublisher = ros2Node->create_publisher<sensor_msgs::msg::Imu>(fullTopic.data(), publisherConfig.GetQoS());
-        
-        m_linearAccelerationCovariance = ToDiagonalCovarianceMatrix(m_linearAccelerationVariance);
-        m_angularVelocityCovariance = ToDiagonalCovarianceMatrix(m_angularVelocityVariance);
-        m_orientationCovariance = ToDiagonalCovarianceMatrix(m_orientationVariance);
-        
+
+        m_linearAccelerationCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_linearAccelerationVariance);
+        m_angularVelocityCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_angularVelocityVariance);
+        m_orientationCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_orientationVariance);
+
         ROS2SensorComponent::Activate();
     }
 

Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h

@@ -13,10 +13,12 @@
 #include <AzFramework/Physics/Common/PhysicsEvents.h>
 #include <AzFramework/Physics/PhysicsSystem.h>
 #include <ROS2/Sensor/ROS2SensorComponent.h>
-#include <Utilities/PhysicsCallbackHandler.h>
+#include <ROS2/Utilities/PhysicsCallbackHandler.h>
 #include <rclcpp/publisher.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 
+#include "ImuSensorConfiguration.h"
+
 namespace ROS2
 {
     //! An IMU (Inertial Measurement Unit) sensor Component.
@@ -29,6 +31,7 @@ namespace ROS2
     public:
         AZ_COMPONENT(ROS2ImuSensorComponent, "{502A955E-7742-4E23-AD77-5E4063739DCA}", ROS2SensorComponent);
         ROS2ImuSensorComponent();
+        ROS2ImuSensorComponent(const SensorConfiguration& sensorConfiguration, const ImuSensorConfiguration& imuConfiguration);
         ~ROS2ImuSensorComponent() = default;
         static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
         static void Reflect(AZ::ReflectContext* context);
@@ -39,17 +42,6 @@ namespace ROS2
         //////////////////////////////////////////////////////////////////////////
 
     private:
-        //! Length of filter that removes numerical noise
-        int m_filterSize{ 10 };
-        int m_minFilterSize{ 1 };
-        int m_maxFilterSize{ 200 };
-
-        //! Include gravity acceleration
-        bool m_includeGravity{ true };
-
-        //! Measure also absolute rotation
-        bool m_absoluteRotation{ true };
-
         std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::Imu>> m_imuPublisher;
         sensor_msgs::msg::Imu m_imuMsg;
         AZ::Vector3 m_previousLinearVelocity = AZ::Vector3::CreateZero();
@@ -58,10 +50,7 @@ namespace ROS2
         AZStd::deque<AZ::Vector3> m_filterAcceleration;
         AZStd::deque<AZ::Vector3> m_filterAngularVelocity;
 
-        AZ::Vector3 m_orientationVariance = AZ::Vector3::CreateZero();
-        AZ::Vector3 m_angularVelocityVariance = AZ::Vector3::CreateZero();
-        AZ::Vector3 m_linearAccelerationVariance = AZ::Vector3::CreateZero();
-
+        ImuSensorConfiguration m_imuConfiguration;
 
         AZ::Matrix3x3 m_orientationCovariance = AZ::Matrix3x3::CreateZero();
         AZ::Matrix3x3 m_angularVelocityCovariance = AZ::Matrix3x3::CreateZero();
@@ -72,8 +61,12 @@ namespace ROS2
         void SetupRefreshLoop() override;
 
         // ROS2::Utils::PhysicsCallbackHandler overrides ...
+        void OnPhysicsInitialization(AzPhysics::SceneHandle sceneHandle) override;
         void OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime) override;
 
         AZ::Matrix3x3 ToDiagonalCovarianceMatrix(const AZ::Vector3& variance);
+
+        // Handle to the simulated physical body
+        AzPhysics::SimulatedBodyHandle m_bodyHandle = AzPhysics::InvalidSimulatedBodyHandle;
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.cpp

@@ -0,0 +1,131 @@
+/*
+ * 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 "Lidar2DSensorConfiguration.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/EditContextConstants.inl>
+
+namespace ROS2
+{
+    void Lidar2DSensorConfiguration::Reflect(AZ::ReflectContext* context)
+    {
+        if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serializeContext->Class<Lidar2DSensorConfiguration>()
+                ->Version(1)
+                ->Field("lidarModel", &Lidar2DSensorConfiguration::m_lidarModel)
+                ->Field("lidarImplementation", &Lidar2DSensorConfiguration::m_lidarSystem)
+                ->Field("LidarParameters", &Lidar2DSensorConfiguration::m_lidarParameters)
+                ->Field("IgnoredLayerIndices", &Lidar2DSensorConfiguration::m_ignoredCollisionLayers)
+                ->Field("ExcludedEntities", &Lidar2DSensorConfiguration::m_excludedEntities)
+                ->Field("PointsAtMax", &Lidar2DSensorConfiguration::m_addPointsAtMax);
+
+            if (AZ::EditContext* ec = serializeContext->GetEditContext())
+            {
+                ec->Class<Lidar2DSensorConfiguration>("ROS2 Lidar 2D Sensor configuration", "Lidar 2D sensor configuration")
+                    ->DataElement(AZ::Edit::UIHandlers::ComboBox, &Lidar2DSensorConfiguration::m_lidarModel, "Lidar Model", "Lidar model")
+                    ->Attribute(AZ::Edit::Attributes::ChangeNotify, &Lidar2DSensorConfiguration::OnLidarModelSelected)
+                    ->EnumAttribute(LidarTemplate::LidarModel::Custom2DLidar, "Custom Lidar 2D")
+                    ->EnumAttribute(LidarTemplate::LidarModel::Slamtec_RPLIDAR_S1, "Slamtec RPLIDAR S1")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::ComboBox,
+                        &Lidar2DSensorConfiguration::m_lidarSystem,
+                        "Lidar Implementation",
+                        "Select a lidar implementation out of registered ones.")
+                    ->Attribute(AZ::Edit::Attributes::ChangeNotify, &Lidar2DSensorConfiguration::OnLidarImplementationSelected)
+                    ->Attribute(AZ::Edit::Attributes::StringList, &Lidar2DSensorConfiguration::FetchLidarSystemList)
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::EntityId,
+                        &Lidar2DSensorConfiguration::m_lidarParameters,
+                        "Lidar parameters",
+                        "Configuration of Custom lidar")
+                    ->Attribute(AZ::Edit::Attributes::Visibility, &Lidar2DSensorConfiguration::IsConfigurationVisible)
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &Lidar2DSensorConfiguration::m_ignoredCollisionLayers,
+                        "Ignored collision layers",
+                        "Indices of collision layers to ignore")
+                    ->Attribute(AZ::Edit::Attributes::Visibility, &Lidar2DSensorConfiguration::IsIgnoredLayerConfigurationVisible)
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &Lidar2DSensorConfiguration::m_excludedEntities,
+                        "Excluded Entities",
+                        "List of entities excluded from raycasting.")
+                    ->Attribute(AZ::Edit::Attributes::AutoExpand, true)
+                    ->Attribute(AZ::Edit::Attributes::ContainerCanBeModified, true)
+                    ->Attribute(AZ::Edit::Attributes::Visibility, &Lidar2DSensorConfiguration::IsEntityExclusionVisible)
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &Lidar2DSensorConfiguration::m_addPointsAtMax,
+                        "Points at Max",
+                        "If set true LiDAR will produce points at max range for free space")
+                    ->Attribute(AZ::Edit::Attributes::Visibility, &Lidar2DSensorConfiguration::IsMaxPointsConfigurationVisible);
+            }
+        }
+    }
+
+    void Lidar2DSensorConfiguration::FetchLidarImplementationFeatures()
+    {
+        if (m_lidarSystem.empty())
+        {
+            m_lidarSystem = Details::GetDefaultLidarSystem();
+        }
+        const auto* lidarMetaData = LidarRegistrarInterface::Get()->GetLidarSystemMetaData(m_lidarSystem);
+        AZ_Warning("Lidar2DSensorConfiguration", lidarMetaData, "No metadata for \"%s\"", m_lidarSystem.c_str());
+        if (lidarMetaData)
+        {
+            m_lidarSystemFeatures = LidarRegistrarInterface::Get()->GetLidarSystemMetaData(m_lidarSystem)->m_features;
+        }
+    }
+
+    bool Lidar2DSensorConfiguration::IsConfigurationVisible() const
+    {
+        return m_lidarModel == LidarTemplate::LidarModel::Custom2DLidar;
+    }
+
+    bool Lidar2DSensorConfiguration::IsIgnoredLayerConfigurationVisible() const
+    {
+        return m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers;
+    }
+
+    bool Lidar2DSensorConfiguration::IsEntityExclusionVisible() const
+    {
+        return m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion;
+    }
+
+    bool Lidar2DSensorConfiguration::IsMaxPointsConfigurationVisible() const
+    {
+        return m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints;
+    }
+
+    AZ::Crc32 Lidar2DSensorConfiguration::OnLidarModelSelected()
+    {
+        m_lidarParameters = LidarTemplateUtils::GetTemplate(m_lidarModel);
+        UpdateShowNoise();
+        return AZ::Edit::PropertyRefreshLevels::EntireTree;
+    }
+
+    AZ::Crc32 Lidar2DSensorConfiguration::OnLidarImplementationSelected()
+    {
+        FetchLidarImplementationFeatures();
+        UpdateShowNoise();
+        return AZ::Edit::PropertyRefreshLevels::EntireTree;
+    }
+
+    AZStd::vector<AZStd::string> Lidar2DSensorConfiguration::FetchLidarSystemList()
+    {
+        FetchLidarImplementationFeatures();
+        UpdateShowNoise();
+        return LidarRegistrarInterface::Get()->GetRegisteredLidarSystems();
+    }
+
+    void Lidar2DSensorConfiguration::UpdateShowNoise()
+    {
+        m_lidarParameters.m_showNoiseConfig = m_lidarSystemFeatures & LidarSystemFeatures::Noise;
+    }
+} // namespace ROS2

Gems/ROS2/Code/Source/Lidar/Lidar2DSensorConfiguration.h

@@ -0,0 +1,53 @@
+/*
+ * 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/Component/EntityId.h>
+#include <AzCore/RTTI/RTTI.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <AzCore/std/string/string.h>
+
+#include "LidarRegistrarSystemComponent.h"
+#include "LidarTemplate.h"
+#include "LidarTemplateUtils.h"
+
+namespace ROS2
+{
+    //! A structure capturing configuration of a lidar sensor (to be used with ROS2Lidar2DSensorComponent).
+    class Lidar2DSensorConfiguration
+    {
+    public:
+        AZ_TYPE_INFO(Lidar2DSensorConfiguration, "{ed89dd3c-81ef-4636-9907-7ea641f8fbb0}");
+        static void Reflect(AZ::ReflectContext* context);
+
+        void FetchLidarImplementationFeatures();
+
+        LidarSystemFeatures m_lidarSystemFeatures;
+
+        AZStd::string m_lidarSystem;
+        LidarTemplate::LidarModel m_lidarModel = LidarTemplate::LidarModel::Custom2DLidar;
+        LidarTemplate m_lidarParameters = LidarTemplateUtils::GetTemplate(LidarTemplate::LidarModel::Custom2DLidar);
+
+        AZStd::unordered_set<AZ::u32> m_ignoredCollisionLayers;
+        AZStd::vector<AZ::EntityId> m_excludedEntities;
+
+        bool m_addPointsAtMax = false;
+
+    private:
+        bool IsConfigurationVisible() const;
+        bool IsIgnoredLayerConfigurationVisible() const;
+        bool IsEntityExclusionVisible() const;
+        bool IsMaxPointsConfigurationVisible() const;
+
+        AZ::Crc32 OnLidarModelSelected();
+        AZ::Crc32 OnLidarImplementationSelected();
+        AZStd::vector<AZStd::string> FetchLidarSystemList();
+
+        void UpdateShowNoise();
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/Lidar/LidarRaycaster.cpp

@@ -43,11 +43,12 @@ namespace ROS2
         : m_busId{ lidarRaycaster.m_busId }
         , m_sceneEntityId{ lidarRaycaster.m_sceneEntityId }
         , m_sceneHandle{ lidarRaycaster.m_sceneHandle }
+        , m_resultFlags{ lidarRaycaster.m_resultFlags }
+        , m_minRange{ lidarRaycaster.m_minRange }
         , m_range{ lidarRaycaster.m_range }
         , m_addMaxRangePoints{ lidarRaycaster.m_addMaxRangePoints }
         , m_rayRotations{ AZStd::move(lidarRaycaster.m_rayRotations) }
-        , m_ignoreLayer{ lidarRaycaster.m_ignoreLayer }
-        , m_ignoredLayerIndex{ lidarRaycaster.m_ignoredLayerIndex }
+        , m_ignoredCollisionLayers{ lidarRaycaster.m_ignoredCollisionLayers }
     {
         lidarRaycaster.BusDisconnect();
         lidarRaycaster.m_busId = LidarId::CreateNull();
@@ -97,18 +98,17 @@ namespace ROS2
             request->m_direction = direction;
             request->m_distance = m_range;
             request->m_reportMultipleHits = false;
-            request->m_filterCallback = [ignoredLayerIndex = this->m_ignoredLayerIndex, ignoreLayer = this->m_ignoreLayer](
-                                            const AzPhysics::SimulatedBody* simBody, const Physics::Shape* shape)
+
+            request->m_filterCallback =
+                [ignoredCollisionLayers = this->m_ignoredCollisionLayers](const AzPhysics::SimulatedBody* simBody, const Physics::Shape* shape)
             {
-                if (ignoreLayer && (shape->GetCollisionLayer().GetIndex() == ignoredLayerIndex))
+                if (ignoredCollisionLayers.contains(shape->GetCollisionLayer().GetIndex()))
                 {
                     return AzPhysics::SceneQuery::QueryHitType::None;
                 }
-                else
-                {
-                    return AzPhysics::SceneQuery::QueryHitType::Block;
-                }
+                return AzPhysics::SceneQuery::QueryHitType::Block;
             };
+
             requests.emplace_back(AZStd::move(request));
         }
         return requests;
@@ -125,7 +125,6 @@ namespace ROS2
         }
 
         const AZStd::vector<AZ::Vector3> rayDirections = LidarTemplateUtils::RotationsToDirections(m_rayRotations, lidarTransform);
-
         AzPhysics::SceneQueryRequests requests = prepareRequests(lidarTransform, rayDirections);
 
         RaycastResult results;
@@ -177,10 +176,9 @@ namespace ROS2
         return results;
     }
 
-    void LidarRaycaster::ConfigureLayerIgnoring(bool ignoreLayer, AZ::u32 layerIndex)
+    void LidarRaycaster::ConfigureIgnoredCollisionLayers(const AZStd::unordered_set<AZ::u32>& layerIndices)
     {
-        m_ignoreLayer = ignoreLayer;
-        m_ignoredLayerIndex = layerIndex;
+        m_ignoredCollisionLayers = layerIndices;
     }
     void LidarRaycaster::ConfigureMaxRangePointAddition(bool addMaxRangePoints)
     {

Gems/ROS2/Code/Source/Lidar/LidarRaycaster.h

@@ -33,7 +33,7 @@ namespace ROS2
 
         RaycastResult PerformRaycast(const AZ::Transform& lidarTransform) override;
 
-        void ConfigureLayerIgnoring(bool ignoreLayer, AZ::u32 layerIndex) override;
+        void ConfigureIgnoredCollisionLayers(const AZStd::unordered_set<AZ::u32>& layerIndices) override;
         void ConfigureMaxRangePointAddition(bool addMaxRangePoints) override;
 
     private:
@@ -50,7 +50,6 @@ namespace ROS2
         bool m_addMaxRangePoints{ false };
         AZStd::vector<AZ::Vector3> m_rayRotations{ { AZ::Vector3::CreateZero() } };
 
-        bool m_ignoreLayer{ false };
-        AZ::u32 m_ignoredLayerIndex{ 0 };
+        AZStd::unordered_set<AZ::u32> m_ignoredCollisionLayers;
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.cpp

@@ -0,0 +1,135 @@
+/*
+ * 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 "LidarSensorConfiguration.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/EditContextConstants.inl>
+
+namespace ROS2
+{
+    void LidarSensorConfiguration::Reflect(AZ::ReflectContext* context)
+    {
+        if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serializeContext->Class<LidarSensorConfiguration>()
+                ->Version(1)
+                ->Field("lidarModel", &LidarSensorConfiguration::m_lidarModel)
+                ->Field("lidarImplementation", &LidarSensorConfiguration::m_lidarSystem)
+                ->Field("LidarParameters", &LidarSensorConfiguration::m_lidarParameters)
+                ->Field("IgnoredLayerIndices", &LidarSensorConfiguration::m_ignoredCollisionLayers)
+                ->Field("ExcludedEntities", &LidarSensorConfiguration::m_excludedEntities)
+                ->Field("PointsAtMax", &LidarSensorConfiguration::m_addPointsAtMax);
+
+            if (AZ::EditContext* ec = serializeContext->GetEditContext())
+            {
+                ec->Class<LidarSensorConfiguration>("ROS2 Lidar Sensor configuration", "Lidar sensor configuration")
+                    ->DataElement(AZ::Edit::UIHandlers::ComboBox, &LidarSensorConfiguration::m_lidarModel, "Lidar Model", "Lidar model")
+                    ->Attribute(AZ::Edit::Attributes::ChangeNotify, &LidarSensorConfiguration::OnLidarModelSelected)
+                    ->EnumAttribute(LidarTemplate::LidarModel::Custom3DLidar, "Custom Lidar")
+                    ->EnumAttribute(LidarTemplate::LidarModel::Ouster_OS0_64, "Ouster OS0-64")
+                    ->EnumAttribute(LidarTemplate::LidarModel::Ouster_OS1_64, "Ouster OS1-64")
+                    ->EnumAttribute(LidarTemplate::LidarModel::Ouster_OS2_64, "Ouster OS2-64")
+                    ->EnumAttribute(LidarTemplate::LidarModel::Velodyne_Puck, "Velodyne Puck (VLP-16)")
+                    ->EnumAttribute(LidarTemplate::LidarModel::Velodyne_HDL_32E, "Velodyne HDL-32E")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::ComboBox,
+                        &LidarSensorConfiguration::m_lidarSystem,
+                        "Lidar Implementation",
+                        "Select a lidar implementation out of registered ones.")
+                    ->Attribute(AZ::Edit::Attributes::ChangeNotify, &LidarSensorConfiguration::OnLidarImplementationSelected)
+                    ->Attribute(AZ::Edit::Attributes::StringList, &LidarSensorConfiguration::FetchLidarSystemList)
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::EntityId,
+                        &LidarSensorConfiguration::m_lidarParameters,
+                        "Lidar parameters",
+                        "Configuration of Custom lidar")
+                    ->Attribute(AZ::Edit::Attributes::Visibility, &LidarSensorConfiguration::IsConfigurationVisible)
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &LidarSensorConfiguration::m_ignoredCollisionLayers,
+                        "Ignored collision layers",
+                        "Indices of collision layers to ignore")
+                    ->Attribute(AZ::Edit::Attributes::Visibility, &LidarSensorConfiguration::IsIgnoredLayerConfigurationVisible)
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &LidarSensorConfiguration::m_excludedEntities,
+                        "Excluded Entities",
+                        "List of entities excluded from raycasting.")
+                    ->Attribute(AZ::Edit::Attributes::AutoExpand, true)
+                    ->Attribute(AZ::Edit::Attributes::ContainerCanBeModified, true)
+                    ->Attribute(AZ::Edit::Attributes::Visibility, &LidarSensorConfiguration::IsEntityExclusionVisible)
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &LidarSensorConfiguration::m_addPointsAtMax,
+                        "Points at Max",
+                        "If set true LiDAR will produce points at max range for free space")
+                    ->Attribute(AZ::Edit::Attributes::Visibility, &LidarSensorConfiguration::IsMaxPointsConfigurationVisible);
+            }
+        }
+    }
+
+    void LidarSensorConfiguration::FetchLidarImplementationFeatures()
+    {
+        if (m_lidarSystem.empty())
+        {
+            m_lidarSystem = Details::GetDefaultLidarSystem();
+        }
+        const auto* lidarMetaData = LidarRegistrarInterface::Get()->GetLidarSystemMetaData(m_lidarSystem);
+        AZ_Warning("LidarSensorConfiguration", lidarMetaData, "No metadata for \"%s\"", m_lidarSystem.c_str());
+        if (lidarMetaData)
+        {
+            m_lidarSystemFeatures = LidarRegistrarInterface::Get()->GetLidarSystemMetaData(m_lidarSystem)->m_features;
+        }
+    }
+
+    bool LidarSensorConfiguration::IsConfigurationVisible() const
+    {
+        return m_lidarModel == LidarTemplate::LidarModel::Custom3DLidar;
+    }
+
+    bool LidarSensorConfiguration::IsIgnoredLayerConfigurationVisible() const
+    {
+        return m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers;
+    }
+
+    bool LidarSensorConfiguration::IsEntityExclusionVisible() const
+    {
+        return m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion;
+    }
+
+    bool LidarSensorConfiguration::IsMaxPointsConfigurationVisible() const
+    {
+        return m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints;
+    }
+
+    AZ::Crc32 LidarSensorConfiguration::OnLidarModelSelected()
+    {
+        m_lidarParameters = LidarTemplateUtils::GetTemplate(m_lidarModel);
+        UpdateShowNoise();
+        return AZ::Edit::PropertyRefreshLevels::EntireTree;
+    }
+
+    AZ::Crc32 LidarSensorConfiguration::OnLidarImplementationSelected()
+    {
+        FetchLidarImplementationFeatures();
+        UpdateShowNoise();
+        return AZ::Edit::PropertyRefreshLevels::EntireTree;
+    }
+
+    AZStd::vector<AZStd::string> LidarSensorConfiguration::FetchLidarSystemList()
+    {
+        FetchLidarImplementationFeatures();
+        UpdateShowNoise();
+        return LidarRegistrarInterface::Get()->GetRegisteredLidarSystems();
+    }
+
+    void LidarSensorConfiguration::UpdateShowNoise()
+    {
+        m_lidarParameters.m_showNoiseConfig = m_lidarSystemFeatures & LidarSystemFeatures::Noise;
+    }
+} // namespace ROS2

Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.h

@@ -0,0 +1,53 @@
+/*
+ * 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/Component/EntityId.h>
+#include <AzCore/RTTI/RTTI.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <AzCore/std/string/string.h>
+
+#include "LidarRegistrarSystemComponent.h"
+#include "LidarTemplate.h"
+#include "LidarTemplateUtils.h"
+
+namespace ROS2
+{
+    //! A structure capturing configuration of a lidar sensor (to be used with ROS2LidarSensorComponent).
+    class LidarSensorConfiguration
+    {
+    public:
+        AZ_TYPE_INFO(LidarSensorConfiguration, "{e46e75f4-1e0e-48ca-a22f-43afc8f25101}");
+        static void Reflect(AZ::ReflectContext* context);
+
+        void FetchLidarImplementationFeatures();
+
+        LidarSystemFeatures m_lidarSystemFeatures;
+
+        AZStd::string m_lidarSystem;
+        LidarTemplate::LidarModel m_lidarModel = LidarTemplate::LidarModel::Custom3DLidar;
+        LidarTemplate m_lidarParameters = LidarTemplateUtils::GetTemplate(LidarTemplate::LidarModel::Custom3DLidar);
+
+        AZStd::unordered_set<AZ::u32> m_ignoredCollisionLayers;
+        AZStd::vector<AZ::EntityId> m_excludedEntities;
+
+        bool m_addPointsAtMax = false;
+
+    private:
+        bool IsConfigurationVisible() const;
+        bool IsIgnoredLayerConfigurationVisible() const;
+        bool IsEntityExclusionVisible() const;
+        bool IsMaxPointsConfigurationVisible() const;
+
+        AZ::Crc32 OnLidarModelSelected();
+        AZ::Crc32 OnLidarImplementationSelected();
+        AZStd::vector<AZStd::string> FetchLidarSystemList();
+
+        void UpdateShowNoise();
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/Lidar/LidarSystem.cpp

@@ -50,7 +50,12 @@ namespace ROS2
     LidarId LidarSystem::CreateLidar(AZ::EntityId lidarEntityId)
     {
         LidarId lidarId = LidarId::CreateRandom();
-        m_lidars.emplace_back(lidarId, lidarEntityId);
+        m_lidars.emplace(lidarId, LidarRaycaster(lidarId, lidarEntityId));
         return lidarId;
     }
+
+    void LidarSystem::DestroyLidar(LidarId lidarId)
+    {
+        m_lidars.erase(lidarId);
+    }
 } // namespace ROS2

Gems/ROS2/Code/Source/Lidar/LidarSystem.h

@@ -31,7 +31,8 @@ namespace ROS2
 
         // LidarSystemRequestBus overrides
         LidarId CreateLidar(AZ::EntityId lidarEntityId) override;
+        void DestroyLidar(LidarId lidarId) override;
 
-        AZStd::vector<LidarRaycaster> m_lidars;
+        AZStd::unordered_map<LidarId, LidarRaycaster> m_lidars;
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/Lidar/LidarTemplate.cpp

@@ -68,6 +68,7 @@ namespace ROS2
                 ->Field("Max vertical angle", &LidarTemplate::m_maxVAngle)
                 ->Field("Min range", &LidarTemplate::m_minRange)
                 ->Field("Max range", &LidarTemplate::m_maxRange)
+                ->Field("Enable Noise", &LidarTemplate::m_isNoiseEnabled)
                 ->Field("Noise Parameters", &LidarTemplate::m_noiseParameters);
 
             if (AZ::EditContext* ec = serializeContext->GetEditContext())
@@ -100,13 +101,25 @@ namespace ROS2
                     ->DataElement(AZ::Edit::UIHandlers::Default, &LidarTemplate::m_maxRange, "Max range", "Maximum beam range [m]")
                     ->Attribute(AZ::Edit::Attributes::Min, 0.001f)
                     ->Attribute(AZ::Edit::Attributes::Max, 1000.0f)
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &LidarTemplate::m_isNoiseEnabled,
+                        "Enable noise",
+                        "Enable the use of noise and it's configuration")
+                    ->Attribute(AZ::Edit::Attributes::Visibility, &LidarTemplate::m_showNoiseConfig)
+                    ->Attribute(AZ::Edit::Attributes::ChangeNotify, AZ::Edit::PropertyRefreshLevels::EntireTree)
                     ->DataElement(
                         AZ::Edit::UIHandlers::Default,
                         &LidarTemplate::m_noiseParameters,
                         "Noise parameters",
                         "Parameters for Noise Configuration")
-                    ->Attribute(AZ::Edit::Attributes::Visibility, &LidarTemplate::m_showNoiseConfig);
+                    ->Attribute(AZ::Edit::Attributes::Visibility, &LidarTemplate::IsNoiseConfigVisible);
             }
         }
     }
+
+    bool LidarTemplate::IsNoiseConfigVisible() const
+    {
+        return m_showNoiseConfig && m_isNoiseEnabled;
+    }
 } // namespace ROS2

Gems/ROS2/Code/Source/Lidar/LidarTemplate.h

@@ -46,7 +46,7 @@ namespace ROS2
             float m_angularNoiseStdDev = 0.0f;
             //! Distance noise standard deviation base value, in meters
             float m_distanceNoiseStdDevBase = 0.0f;
-            //! Distance noise standard deviation increase per meter distance travelled, in meters
+            //! Distance noise standard deviation increase per meter distance traveled, in meters
             float m_distanceNoiseStdDevRisePerMeter = 0.0f;
         };
 
@@ -72,9 +72,11 @@ namespace ROS2
         float m_maxRange = 0.0f;
 
         NoiseParameters m_noiseParameters;
+        bool m_isNoiseEnabled = true;
         bool m_showNoiseConfig = false;
 
     private:
         bool IsLayersVisible() const;
+        [[nodiscard]] bool IsNoiseConfigVisible() const;
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp

@@ -9,11 +9,11 @@
 #include <Atom/RPI.Public/AuxGeom/AuxGeomFeatureProcessorInterface.h>
 #include <Atom/RPI.Public/Scene.h>
 #include <AzFramework/Physics/PhysicsSystem.h>
+#include <Lidar/LidarRegistrarSystemComponent.h>
 #include <Lidar/ROS2Lidar2DSensorComponent.h>
 #include <ROS2/Frame/ROS2FrameComponent.h>
 #include <ROS2/ROS2Bus.h>
 #include <ROS2/Utilities/ROS2Names.h>
-#include <Lidar/LidarRegistrarSystemComponent.h>
 
 namespace ROS2
 {
@@ -24,17 +24,12 @@ namespace ROS2
 
     void ROS2Lidar2DSensorComponent::Reflect(AZ::ReflectContext* context)
     {
+        Lidar2DSensorConfiguration::Reflect(context);
+
         if (auto* serialize = azrtti_cast<AZ::SerializeContext*>(context))
         {
-            serialize->Class<ROS2Lidar2DSensorComponent, ROS2SensorComponent>()
-                ->Version(1)
-                ->Field("lidarModel", &ROS2Lidar2DSensorComponent::m_lidarModel)
-                ->Field("lidarImplementation", &ROS2Lidar2DSensorComponent::m_lidarSystem)
-                ->Field("LidarParameters", &ROS2Lidar2DSensorComponent::m_lidarParameters)
-                ->Field("IgnoreLayer", &ROS2Lidar2DSensorComponent::m_ignoreLayer)
-                ->Field("IgnoredLayerIndex", &ROS2Lidar2DSensorComponent::m_ignoredLayerIndex)
-                ->Field("ExcludedEntities", &ROS2Lidar2DSensorComponent::m_excludedEntities)
-                ->Field("PointsAtMax", &ROS2Lidar2DSensorComponent::m_addPointsAtMax);
+            serialize->Class<ROS2Lidar2DSensorComponent, ROS2SensorComponent>()->Version(2)->Field(
+                "lidarConfiguration", &ROS2Lidar2DSensorComponent::m_lidarConfiguration);
 
             if (AZ::EditContext* ec = serialize->GetEditContext())
             {
@@ -42,108 +37,20 @@ namespace ROS2
                     ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
                     ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
                     ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"))
-                    ->DataElement(AZ::Edit::UIHandlers::ComboBox, &ROS2Lidar2DSensorComponent::m_lidarModel, "Lidar Model", "Lidar model")
-                    ->Attribute(AZ::Edit::Attributes::ChangeNotify, &ROS2Lidar2DSensorComponent::OnLidarModelSelected)
-                    ->EnumAttribute(LidarTemplate::LidarModel::Custom2DLidar, "Custom Lidar 2D")
-                    ->EnumAttribute(LidarTemplate::LidarModel::Slamtec_RPLIDAR_S1, "Slamtec RPLIDAR S1")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::ComboBox,
-                        &ROS2Lidar2DSensorComponent::m_lidarSystem,
-                        "Lidar Implementation",
-                        "Select a lidar implementation out of registered ones.")
-                    ->Attribute(AZ::Edit::Attributes::ChangeNotify, &ROS2Lidar2DSensorComponent::OnLidarImplementationSelected)
-                    ->Attribute(AZ::Edit::Attributes::StringList, &ROS2Lidar2DSensorComponent::FetchLidarSystemList)
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::EntityId,
-                        &ROS2Lidar2DSensorComponent::m_lidarParameters,
-                        "Lidar parameters",
-                        "Configuration of Custom lidar")
-                    ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2Lidar2DSensorComponent::IsConfigurationVisible)
                     ->DataElement(
                         AZ::Edit::UIHandlers::ComboBox,
-                        &ROS2Lidar2DSensorComponent::m_ignoreLayer,
-                        "Ignore layer",
-                        "Should we ignore selected layer index")
-                    ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2Lidar2DSensorComponent::IsIgnoredLayerConfigurationVisible)
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &ROS2Lidar2DSensorComponent::m_ignoredLayerIndex,
-                        "Ignored layer index",
-                        "Layer index to ignore")
-                    ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2Lidar2DSensorComponent::IsIgnoredLayerConfigurationVisible)
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &ROS2Lidar2DSensorComponent::m_excludedEntities,
-                        "Excluded Entities",
-                        "List of entities excluded from raycasting.")
-                    ->Attribute(AZ::Edit::Attributes::AutoExpand, true)
-                    ->Attribute(AZ::Edit::Attributes::ContainerCanBeModified, true)
-                    ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2Lidar2DSensorComponent::IsEntityExclusionVisible)
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &ROS2Lidar2DSensorComponent::m_addPointsAtMax,
-                        "Points at Max",
-                        "If set true LiDAR will produce points at max range for free space")
-                    ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2Lidar2DSensorComponent::IsMaxPointsConfigurationVisible);
+                        &ROS2Lidar2DSensorComponent::m_lidarConfiguration,
+                        "Lidar configuration",
+                        "Lidar configuration")
+                    ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly);
             }
         }
     }
 
-    void ROS2Lidar2DSensorComponent::FetchLidarImplementationFeatures()
-    {
-        if (m_lidarSystem.empty())
-        {
-            m_lidarSystem = Details::GetDefaultLidarSystem();
-        }
-        const auto* lidarMetaData = LidarRegistrarInterface::Get()->GetLidarSystemMetaData(m_lidarSystem);
-        AZ_Warning("ROS2Lidar2DSensorComponent", lidarMetaData, "No metadata for \"%s\"", m_lidarSystem.c_str());
-        if (lidarMetaData)
-        {
-            m_lidarSystemFeatures = LidarRegistrarInterface::Get()->GetLidarSystemMetaData(m_lidarSystem)->m_features;
-        }
-    }
-
-    bool ROS2Lidar2DSensorComponent::IsConfigurationVisible() const
-    {
-        return m_lidarModel == LidarTemplate::LidarModel::Custom2DLidar;
-    }
-
-    bool ROS2Lidar2DSensorComponent::IsIgnoredLayerConfigurationVisible() const
-    {
-        return m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers;
-    }
-
-    bool ROS2Lidar2DSensorComponent::IsEntityExclusionVisible() const
-    {
-        return m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion;
-    }
-
-    bool ROS2Lidar2DSensorComponent::IsMaxPointsConfigurationVisible() const
-    {
-        return m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints;
-    }
-
-    AZStd::vector<AZStd::string> ROS2Lidar2DSensorComponent::FetchLidarSystemList()
-    {
-        FetchLidarImplementationFeatures();
-        return LidarRegistrarInterface::Get()->GetRegisteredLidarSystems();
-    }
-
-    AZ::Crc32 ROS2Lidar2DSensorComponent::OnLidarModelSelected()
-    {
-        m_lidarParameters = LidarTemplateUtils::GetTemplate(m_lidarModel);
-        return AZ::Edit::PropertyRefreshLevels::EntireTree;
-    }
-
-    AZ::Crc32 ROS2Lidar2DSensorComponent::OnLidarImplementationSelected()
-    {
-        FetchLidarImplementationFeatures();
-        return AZ::Edit::PropertyRefreshLevels::EntireTree;
-    }
-
     void ROS2Lidar2DSensorComponent::ConnectToLidarRaycaster()
     {
-        if (auto raycasterId = m_implementationToRaycasterMap.find(m_lidarSystem); raycasterId != m_implementationToRaycasterMap.end())
+        if (auto raycasterId = m_implementationToRaycasterMap.find(m_lidarConfiguration.m_lidarSystem);
+            raycasterId != m_implementationToRaycasterMap.end())
         {
             m_lidarRaycasterId = raycasterId->second;
             return;
@@ -151,43 +58,59 @@ namespace ROS2
 
         m_lidarRaycasterId = LidarId::CreateNull();
         LidarSystemRequestBus::EventResult(
-            m_lidarRaycasterId, AZ_CRC(m_lidarSystem), &LidarSystemRequestBus::Events::CreateLidar, GetEntityId());
+            m_lidarRaycasterId, AZ_CRC(m_lidarConfiguration.m_lidarSystem), &LidarSystemRequestBus::Events::CreateLidar, GetEntityId());
         AZ_Assert(!m_lidarRaycasterId.IsNull(), "Could not access selected Lidar System.");
 
-        m_implementationToRaycasterMap.emplace(m_lidarSystem, m_lidarRaycasterId);
+        m_implementationToRaycasterMap.emplace(m_lidarConfiguration.m_lidarSystem, m_lidarRaycasterId);
     }
 
     void ROS2Lidar2DSensorComponent::ConfigureLidarRaycaster()
     {
-        FetchLidarImplementationFeatures();
+        m_lidarConfiguration.FetchLidarImplementationFeatures();
         LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayOrientations, m_lastRotations);
         LidarRaycasterRequestBus::Event(
-            m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayRange, m_lidarParameters.m_maxRange);
+            m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayRange, m_lidarConfiguration.m_lidarParameters.m_maxRange);
         LidarRaycasterRequestBus::Event(
-            m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureMinimumRayRange, m_lidarParameters.m_minRange);
+            m_lidarRaycasterId,
+            &LidarRaycasterRequestBus::Events::ConfigureMinimumRayRange,
+            m_lidarConfiguration.m_lidarParameters.m_minRange);
+
+        if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::Noise &&
+            m_lidarConfiguration.m_lidarParameters.m_isNoiseEnabled)
+        {
+            LidarRaycasterRequestBus::Event(
+                m_lidarRaycasterId,
+                &LidarRaycasterRequestBus::Events::ConfigureNoiseParameters,
+                m_lidarConfiguration.m_lidarParameters.m_noiseParameters.m_angularNoiseStdDev,
+                m_lidarConfiguration.m_lidarParameters.m_noiseParameters.m_distanceNoiseStdDevBase,
+                m_lidarConfiguration.m_lidarParameters.m_noiseParameters.m_distanceNoiseStdDevRisePerMeter);
+        }
 
         RaycastResultFlags requestedFlags = RaycastResultFlags::Ranges;
-        if (m_sensorConfiguration.m_visualise)
+        if (m_sensorConfiguration.m_visualize)
         {
             requestedFlags |= RaycastResultFlags::Points;
         }
         LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRaycastResultFlags, requestedFlags);
 
-        if (m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers)
+        if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers)
         {
             LidarRaycasterRequestBus::Event(
-                m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureLayerIgnoring, m_ignoreLayer, m_ignoredLayerIndex);
+                m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureIgnoredCollisionLayers, m_lidarConfiguration.m_ignoredCollisionLayers);
         }
 
-        if (m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion)
+        if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion)
         {
-            LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ExcludeEntities, m_excludedEntities);
+            LidarRaycasterRequestBus::Event(
+                m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ExcludeEntities, m_lidarConfiguration.m_excludedEntities);
         }
 
-        if (m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints)
+        if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints)
         {
             LidarRaycasterRequestBus::Event(
-                m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureMaxRangePointAddition, m_addPointsAtMax);
+                m_lidarRaycasterId,
+                &LidarRaycasterRequestBus::Events::ConfigureMaxRangePointAddition,
+                m_lidarConfiguration.m_addPointsAtMax);
         }
     }
 
@@ -201,9 +124,16 @@ namespace ROS2
         m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(type, ls));
     }
 
-    void ROS2Lidar2DSensorComponent::Visualise()
+    ROS2Lidar2DSensorComponent::ROS2Lidar2DSensorComponent(
+        const SensorConfiguration& sensorConfiguration, const Lidar2DSensorConfiguration& lidarConfiguration)
+        : m_lidarConfiguration(lidarConfiguration)
     {
-        if (m_visualisationPoints.empty())
+        m_sensorConfiguration = sensorConfiguration;
+    }
+
+    void ROS2Lidar2DSensorComponent::Visualize()
+    {
+        if (m_visualizationPoints.empty())
         {
             return;
         }
@@ -212,8 +142,8 @@ namespace ROS2
         {
             const uint8_t pixelSize = 2;
             AZ::RPI::AuxGeomDraw::AuxGeomDynamicDrawArguments drawArgs;
-            drawArgs.m_verts = m_visualisationPoints.data();
-            drawArgs.m_vertCount = m_visualisationPoints.size();
+            drawArgs.m_verts = m_visualizationPoints.data();
+            drawArgs.m_vertCount = m_visualizationPoints.size();
             drawArgs.m_colors = &AZ::Colors::Red;
             drawArgs.m_colorCount = 1;
             drawArgs.m_opacityType = AZ::RPI::AuxGeomDraw::OpacityType::Opaque;
@@ -231,15 +161,15 @@ namespace ROS2
         AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic);
         m_laserScanPublisher = ros2Node->create_publisher<sensor_msgs::msg::LaserScan>(fullTopic.data(), publisherConfig.GetQoS());
 
-        if (m_sensorConfiguration.m_visualise)
+        if (m_sensorConfiguration.m_visualize)
         {
             auto* entityScene = AZ::RPI::Scene::GetSceneForEntityId(GetEntityId());
             m_drawQueue = AZ::RPI::AuxGeomFeatureProcessorInterface::GetDrawQueueForScene(entityScene);
         }
 
-        m_lastRotations = LidarTemplateUtils::PopulateRayRotations(m_lidarParameters);
+        m_lastRotations = LidarTemplateUtils::PopulateRayRotations(m_lidarConfiguration.m_lidarParameters);
 
-        FetchLidarImplementationFeatures();
+        m_lidarConfiguration.FetchLidarImplementationFeatures();
         ConnectToLidarRaycaster();
         ConfigureLidarRaycaster();
 
@@ -257,7 +187,7 @@ namespace ROS2
         auto entityTransform = GetEntity()->FindComponent<AzFramework::TransformComponent>();
 
         RaycastResultFlags requestedFlags = RaycastResultFlags::Ranges;
-        if (m_sensorConfiguration.m_visualise)
+        if (m_sensorConfiguration.m_visualize)
         {
             requestedFlags |= RaycastResultFlags::Points;
         }
@@ -269,28 +199,22 @@ namespace ROS2
             return;
         }
 
-        if (m_sensorConfiguration.m_visualise)
-        { // Store points for visualisation purposes, in global frame
-            m_visualisationPoints = m_lastScanResults.m_points;
-        }
-
-        // TODO(mzak): possibly unneccessary post-proc?
-        const auto inverseLidarTM = entityTransform->GetWorldTM().GetInverse();
-        for (auto& point : m_lastScanResults.m_points)
-        {
-            point = inverseLidarTM.TransformPoint(point);
+        if (m_sensorConfiguration.m_visualize)
+        { // Store points for visualization purposes, in global frame
+            m_visualizationPoints = m_lastScanResults.m_points;
         }
 
         auto* ros2Frame = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(GetEntity());
         auto message = sensor_msgs::msg::LaserScan();
         message.header.frame_id = ros2Frame->GetFrameID().data();
         message.header.stamp = ROS2Interface::Get()->GetROSTimestamp();
-        message.angle_min = AZ::DegToRad(m_lidarParameters.m_minHAngle);
-        message.angle_max = AZ::DegToRad(m_lidarParameters.m_maxHAngle);
-        message.angle_increment = (message.angle_max - message.angle_min) / aznumeric_cast<float>(m_lidarParameters.m_numberOfIncrements);
+        message.angle_min = AZ::DegToRad(m_lidarConfiguration.m_lidarParameters.m_minHAngle);
+        message.angle_max = AZ::DegToRad(m_lidarConfiguration.m_lidarParameters.m_maxHAngle);
+        message.angle_increment =
+            (message.angle_max - message.angle_min) / aznumeric_cast<float>(m_lidarConfiguration.m_lidarParameters.m_numberOfIncrements);
 
-        message.range_min = m_lidarParameters.m_minRange;
-        message.range_max = m_lidarParameters.m_maxRange;
+        message.range_min = m_lidarConfiguration.m_lidarParameters.m_minRange;
+        message.range_max = m_lidarConfiguration.m_lidarParameters.m_maxRange;
         message.scan_time = 1.f / m_sensorConfiguration.m_frequency;
         message.time_increment = 0.0f;
 

Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h

@@ -9,15 +9,15 @@
 
 #include <Atom/RPI.Public/AuxGeom/AuxGeomDraw.h>
 #include <AzCore/Serialization/SerializeContext.h>
-#include <Lidar/LidarRaycaster.h>
-#include <Lidar/LidarTemplate.h>
-#include <Lidar/LidarTemplateUtils.h>
 #include <ROS2/Lidar/LidarRegistrarBus.h>
 #include <ROS2/Lidar/LidarSystemBus.h>
 #include <ROS2/Sensor/ROS2SensorComponent.h>
 #include <rclcpp/publisher.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
 
+#include "Lidar2DSensorConfiguration.h"
+#include "LidarRaycaster.h"
+
 namespace ROS2
 {
     //! Lidar 2D sensor Component.
@@ -29,6 +29,7 @@ namespace ROS2
     public:
         AZ_COMPONENT(ROS2Lidar2DSensorComponent, "{F4C2D970-1D69-40F2-9D4D-B52DCFDD2704}", ROS2SensorComponent);
         ROS2Lidar2DSensorComponent();
+        ROS2Lidar2DSensorComponent(const SensorConfiguration& sensorConfiguration, const Lidar2DSensorConfiguration& lidarConfiguration);
         ~ROS2Lidar2DSensorComponent() = default;
         static void Reflect(AZ::ReflectContext* context);
         //////////////////////////////////////////////////////////////////////////
@@ -41,41 +42,23 @@ namespace ROS2
         //////////////////////////////////////////////////////////////////////////
         // ROS2SensorComponent overrides
         void FrequencyTick() override;
-        void Visualise() override;
-
-        bool IsConfigurationVisible() const;
-        bool IsIgnoredLayerConfigurationVisible() const;
-        bool IsEntityExclusionVisible() const;
-        bool IsMaxPointsConfigurationVisible() const;
+        void Visualize() override;
 
-        AZ::Crc32 OnLidarModelSelected();
-        AZ::Crc32 OnLidarImplementationSelected();
-        void FetchLidarImplementationFeatures();
-        AZStd::vector<AZStd::string> FetchLidarSystemList();
         void ConnectToLidarRaycaster();
         void ConfigureLidarRaycaster();
 
-        LidarSystemFeatures m_lidarSystemFeatures;
-        LidarTemplate::LidarModel m_lidarModel = LidarTemplate::LidarModel::Custom2DLidar;
-        LidarTemplate m_lidarParameters = LidarTemplateUtils::GetTemplate(LidarTemplate::LidarModel::Custom2DLidar);
-        AZStd::vector<AZ::Vector3> m_lastRotations;
-
-        AZStd::string m_lidarSystem;
         // A structure that maps each lidar implementation busId to the busId of a raycaster created by this LidarSensorComponent.
         AZStd::unordered_map<AZStd::string, LidarId> m_implementationToRaycasterMap;
         LidarId m_lidarRaycasterId;
         std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::LaserScan>> m_laserScanPublisher;
 
-        // Used only when visualisation is on - points differ since they are in global transform as opposed to local
-        AZStd::vector<AZ::Vector3> m_visualisationPoints;
+        // Used only when visualization is on - points differ since they are in global transform as opposed to local
+        AZStd::vector<AZ::Vector3> m_visualizationPoints;
         AZ::RPI::AuxGeomDrawPtr m_drawQueue;
 
-        RaycastResult m_lastScanResults;
-
-        AZ::u32 m_ignoredLayerIndex = 0;
-        bool m_ignoreLayer = false;
-        AZStd::vector<AZ::EntityId> m_excludedEntities;
+        Lidar2DSensorConfiguration m_lidarConfiguration;
 
-        bool m_addPointsAtMax = false;
+        AZStd::vector<AZ::Vector3> m_lastRotations;
+        RaycastResult m_lastScanResults;
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp

@@ -9,11 +9,11 @@
 #include <Atom/RPI.Public/AuxGeom/AuxGeomFeatureProcessorInterface.h>
 #include <Atom/RPI.Public/Scene.h>
 #include <AzFramework/Physics/PhysicsSystem.h>
+#include <Lidar/LidarRegistrarSystemComponent.h>
 #include <Lidar/ROS2LidarSensorComponent.h>
 #include <ROS2/Frame/ROS2FrameComponent.h>
 #include <ROS2/ROS2Bus.h>
 #include <ROS2/Utilities/ROS2Names.h>
-#include <Lidar/LidarRegistrarSystemComponent.h>
 
 namespace ROS2
 {
@@ -24,17 +24,12 @@ namespace ROS2
 
     void ROS2LidarSensorComponent::Reflect(AZ::ReflectContext* context)
     {
+        LidarSensorConfiguration::Reflect(context);
+
         if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
         {
-            serialize->Class<ROS2LidarSensorComponent, ROS2SensorComponent>()
-                ->Version(1)
-                ->Field("lidarModel", &ROS2LidarSensorComponent::m_lidarModel)
-                ->Field("lidarImplementation", &ROS2LidarSensorComponent::m_lidarSystem)
-                ->Field("LidarParameters", &ROS2LidarSensorComponent::m_lidarParameters)
-                ->Field("IgnoreLayer", &ROS2LidarSensorComponent::m_ignoreLayer)
-                ->Field("IgnoredLayerIndex", &ROS2LidarSensorComponent::m_ignoredLayerIndex)
-                ->Field("ExcludedEntities", &ROS2LidarSensorComponent::m_excludedEntities)
-                ->Field("PointsAtMax", &ROS2LidarSensorComponent::m_addPointsAtMax);
+            serialize->Class<ROS2LidarSensorComponent, ROS2SensorComponent>()->Version(2)->Field(
+                "lidarConfiguration", &ROS2LidarSensorComponent::m_lidarConfiguration);
 
             if (AZ::EditContext* ec = serialize->GetEditContext())
             {
@@ -42,112 +37,20 @@ namespace ROS2
                     ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
                     ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
                     ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"))
-                    ->DataElement(AZ::Edit::UIHandlers::ComboBox, &ROS2LidarSensorComponent::m_lidarModel, "Lidar Model", "Lidar model")
-                    ->Attribute(AZ::Edit::Attributes::ChangeNotify, &ROS2LidarSensorComponent::OnLidarModelSelected)
-                    ->EnumAttribute(LidarTemplate::LidarModel::Custom3DLidar, "Custom Lidar")
-                    ->EnumAttribute(LidarTemplate::LidarModel::Ouster_OS0_64, "Ouster OS0-64")
-                    ->EnumAttribute(LidarTemplate::LidarModel::Ouster_OS1_64, "Ouster OS1-64")
-                    ->EnumAttribute(LidarTemplate::LidarModel::Ouster_OS2_64, "Ouster OS2-64")
-                    ->EnumAttribute(LidarTemplate::LidarModel::Velodyne_Puck, "Velodyne Puck (VLP-16)")
-                    ->EnumAttribute(LidarTemplate::LidarModel::Velodyne_HDL_32E, "Velodyne HDL-32E")
                     ->DataElement(
                         AZ::Edit::UIHandlers::ComboBox,
-                        &ROS2LidarSensorComponent::m_lidarSystem,
-                        "Lidar Implementation",
-                        "Select a lidar implementation out of registered ones.")
-                    ->Attribute(AZ::Edit::Attributes::ChangeNotify, &ROS2LidarSensorComponent::OnLidarImplementationSelected)
-                    ->Attribute(AZ::Edit::Attributes::StringList, &ROS2LidarSensorComponent::FetchLidarSystemList)
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::EntityId,
-                        &ROS2LidarSensorComponent::m_lidarParameters,
-                        "Lidar parameters",
-                        "Configuration of Custom lidar")
-                    ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2LidarSensorComponent::IsConfigurationVisible)
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::ComboBox,
-                        &ROS2LidarSensorComponent::m_ignoreLayer,
-                        "Ignore layer",
-                        "Should we ignore selected layer index")
-                    ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2LidarSensorComponent::IsIgnoredLayerConfigurationVisible)
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &ROS2LidarSensorComponent::m_ignoredLayerIndex,
-                        "Ignored layer index",
-                        "Layer index to ignore")
-                    ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2LidarSensorComponent::IsIgnoredLayerConfigurationVisible)
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &ROS2LidarSensorComponent::m_excludedEntities,
-                        "Excluded Entities",
-                        "List of entities excluded from raycasting.")
-                    ->Attribute(AZ::Edit::Attributes::AutoExpand, true)
-                    ->Attribute(AZ::Edit::Attributes::ContainerCanBeModified, true)
-                    ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2LidarSensorComponent::IsEntityExclusionVisible)
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &ROS2LidarSensorComponent::m_addPointsAtMax,
-                        "Points at Max",
-                        "If set true LiDAR will produce points at max range for free space")
-                    ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2LidarSensorComponent::IsMaxPointsConfigurationVisible);
+                        &ROS2LidarSensorComponent::m_lidarConfiguration,
+                        "Lidar configuration",
+                        "Lidar configuration")
+                    ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly);
             }
         }
     }
 
-    void ROS2LidarSensorComponent::FetchLidarImplementationFeatures()
-    {
-        if (m_lidarSystem.empty())
-        {
-            m_lidarSystem = Details::GetDefaultLidarSystem();
-        }
-        const auto* lidarMetaData = LidarRegistrarInterface::Get()->GetLidarSystemMetaData(m_lidarSystem);
-        AZ_Warning("ROS2LidarSensorComponent", lidarMetaData, "No metadata for \"%s\"", m_lidarSystem.c_str());
-        if (lidarMetaData)
-        {
-            m_lidarSystemFeatures = LidarRegistrarInterface::Get()->GetLidarSystemMetaData(m_lidarSystem)->m_features;
-        }
-    }
-
-    bool ROS2LidarSensorComponent::IsConfigurationVisible() const
-    {
-        return m_lidarModel == LidarTemplate::LidarModel::Custom3DLidar;
-    }
-
-    bool ROS2LidarSensorComponent::IsIgnoredLayerConfigurationVisible() const
-    {
-        return m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers;
-    }
-
-    bool ROS2LidarSensorComponent::IsEntityExclusionVisible() const
-    {
-        return m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion;
-    }
-
-    bool ROS2LidarSensorComponent::IsMaxPointsConfigurationVisible() const
-    {
-        return m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints;
-    }
-
-    AZStd::vector<AZStd::string> ROS2LidarSensorComponent::FetchLidarSystemList()
-    {
-        FetchLidarImplementationFeatures();
-        return LidarRegistrarInterface::Get()->GetRegisteredLidarSystems();
-    }
-
-    AZ::Crc32 ROS2LidarSensorComponent::OnLidarModelSelected()
-    {
-        m_lidarParameters = LidarTemplateUtils::GetTemplate(m_lidarModel);
-        return AZ::Edit::PropertyRefreshLevels::EntireTree;
-    }
-
-    AZ::Crc32 ROS2LidarSensorComponent::OnLidarImplementationSelected()
-    {
-        FetchLidarImplementationFeatures();
-        return AZ::Edit::PropertyRefreshLevels::EntireTree;
-    }
-
     void ROS2LidarSensorComponent::ConnectToLidarRaycaster()
     {
-        if (auto raycasterId = m_implementationToRaycasterMap.find(m_lidarSystem); raycasterId != m_implementationToRaycasterMap.end())
+        if (auto raycasterId = m_implementationToRaycasterMap.find(m_lidarConfiguration.m_lidarSystem);
+            raycasterId != m_implementationToRaycasterMap.end())
         {
             m_lidarRaycasterId = raycasterId->second;
             return;
@@ -155,48 +58,67 @@ namespace ROS2
 
         m_lidarRaycasterId = LidarId::CreateNull();
         LidarSystemRequestBus::EventResult(
-            m_lidarRaycasterId, AZ_CRC(m_lidarSystem), &LidarSystemRequestBus::Events::CreateLidar, GetEntityId());
+            m_lidarRaycasterId, AZ_CRC(m_lidarConfiguration.m_lidarSystem), &LidarSystemRequestBus::Events::CreateLidar, GetEntityId());
         AZ_Assert(!m_lidarRaycasterId.IsNull(), "Could not access selected Lidar System.");
 
-        m_implementationToRaycasterMap.emplace(m_lidarSystem, m_lidarRaycasterId);
+        m_implementationToRaycasterMap.emplace(m_lidarConfiguration.m_lidarSystem, m_lidarRaycasterId);
     }
 
     void ROS2LidarSensorComponent::ConfigureLidarRaycaster()
     {
-        FetchLidarImplementationFeatures();
+        m_lidarConfiguration.FetchLidarImplementationFeatures();
         LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayOrientations, m_lastRotations);
         LidarRaycasterRequestBus::Event(
-            m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureMinimumRayRange, m_lidarParameters.m_minRange);
+            m_lidarRaycasterId,
+            &LidarRaycasterRequestBus::Events::ConfigureMinimumRayRange,
+            m_lidarConfiguration.m_lidarParameters.m_minRange);
         LidarRaycasterRequestBus::Event(
-            m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayRange, m_lidarParameters.m_maxRange);
+            m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayRange, m_lidarConfiguration.m_lidarParameters.m_maxRange);
         LidarRaycasterRequestBus::Event(
             m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRaycastResultFlags, RaycastResultFlags::Points);
 
-        if (m_lidarSystemFeatures & LidarSystemFeatures::Noise)
+        if ((m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::Noise) &&
+            m_lidarConfiguration.m_lidarParameters.m_isNoiseEnabled)
         {
             LidarRaycasterRequestBus::Event(
                 m_lidarRaycasterId,
                 &LidarRaycasterRequestBus::Events::ConfigureNoiseParameters,
-                m_lidarParameters.m_noiseParameters.m_angularNoiseStdDev,
-                m_lidarParameters.m_noiseParameters.m_distanceNoiseStdDevBase,
-                m_lidarParameters.m_noiseParameters.m_distanceNoiseStdDevRisePerMeter);
+                m_lidarConfiguration.m_lidarParameters.m_noiseParameters.m_angularNoiseStdDev,
+                m_lidarConfiguration.m_lidarParameters.m_noiseParameters.m_distanceNoiseStdDevBase,
+                m_lidarConfiguration.m_lidarParameters.m_noiseParameters.m_distanceNoiseStdDevRisePerMeter);
         }
 
-        if (m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers)
+        if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers)
         {
             LidarRaycasterRequestBus::Event(
-                m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureLayerIgnoring, m_ignoreLayer, m_ignoredLayerIndex);
+                m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureIgnoredCollisionLayers, m_lidarConfiguration.m_ignoredCollisionLayers);
         }
 
-        if (m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion)
+        if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion)
         {
-            LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ExcludeEntities, m_excludedEntities);
+            LidarRaycasterRequestBus::Event(
+                m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ExcludeEntities, m_lidarConfiguration.m_excludedEntities);
         }
 
-        if (m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints)
+        if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints)
         {
             LidarRaycasterRequestBus::Event(
-                m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureMaxRangePointAddition, m_addPointsAtMax);
+                m_lidarRaycasterId,
+                &LidarRaycasterRequestBus::Events::ConfigureMaxRangePointAddition,
+                m_lidarConfiguration.m_addPointsAtMax);
+        }
+
+        if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::PointcloudPublishing)
+        {
+            const TopicConfiguration& publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kPointCloudType];
+            auto* ros2Frame = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(GetEntity());
+
+            LidarRaycasterRequestBus::Event(
+                m_lidarRaycasterId,
+                &LidarRaycasterRequestBus::Events::ConfigurePointCloudPublisher,
+                ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic),
+                ros2Frame->GetFrameID().data(),
+                publisherConfig.GetQoS());
         }
     }
 
@@ -210,9 +132,16 @@ namespace ROS2
         m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(type, pc));
     }
 
-    void ROS2LidarSensorComponent::Visualise()
+    ROS2LidarSensorComponent::ROS2LidarSensorComponent(
+        const SensorConfiguration& sensorConfiguration, const LidarSensorConfiguration& lidarConfiguration)
+        : m_lidarConfiguration(lidarConfiguration)
+    {
+        m_sensorConfiguration = sensorConfiguration;
+    }
+
+    void ROS2LidarSensorComponent::Visualize()
     {
-        if (m_visualisationPoints.empty())
+        if (m_visualizationPoints.empty())
         {
             return;
         }
@@ -221,8 +150,8 @@ namespace ROS2
         {
             const uint8_t pixelSize = 2;
             AZ::RPI::AuxGeomDraw::AuxGeomDynamicDrawArguments drawArgs;
-            drawArgs.m_verts = m_visualisationPoints.data();
-            drawArgs.m_vertCount = m_visualisationPoints.size();
+            drawArgs.m_verts = m_visualizationPoints.data();
+            drawArgs.m_vertCount = m_visualizationPoints.size();
             drawArgs.m_colors = &AZ::Colors::Red;
             drawArgs.m_colorCount = 1;
             drawArgs.m_opacityType = AZ::RPI::AuxGeomDraw::OpacityType::Opaque;
@@ -233,25 +162,35 @@ namespace ROS2
 
     void ROS2LidarSensorComponent::Activate()
     {
-        auto ros2Node = ROS2Interface::Get()->GetNode();
-        AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for lidar sensor");
-
-        const TopicConfiguration& publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kPointCloudType];
-        AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic);
-        m_pointCloudPublisher = ros2Node->create_publisher<sensor_msgs::msg::PointCloud2>(fullTopic.data(), publisherConfig.GetQoS());
-
-        if (m_sensorConfiguration.m_visualise)
+        if (m_sensorConfiguration.m_visualize)
         {
             auto* entityScene = AZ::RPI::Scene::GetSceneForEntityId(GetEntityId());
             m_drawQueue = AZ::RPI::AuxGeomFeatureProcessorInterface::GetDrawQueueForScene(entityScene);
         }
 
-        m_lastRotations = LidarTemplateUtils::PopulateRayRotations(m_lidarParameters);
+        m_lastRotations = LidarTemplateUtils::PopulateRayRotations(m_lidarConfiguration.m_lidarParameters);
 
-        FetchLidarImplementationFeatures();
+        m_lidarConfiguration.FetchLidarImplementationFeatures();
         ConnectToLidarRaycaster();
         ConfigureLidarRaycaster();
 
+        m_canRaycasterPublish = false;
+        if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::PointcloudPublishing)
+        {
+            LidarRaycasterRequestBus::EventResult(
+                m_canRaycasterPublish, m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::CanHandlePublishing);
+        }
+
+        if (!m_canRaycasterPublish)
+        {
+            auto ros2Node = ROS2Interface::Get()->GetNode();
+            AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for lidar sensor");
+
+            const TopicConfiguration& publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kPointCloudType];
+            AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic);
+            m_pointCloudPublisher = ros2Node->create_publisher<sensor_msgs::msg::PointCloud2>(fullTopic.data(), publisherConfig.GetQoS());
+        }
+
         ROS2SensorComponent::Activate();
     }
 
@@ -259,12 +198,28 @@ namespace ROS2
     {
         ROS2SensorComponent::Deactivate();
         m_pointCloudPublisher.reset();
+
+        for (auto& [implementation, raycasterId] : m_implementationToRaycasterMap)
+        {
+            LidarSystemRequestBus::Event(AZ_CRC(implementation), &LidarSystemRequestBus::Events::DestroyLidar, raycasterId);
+        }
+
+        m_implementationToRaycasterMap.clear();
     }
 
     void ROS2LidarSensorComponent::FrequencyTick()
     {
         auto entityTransform = GetEntity()->FindComponent<AzFramework::TransformComponent>();
 
+        if (m_canRaycasterPublish)
+        {
+            const builtin_interfaces::msg::Time timestamp = ROS2Interface::Get()->GetROSTimestamp();
+            LidarRaycasterRequestBus::Event(
+                m_lidarRaycasterId,
+                &LidarRaycasterRequestBus::Events::UpdatePublisherTimestamp,
+                aznumeric_cast<AZ::u64>(timestamp.sec) * aznumeric_cast<AZ::u64>(1.0e9f) + timestamp.nanosec);
+        }
+
         LidarRaycasterRequestBus::EventResult(
             m_lastScanResults, m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::PerformRaycast, entityTransform->GetWorldTM());
         if (m_lastScanResults.m_points.empty())
@@ -273,9 +228,14 @@ namespace ROS2
             return;
         }
 
-        if (m_sensorConfiguration.m_visualise)
-        { // Store points for visualisation purposes, in global frame
-            m_visualisationPoints = m_lastScanResults.m_points;
+        if (m_sensorConfiguration.m_visualize)
+        { // Store points for visualization purposes, in global frame
+            m_visualizationPoints = m_lastScanResults.m_points;
+        }
+
+        if (m_canRaycasterPublish)
+        { // Skip publishing when it can be handled by the raycaster.
+            return;
         }
 
         const auto inverseLidarTM = entityTransform->GetWorldTM().GetInverse();

Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h

@@ -9,15 +9,15 @@
 
 #include <Atom/RPI.Public/AuxGeom/AuxGeomDraw.h>
 #include <AzCore/Serialization/SerializeContext.h>
-#include <Lidar/LidarRaycaster.h>
-#include <Lidar/LidarTemplate.h>
-#include <Lidar/LidarTemplateUtils.h>
 #include <ROS2/Lidar/LidarRegistrarBus.h>
 #include <ROS2/Lidar/LidarSystemBus.h>
 #include <ROS2/Sensor/ROS2SensorComponent.h>
 #include <rclcpp/publisher.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
+#include "LidarRaycaster.h"
+#include "LidarSensorConfiguration.h"
+
 namespace ROS2
 {
     //! Lidar sensor Component.
@@ -29,6 +29,7 @@ namespace ROS2
     public:
         AZ_COMPONENT(ROS2LidarSensorComponent, "{502A955F-7742-4E23-AD77-5E4063739DCA}", ROS2SensorComponent);
         ROS2LidarSensorComponent();
+        ROS2LidarSensorComponent(const SensorConfiguration& sensorConfiguration, const LidarSensorConfiguration& lidarConfiguration);
         ~ROS2LidarSensorComponent() = default;
         static void Reflect(AZ::ReflectContext* context);
         //////////////////////////////////////////////////////////////////////////
@@ -41,41 +42,24 @@ namespace ROS2
         //////////////////////////////////////////////////////////////////////////
         // ROS2SensorComponent overrides
         void FrequencyTick() override;
-        void Visualise() override;
-
-        bool IsConfigurationVisible() const;
-        bool IsIgnoredLayerConfigurationVisible() const;
-        bool IsEntityExclusionVisible() const;
-        bool IsMaxPointsConfigurationVisible() const;
+        void Visualize() override;
 
-        AZ::Crc32 OnLidarModelSelected();
-        AZ::Crc32 OnLidarImplementationSelected();
-        void FetchLidarImplementationFeatures();
-        AZStd::vector<AZStd::string> FetchLidarSystemList();
         void ConnectToLidarRaycaster();
         void ConfigureLidarRaycaster();
 
-        LidarSystemFeatures m_lidarSystemFeatures;
-        LidarTemplate::LidarModel m_lidarModel = LidarTemplate::LidarModel::Custom3DLidar;
-        LidarTemplate m_lidarParameters = LidarTemplateUtils::GetTemplate(LidarTemplate::LidarModel::Custom3DLidar);
-        AZStd::vector<AZ::Vector3> m_lastRotations;
-
-        AZStd::string m_lidarSystem;
         // A structure that maps each lidar implementation busId to the busId of a raycaster created by this LidarSensorComponent.
         AZStd::unordered_map<AZStd::string, LidarId> m_implementationToRaycasterMap;
+        bool m_canRaycasterPublish = false;
         LidarId m_lidarRaycasterId;
         std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>> m_pointCloudPublisher;
 
-        // Used only when visualisation is on - points differ since they are in global transform as opposed to local
-        AZStd::vector<AZ::Vector3> m_visualisationPoints;
+        // Used only when visualization is on - points differ since they are in global transform as opposed to local
+        AZStd::vector<AZ::Vector3> m_visualizationPoints;
         AZ::RPI::AuxGeomDrawPtr m_drawQueue;
 
-        RaycastResult m_lastScanResults;
-
-        AZ::u32 m_ignoredLayerIndex = 0;
-        bool m_ignoreLayer = false;
-        AZStd::vector<AZ::EntityId> m_excludedEntities;
+        LidarSensorConfiguration m_lidarConfiguration;
 
-        bool m_addPointsAtMax = false;
+        AZStd::vector<AZ::Vector3> m_lastRotations;
+        RaycastResult m_lastScanResults;
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/Manipulation/Controllers/JointsArticulationControllerComponent.cpp

@@ -40,6 +40,11 @@ namespace ROS2
         return AZ::Success();
     }
 
+    void JointsArticulationControllerComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
+    {
+        required.push_back(AZ_CRC_CE("ArticulationLinkService"));
+    }
+
     void JointsArticulationControllerComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
     {
         provided.push_back(AZ_CRC_CE("JointsControllerService"));

Gems/ROS2/Code/Source/Manipulation/Controllers/JointsArticulationControllerComponent.h

@@ -23,6 +23,7 @@ namespace ROS2
         ~JointsArticulationControllerComponent() = default;
         AZ_COMPONENT(JointsArticulationControllerComponent, "{243E9F07-5F84-4F83-9E6D-D1DA04D7CEF9}", AZ::Component);
 
+        static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
         static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided);
         static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible);
         static void Reflect(AZ::ReflectContext* context);

Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.cpp

@@ -28,10 +28,15 @@ namespace ROS2
         return m_goalStatus;
     }
 
+    void FollowJointTrajectoryActionServer::SetGoalSuccess()
+    {
+        m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Succeeded;
+    }
+
     void FollowJointTrajectoryActionServer::CancelGoal(std::shared_ptr<FollowJointTrajectory::Result> result)
     {
         AZ_Assert(m_goalHandle, "Invalid goal handle!");
-        if (m_goalHandle && m_goalHandle->is_executing())
+        if (m_goalHandle && m_goalHandle->is_canceling())
         {
             AZ_Trace("FollowJointTrajectoryActionServer", "Cancelling goal\n");
             m_goalHandle->canceled(result);
@@ -80,44 +85,16 @@ namespace ROS2
     }
 
     rclcpp_action::GoalResponse FollowJointTrajectoryActionServer::GoalReceivedCallback(
-        [[maybe_unused]] const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const FollowJointTrajectory::Goal> goal)
-    { // Accept each received goal unless other goal is active (no deferring/queuing)
-        if (!IsReadyForExecution())
-        {
-            AZ_Trace("FollowJointTrajectoryActionServer", "Goal rejected: server is not ready for execution!");
-            if (m_goalHandle)
-            {
-                AZ_Trace(
-                    "FollowJointTrajectoryActionServer",
-                    " is_active: %d,  is_executing %d, is_canceling : %d",
-                    m_goalHandle->is_active(),
-                    m_goalHandle->is_executing(),
-                    m_goalHandle->is_canceling());
-            }
-            return rclcpp_action::GoalResponse::REJECT;
-        }
-
-        AZ::Outcome<void, AZStd::string> executionOrderOutcome;
-        JointsTrajectoryRequestBus::EventResult(executionOrderOutcome, m_entityId, &JointsTrajectoryRequests::StartTrajectoryGoal, goal);
-
-        if (!executionOrderOutcome)
-        {
-            AZ_Trace("FollowJointTrajectoryActionServer", "Execution not be accepted: %s", executionOrderOutcome.GetError().c_str());
-            return rclcpp_action::GoalResponse::REJECT;
-        }
-
+        [[maybe_unused]] const rclcpp_action::GoalUUID& uuid, [[maybe_unused]] std::shared_ptr<const FollowJointTrajectory::Goal> goal)
+    { // Accept each received goal. It will be aborted if other goal is active (no deferring/queuing).
         return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
     }
 
     rclcpp_action::CancelResponse FollowJointTrajectoryActionServer::GoalCancelledCallback(
         [[maybe_unused]] const std::shared_ptr<GoalHandle> goalHandle)
     { // Accept each cancel attempt
-        auto result = std::make_shared<FollowJointTrajectory::Result>();
-        result->error_string = "User Cancelled";
-        result->error_code = FollowJointTrajectory::Result::SUCCESSFUL;
-
         AZ::Outcome<void, AZStd::string> cancelOutcome;
-        JointsTrajectoryRequestBus::EventResult(cancelOutcome, m_entityId, &JointsTrajectoryRequests::CancelTrajectoryGoal, result);
+        JointsTrajectoryRequestBus::EventResult(cancelOutcome, m_entityId, &JointsTrajectoryRequests::CancelTrajectoryGoal);
 
         if (!cancelOutcome)
         { // This will not happen in simulation unless intentionally done for behavior validation
@@ -132,6 +109,44 @@ namespace ROS2
     void FollowJointTrajectoryActionServer::GoalAcceptedCallback(const std::shared_ptr<GoalHandle> goalHandle)
     {
         AZ_Trace("FollowJointTrajectoryActionServer", "Goal accepted\n");
+
+        if (!IsReadyForExecution())
+        {
+            AZ_Trace("FollowJointTrajectoryActionServer", "Goal aborted: server is not ready for execution!");
+            if (m_goalHandle)
+            {
+                AZ_Trace(
+                    "FollowJointTrajectoryActionServer",
+                    " is_active: %d,  is_executing %d, is_canceling : %d",
+                    m_goalHandle->is_active(),
+                    m_goalHandle->is_executing(),
+                    m_goalHandle->is_canceling());
+            }
+
+            auto result = std::make_shared<FollowJointTrajectory::Result>();
+            result->error_string = "Goal aborted: server is not ready for execution!";
+            result->error_code = FollowJointTrajectory::Result::INVALID_GOAL;
+            goalHandle->abort(result);
+            return;
+        }
+
+        AZ::Outcome<void, FollowJointTrajectory::Result> executionOrderOutcome;
+        JointsTrajectoryRequestBus::EventResult(
+            executionOrderOutcome, m_entityId, &JointsTrajectoryRequests::StartTrajectoryGoal, goalHandle->get_goal());
+
+        if (!executionOrderOutcome)
+        {
+            AZ_Trace(
+                "FollowJointTrajectoryActionServer",
+                "Execution was not accepted: %s",
+                executionOrderOutcome.GetError().error_string.c_str());
+
+            auto result = std::make_shared<FollowJointTrajectory::Result>(executionOrderOutcome.GetError());
+
+            goalHandle->abort(result);
+            return;
+        }
+
         m_goalHandle = goalHandle;
         // m_goalHandle->execute(); // No need to call this, as we are already executing the goal due to ACCEPT_AND_EXECUTE
         m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Executing;

Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.h

@@ -40,6 +40,9 @@ namespace ROS2
         //! @param result Result to be passed to through action server to the client.
         void CancelGoal(std::shared_ptr<FollowJointTrajectory::Result> result);
 
+        //! Sets the goal status to success
+        void SetGoalSuccess();
+
         //! Report goal success to the action server.
         //! @param result Result which contains success code.
         void GoalSuccess(std::shared_ptr<FollowJointTrajectory::Result> result);

Gems/ROS2/Code/Source/Manipulation/JointStatePublisher.cpp

@@ -7,6 +7,7 @@
  */
 
 #include "JointStatePublisher.h"
+#include "ManipulationUtils.h"
 #include <ROS2/Manipulation/JointsManipulationRequests.h>
 #include <ROS2/ROS2Bus.h>
 #include <ROS2/Utilities/ROS2Names.h>
@@ -30,30 +31,42 @@ namespace ROS2
         rosHeader.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp();
         m_jointStateMsg.header = rosHeader;
 
+        AZ_Assert(m_jointNames.size() == m_jointStateMsg.name.size(), "The expected message size doesn't match with the joint list size");
+
+        for (size_t i = 0; i < m_jointStateMsg.name.size(); i++)
+        {
+            m_jointStateMsg.name[i] = m_jointNames[i].c_str();
+            JointInfo& jointInfo = m_jointInfos[i];
+
+            auto jointStateData = Utils::GetJointState(jointInfo);
+
+            m_jointStateMsg.position[i] = jointStateData.position;
+            m_jointStateMsg.velocity[i] = jointStateData.velocity;
+            m_jointStateMsg.effort[i] = jointStateData.effort;
+        }
+        m_jointStatePublisher->publish(m_jointStateMsg);
+    }
+
+    void JointStatePublisher::InitializePublisher(AZ::EntityId entityId)
+    {
         ManipulationJoints manipulatorJoints;
         JointsManipulationRequestBus::EventResult(manipulatorJoints, m_context.m_entityId, &JointsManipulationRequests::GetJoints);
 
+        for (const auto& [jointName, jointInfo] : manipulatorJoints)
+        {
+            m_jointNames.push_back(jointName);
+            m_jointInfos.push_back(jointInfo);
+        }
+
         m_jointStateMsg.name.resize(manipulatorJoints.size());
         m_jointStateMsg.position.resize(manipulatorJoints.size());
         m_jointStateMsg.velocity.resize(manipulatorJoints.size());
         m_jointStateMsg.effort.resize(manipulatorJoints.size());
-        size_t i = 0;
-        for (const auto& [jointName, jointInfo] : manipulatorJoints)
-        {
-            AZ::Outcome<float, AZStd::string> result;
-            JointsManipulationRequestBus::EventResult(result, m_context.m_entityId, &JointsManipulationRequests::GetJointPosition, jointName);
-            auto currentJointPosition = result.GetValue();
 
-            m_jointStateMsg.name[i] = jointName.c_str();
-            m_jointStateMsg.position[i] = currentJointPosition;
-            m_jointStateMsg.velocity[i] = 0.0;
-            m_jointStateMsg.effort[i] = 0.0;
-            i++;
-        }
-        m_jointStatePublisher->publish(m_jointStateMsg);
+        InstallPhysicalCallback();
     }
 
-    void JointStatePublisher::OnTick(float deltaTime)
+    void JointStatePublisher::OnPhysicsSimulationFinished([[maybe_unused]] AzPhysics::SceneHandle sceneHandle, float deltaTime)
     {
         AZ_Assert(m_configuration.m_frequency > 0.f, "JointPublisher frequency must be greater than zero");
         auto frameTime = 1.f / m_configuration.m_frequency;

Gems/ROS2/Code/Source/Manipulation/JointStatePublisher.h

@@ -10,6 +10,8 @@
 
 #include <AzCore/Component/EntityId.h>
 #include <ROS2/Communication/PublisherConfiguration.h>
+#include <ROS2/Manipulation/JointInfo.h>
+#include <ROS2/Utilities/PhysicsCallbackHandler.h>
 #include <rclcpp/publisher.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
 
@@ -24,13 +26,13 @@ namespace ROS2
 
     //! A class responsible for publishing the joint positions on ROS2 /joint_states topic.
     //!< @see <a href="https://docs.ros2.org/latest/api/sensor_msgs/msg/JointState.html">jointState message</a>.
-    class JointStatePublisher
+    class JointStatePublisher : public ROS2::Utils::PhysicsCallbackHandler
     {
     public:
         JointStatePublisher(const PublisherConfiguration& configuration, const JointStatePublisherContext& context);
+        virtual ~JointStatePublisher() = default;
 
-        //! Update time tick. This will result in state publishing if timing matches frequency.
-        void OnTick(float deltaTime);
+        void InitializePublisher(AZ::EntityId entityId);
 
     private:
         void PublishMessage();
@@ -41,5 +43,11 @@ namespace ROS2
         std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>> m_jointStatePublisher;
         sensor_msgs::msg::JointState m_jointStateMsg;
         float m_timeElapsedSinceLastTick = 0.0f;
+
+        AZStd::vector<AZStd::string> m_jointNames;
+        AZStd::vector<JointInfo> m_jointInfos;
+
+        // ROS2::Utils::PhysicsCallbackHandler overrides ...
+        void OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime) override;
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.cpp

@@ -10,20 +10,23 @@
 #include "Controllers/JointsArticulationControllerComponent.h"
 #include "Controllers/JointsPIDControllerComponent.h"
 #include "JointStatePublisher.h"
+#include "ManipulationUtils.h"
 #include <AzCore/Component/ComponentApplicationBus.h>
 #include <AzCore/Component/TransformBus.h>
+#include <AzCore/Debug/Trace.h>
 #include <AzCore/Serialization/EditContext.h>
 #include <ROS2/Frame/ROS2FrameComponent.h>
 #include <ROS2/Manipulation/Controllers/JointsPositionControllerRequests.h>
 #include <ROS2/Utilities/ROS2Names.h>
 #include <Source/ArticulationLinkComponent.h>
 #include <Source/HingeJointComponent.h>
+#include <Source/PrismaticJointComponent.h>
 
 namespace ROS2
 {
     namespace Internal
     {
-        void AddHingeJointInfo(const AZ::EntityComponentIdPair& idPair, const AZStd::string& jointName, ManipulationJoints& joints)
+        void Add1DOFJointInfo(const AZ::EntityComponentIdPair& idPair, const AZStd::string& jointName, ManipulationJoints& joints)
         {
             if (joints.find(jointName) != joints.end())
             {
@@ -117,11 +120,15 @@ namespace ROS2
                 }
                 const AZStd::string jointName(frameComponent->GetJointName().GetCStr());
 
-                auto* hingeComponent = Utils::GetGameOrEditorComponent<PhysX::HingeJointComponent>(entity);
+                auto* hingeComponent =
+                    azrtti_cast<PhysX::JointComponent*>(Utils::GetGameOrEditorComponent<PhysX::HingeJointComponent>(entity));
+                auto* prismaticComponent =
+                    azrtti_cast<PhysX::JointComponent*>(Utils::GetGameOrEditorComponent<PhysX::PrismaticJointComponent>(entity));
                 auto* articulationComponent = Utils::GetGameOrEditorComponent<PhysX::ArticulationLinkComponent>(entity);
+                bool classicJoint = hingeComponent || prismaticComponent;
                 AZ_Warning(
                     "JointsManipulationComponent",
-                    (hingeComponent && supportsClassicJoints) || !hingeComponent,
+                    (classicJoint && supportsClassicJoints) || !classicJoint,
                     "Found classic joints but the controller does not support them!");
                 AZ_Warning(
                     "JointsManipulationComponent",
@@ -132,7 +139,14 @@ namespace ROS2
                 if (supportsClassicJoints && hingeComponent)
                 {
                     auto idPair = AZ::EntityComponentIdPair(hingeComponent->GetEntityId(), hingeComponent->GetId());
-                    Internal::AddHingeJointInfo(idPair, jointName, manipulationJoints);
+                    Internal::Add1DOFJointInfo(idPair, jointName, manipulationJoints);
+                }
+
+                // See if there is a Prismatic Joint in the entity, add it to map.
+                if (supportsClassicJoints && prismaticComponent)
+                {
+                    auto idPair = AZ::EntityComponentIdPair(prismaticComponent->GetEntityId(), prismaticComponent->GetId());
+                    Internal::Add1DOFJointInfo(idPair, jointName, manipulationJoints);
                 }
 
                 // See if there is an Articulation Link in the entity, add it to map.
@@ -198,6 +212,24 @@ namespace ROS2
         return m_manipulationJoints;
     }
 
+    AZ::Outcome<JointPosition, AZStd::string> JointsManipulationComponent::GetJointPosition(const JointInfo& jointInfo)
+    {
+        float position{ 0.f };
+        if (jointInfo.m_isArticulation)
+        {
+            PhysX::ArticulationJointRequestBus::EventResult(
+                position,
+                jointInfo.m_entityComponentIdPair.GetEntityId(),
+                &PhysX::ArticulationJointRequests::GetJointPosition,
+                jointInfo.m_axis);
+        }
+        else
+        {
+            PhysX::JointRequestBus::EventResult(position, jointInfo.m_entityComponentIdPair, &PhysX::JointRequests::GetPosition);
+        }
+        return AZ::Success(position);
+    }
+
     AZ::Outcome<JointPosition, AZStd::string> JointsManipulationComponent::GetJointPosition(const AZStd::string& jointName)
     {
         if (!m_manipulationJoints.contains(jointName))
@@ -206,20 +238,37 @@ namespace ROS2
         }
 
         auto jointInfo = m_manipulationJoints.at(jointName);
-        float position{ 0 };
+
+        return GetJointPosition(jointInfo);
+    }
+
+    AZ::Outcome<JointVelocity, AZStd::string> JointsManipulationComponent::GetJointVelocity(const JointInfo& jointInfo)
+    {
+        float velocity{ 0.f };
         if (jointInfo.m_isArticulation)
         {
             PhysX::ArticulationJointRequestBus::EventResult(
-                position,
+                velocity,
                 jointInfo.m_entityComponentIdPair.GetEntityId(),
-                &PhysX::ArticulationJointRequests::GetJointPosition,
+                &PhysX::ArticulationJointRequests::GetJointVelocity,
                 jointInfo.m_axis);
         }
         else
         {
-            PhysX::JointRequestBus::EventResult(position, jointInfo.m_entityComponentIdPair, &PhysX::JointRequests::GetPosition);
+            PhysX::JointRequestBus::EventResult(velocity, jointInfo.m_entityComponentIdPair, &PhysX::JointRequests::GetVelocity);
         }
-        return AZ::Success(position);
+        return AZ::Success(velocity);
+    }
+
+    AZ::Outcome<JointVelocity, AZStd::string> JointsManipulationComponent::GetJointVelocity(const AZStd::string& jointName)
+    {
+        if (!m_manipulationJoints.contains(jointName))
+        {
+            return AZ::Failure(AZStd::string::format("Joint %s does not exist", jointName.c_str()));
+        }
+
+        auto jointInfo = m_manipulationJoints.at(jointName);
+        return GetJointVelocity(jointInfo);
     }
 
     JointsManipulationRequests::JointsPositionsMap JointsManipulationComponent::GetAllJointsPositions()
@@ -227,11 +276,70 @@ namespace ROS2
         JointsManipulationRequests::JointsPositionsMap positions;
         for (const auto& [jointName, jointInfo] : m_manipulationJoints)
         {
-            positions[jointName] = GetJointPosition(jointName).GetValue();
+            positions[jointName] = GetJointPosition(jointInfo).GetValue();
         }
         return positions;
     }
 
+    JointsManipulationRequests::JointsVelocitiesMap JointsManipulationComponent::GetAllJointsVelocities()
+    {
+        JointsManipulationRequests::JointsVelocitiesMap velocities;
+        for (const auto& [jointName, jointInfo] : m_manipulationJoints)
+        {
+            velocities[jointName] = GetJointVelocity(jointInfo).GetValue();
+        }
+        return velocities;
+    }
+
+    AZ::Outcome<JointEffort, AZStd::string> JointsManipulationComponent::GetJointEffort(const JointInfo& jointInfo)
+    {
+        auto jointStateData = Utils::GetJointState(jointInfo);
+
+        return AZ::Success(jointStateData.effort);
+    }
+
+    AZ::Outcome<JointEffort, AZStd::string> JointsManipulationComponent::GetJointEffort(const AZStd::string& jointName)
+    {
+        if (!m_manipulationJoints.contains(jointName))
+        {
+            return AZ::Failure(AZStd::string::format("Joint %s does not exist", jointName.c_str()));
+        }
+
+        auto jointInfo = m_manipulationJoints.at(jointName);
+        return GetJointEffort(jointInfo);
+    }
+
+    JointsManipulationRequests::JointsEffortsMap JointsManipulationComponent::GetAllJointsEfforts()
+    {
+        JointsManipulationRequests::JointsEffortsMap efforts;
+        for (const auto& [jointName, jointInfo] : m_manipulationJoints)
+        {
+            efforts[jointName] = GetJointEffort(jointInfo).GetValue();
+        }
+        return efforts;
+    }
+
+    AZ::Outcome<void, AZStd::string> JointsManipulationComponent::SetMaxJointEffort(const AZStd::string& jointName, JointEffort maxEffort)
+    {
+        if (!m_manipulationJoints.contains(jointName))
+        {
+            return AZ::Failure(AZStd::string::format("Joint %s does not exist", jointName.c_str()));
+        }
+
+        auto jointInfo = m_manipulationJoints.at(jointName);
+
+        if (jointInfo.m_isArticulation)
+        {
+            PhysX::ArticulationJointRequestBus::Event(
+                jointInfo.m_entityComponentIdPair.GetEntityId(),
+                &PhysX::ArticulationJointRequests::SetMaxForce,
+                jointInfo.m_axis,
+                maxEffort);
+        }
+
+        return AZ::Success();
+    }
+
     AZ::Outcome<void, AZStd::string> JointsManipulationComponent::MoveJointToPosition(
         const AZStd::string& jointName, JointPosition position)
     {
@@ -315,7 +423,8 @@ namespace ROS2
     void JointsManipulationComponent::Stop()
     {
         for (auto& [jointName, jointInfo] : m_manipulationJoints)
-        { // Set all target joint positions to their current positions.
+        { // Set all target joint positions to their current positions. There is no need to check if the outcome is successful, because
+          // jointName is always valid.
             jointInfo.m_restPosition = GetJointPosition(jointName).GetValue();
         }
     }
@@ -332,8 +441,8 @@ namespace ROS2
                 AZ::TickBus::Handler::BusDisconnect();
                 return;
             }
+            m_jointStatePublisher->InitializePublisher(GetEntityId());
         }
-        m_jointStatePublisher->OnTick(deltaTime);
         MoveToSetPositions(deltaTime);
     }
 } // namespace ROS2

Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.h

@@ -42,8 +42,18 @@ namespace ROS2
         ManipulationJoints GetJoints() override;
         //! @see ROS2::JointsManipulationRequestBus::GetJointPosition
         AZ::Outcome<JointPosition, AZStd::string> GetJointPosition(const AZStd::string& jointName) override;
+        //! @see ROS2::JointsManipulationRequestBus::GetJointVelocity
+        AZ::Outcome<JointVelocity, AZStd::string> GetJointVelocity(const AZStd::string& jointName) override;
         //! @see ROS2::JointsManipulationRequestBus::GetAllJointsPositions
         JointsPositionsMap GetAllJointsPositions() override;
+        //! @see ROS2::JointsManipulationRequestBus::GetAllJointsVelocities
+        JointsVelocitiesMap GetAllJointsVelocities() override;
+        //! @see ROS2::JointsManipulationRequestBus::GetJointEffort
+        AZ::Outcome<JointEffort, AZStd::string> GetJointEffort(const AZStd::string& jointName) override;
+        //! @see ROS2::JointsManipulationRequestBus::GetAllJointsEfforts
+        JointsEffortsMap GetAllJointsEfforts() override;
+        //! @see ROS2::JointsManipulationRequestBus::SetMaxJointEffort
+        AZ::Outcome<void, AZStd::string> SetMaxJointEffort(const AZStd::string& jointName, JointEffort maxEffort);
         //! @see ROS2::JointsManipulationRequestBus::MoveJointsToPositions
         AZ::Outcome<void, AZStd::string> MoveJointsToPositions(const JointsPositionsMap& positions) override;
         //! @see ROS2::JointsManipulationRequestBus::MoveJointToPosition
@@ -61,6 +71,10 @@ namespace ROS2
 
         void MoveToSetPositions(float deltaTime);
 
+        AZ::Outcome<JointPosition, AZStd::string> GetJointPosition(const JointInfo& jointInfo);
+        AZ::Outcome<JointVelocity, AZStd::string> GetJointVelocity(const JointInfo& jointInfo);
+        AZ::Outcome<JointEffort, AZStd::string> GetJointEffort(const JointInfo& jointInfo);
+
         AZStd::unique_ptr<JointStatePublisher> m_jointStatePublisher;
         PublisherConfiguration m_jointStatePublisherConfiguration;
         ManipulationJoints m_manipulationJoints;

Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp

@@ -8,6 +8,7 @@
 
 #include "JointsTrajectoryComponent.h"
 #include <AzCore/Serialization/EditContext.h>
+#include <PhysX/ArticulationJointBus.h>
 #include <ROS2/Frame/ROS2FrameComponent.h>
 #include <ROS2/Manipulation/JointsManipulationRequests.h>
 #include <ROS2/ROS2Bus.h>
@@ -79,11 +80,15 @@ namespace ROS2
         incompatible.push_back(AZ_CRC_CE("ManipulatorJointTrajectoryService"));
     }
 
-    AZ::Outcome<void, AZStd::string> JointsTrajectoryComponent::StartTrajectoryGoal(TrajectoryGoalPtr trajectoryGoal)
+    AZ::Outcome<void, JointsTrajectoryComponent::TrajectoryResult> JointsTrajectoryComponent::StartTrajectoryGoal(
+        TrajectoryGoalPtr trajectoryGoal)
     {
         if (m_trajectoryInProgress)
         {
-            return AZ::Failure("Another trajectory goal is executing. Wait for completion or cancel it");
+            auto result = JointsTrajectoryComponent::TrajectoryResult();
+            result.error_code = JointsTrajectoryComponent::TrajectoryResult::INVALID_GOAL;
+            result.error_string = "Another trajectory goal is executing. Wait for completion or cancel it";
+            return AZ::Failure(result);
         }
 
         auto validationResult = ValidateGoal(trajectoryGoal);
@@ -97,7 +102,7 @@ namespace ROS2
         return AZ::Success();
     }
 
-    AZ::Outcome<void, AZStd::string> JointsTrajectoryComponent::ValidateGoal(TrajectoryGoalPtr trajectoryGoal)
+    AZ::Outcome<void, JointsTrajectoryComponent::TrajectoryResult> JointsTrajectoryComponent::ValidateGoal(TrajectoryGoalPtr trajectoryGoal)
     {
         // Check joint names validity
         for (const auto& jointName : trajectoryGoal->trajectory.joint_names)
@@ -105,11 +110,14 @@ namespace ROS2
             AZStd::string azJointName(jointName.c_str());
             if (m_manipulationJoints.find(azJointName) == m_manipulationJoints.end())
             {
-                AZ_Printf(
-                    "JointsTrajectoryComponent",
-                    "Trajectory goal is invalid: no joint %s in manipulator",
-                    azJointName.c_str());
-                return AZ::Failure(AZStd::string::format("Trajectory goal is invalid: no joint %s in manipulator", azJointName.c_str()));
+                AZ_Printf("JointsTrajectoryComponent", "Trajectory goal is invalid: no joint %s in manipulator", azJointName.c_str());
+
+                auto result = JointsTrajectoryComponent::TrajectoryResult();
+                result.error_code = JointsTrajectoryComponent::TrajectoryResult::INVALID_JOINTS;
+                result.error_string = std::string(
+                    AZStd::string::format("Trajectory goal is invalid: no joint %s in manipulator", azJointName.c_str()).c_str());
+
+                return AZ::Failure(result);
             }
         }
         return AZ::Success();
@@ -117,64 +125,58 @@ namespace ROS2
 
     void JointsTrajectoryComponent::UpdateFeedback()
     {
+        auto goalStatus = GetGoalStatus();
+        if (goalStatus != JointsTrajectoryRequests::TrajectoryActionStatus::Executing)
+        {
+            return;
+        }
+
         auto feedback = std::make_shared<control_msgs::action::FollowJointTrajectory::Feedback>();
-        trajectory_msgs::msg::JointTrajectoryPoint desiredPoint;
-        for (const auto& [jointName, hingeComponent] : m_manipulationJoints)
+
+        trajectory_msgs::msg::JointTrajectoryPoint desiredPoint = m_trajectoryGoal.trajectory.points.front();
+
+        trajectory_msgs::msg::JointTrajectoryPoint actualPoint;
+
+        size_t jointCount = m_trajectoryGoal.trajectory.joint_names.size();
+        for (size_t jointIndex = 0; jointIndex < jointCount; jointIndex++)
         {
+            AZStd::string jointName(m_trajectoryGoal.trajectory.joint_names[jointIndex].c_str());
             std::string jointNameStdString(jointName.c_str());
             feedback->joint_names.push_back(jointNameStdString);
 
-            AZ::Outcome<float, AZStd::string> result;
-            JointsManipulationRequestBus::EventResult(result, GetEntityId(), &JointsManipulationRequests::GetJointPosition, jointName);
-            auto currentJointPosition = result.GetValue();
-
-            desiredPoint.positions.push_back(static_cast<double>(currentJointPosition));
-            desiredPoint.velocities.push_back(0.0f); // Velocities not supported yet!
-            desiredPoint.accelerations.push_back(0.0f); // Accelerations not supported yet!
-            desiredPoint.effort.push_back(0.0f); // Effort not supported yet!
+            float currentJointPosition;
+            float currentJointVelocity;
+            auto& jointInfo = m_manipulationJoints[jointName];
+            PhysX::ArticulationJointRequestBus::Event(
+                jointInfo.m_entityComponentIdPair.GetEntityId(),
+                [&](PhysX::ArticulationJointRequests* articulationJointRequests)
+                {
+                    currentJointPosition = articulationJointRequests->GetJointPosition(jointInfo.m_axis);
+                    currentJointVelocity = articulationJointRequests->GetJointVelocity(jointInfo.m_axis);
+                });
+
+            actualPoint.positions.push_back(static_cast<double>(currentJointPosition));
+            actualPoint.velocities.push_back(static_cast<double>(currentJointVelocity));
+            // Acceleration should also be filled in somehow, or removed from the trajectory altogether.
         }
 
-        trajectory_msgs::msg::JointTrajectoryPoint actualPoint = desiredPoint;
         trajectory_msgs::msg::JointTrajectoryPoint currentError;
-
-        std::transform(
-            desiredPoint.positions.begin(),
-            desiredPoint.positions.end(),
-            actualPoint.positions.begin(),
-            currentError.positions.begin(),
-            std::minus<double>());
-
-        std::transform(
-            desiredPoint.velocities.begin(),
-            desiredPoint.velocities.end(),
-            actualPoint.velocities.begin(),
-            currentError.velocities.begin(),
-            std::minus<double>());
-
-        std::transform(
-            desiredPoint.accelerations.begin(),
-            desiredPoint.accelerations.end(),
-            actualPoint.accelerations.begin(),
-            currentError.accelerations.begin(),
-            std::minus<double>());
-
-        std::transform(
-            desiredPoint.effort.begin(),
-            desiredPoint.effort.end(),
-            actualPoint.effort.begin(),
-            currentError.effort.begin(),
-            std::minus<double>());
+        for (size_t jointIndex = 0; jointIndex < jointCount; jointIndex++)
+        {
+            currentError.positions.push_back(actualPoint.positions[jointIndex] - desiredPoint.positions[jointIndex]);
+            currentError.velocities.push_back(actualPoint.velocities[jointIndex] - desiredPoint.velocities[jointIndex]);
+        }
 
         feedback->desired = desiredPoint;
         feedback->actual = actualPoint;
         feedback->error = currentError;
+
         m_followTrajectoryServer->PublishFeedback(feedback);
     }
 
-    AZ::Outcome<void, AZStd::string> JointsTrajectoryComponent::CancelTrajectoryGoal(TrajectoryResultPtr result)
+    AZ::Outcome<void, AZStd::string> JointsTrajectoryComponent::CancelTrajectoryGoal()
     {
         m_trajectoryGoal.trajectory.points.clear();
-        m_followTrajectoryServer->CancelGoal(result);
         m_trajectoryInProgress = false;
         return AZ::Success();
     }
@@ -190,6 +192,11 @@ namespace ROS2
         if (goalStatus == JointsTrajectoryRequests::TrajectoryActionStatus::Cancelled)
         {
             JointsManipulationRequestBus::Event(GetEntityId(), &JointsManipulationRequests::Stop);
+            auto result = std::make_shared<FollowJointTrajectoryActionServer::FollowJointTrajectory::Result>();
+            result->error_string = "User Cancelled";
+            result->error_code = FollowJointTrajectoryActionServer::FollowJointTrajectory::Result::SUCCESSFUL;
+            m_followTrajectoryServer->CancelGoal(result);
+            m_followTrajectoryServer->SetGoalSuccess();
             return;
         }
 
@@ -246,5 +253,6 @@ namespace ROS2
         }
         uint64_t deltaTimeNs = deltaTime * 1'000'000'000;
         FollowTrajectory(deltaTimeNs);
+        UpdateFeedback();
     }
 } // namespace ROS2

Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.h

@@ -36,9 +36,9 @@ namespace ROS2
 
         // JointsTrajectoryRequestBus::Handler overrides ...
         //! @see ROS2::JointsTrajectoryRequestBus::StartTrajectoryGoal
-        AZ::Outcome<void, AZStd::string> StartTrajectoryGoal(TrajectoryGoalPtr trajectoryGoal) override;
+        AZ::Outcome<void, TrajectoryResult> StartTrajectoryGoal(TrajectoryGoalPtr trajectoryGoal) override;
         //! @see ROS2::JointsTrajectoryRequestBus::CancelTrajectoryGoal
-        AZ::Outcome<void, AZStd::string> CancelTrajectoryGoal(TrajectoryResultPtr trajectoryResult) override;
+        AZ::Outcome<void, AZStd::string> CancelTrajectoryGoal() override;
         //! @see ROS2::JointsTrajectoryRequestBus::GetGoalStatus
         TrajectoryActionStatus GetGoalStatus() override;
 
@@ -53,7 +53,7 @@ namespace ROS2
         //! Follow set trajectory.
         //! @param deltaTimeNs frame time step, to advance trajectory by.
         void FollowTrajectory(const uint64_t deltaTimeNs);
-        AZ::Outcome<void, AZStd::string> ValidateGoal(TrajectoryGoalPtr trajectoryGoal);
+        AZ::Outcome<void, TrajectoryResult> ValidateGoal(TrajectoryGoalPtr trajectoryGoal);
         void MoveToNextPoint(const trajectory_msgs::msg::JointTrajectoryPoint currentTrajectoryPoint);
         void UpdateFeedback();
 

Gems/ROS2/Code/Source/Manipulation/ManipulationUtils.cpp

@@ -0,0 +1,52 @@
+/*
+ * 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 "ManipulationUtils.h"
+#include <PhysX/ArticulationJointBus.h>
+#include <PhysX/Joint/PhysXJointRequestsBus.h>
+
+namespace ROS2::Utils
+{
+    JointStateData GetJointState(const JointInfo& jointInfo)
+    {
+        JointStateData result;
+
+        if (jointInfo.m_isArticulation)
+        {
+            PhysX::ArticulationJointRequestBus::Event(
+                jointInfo.m_entityComponentIdPair.GetEntityId(),
+                [&](PhysX::ArticulationJointRequests* articulationJointRequests)
+                {
+                    result.position = articulationJointRequests->GetJointPosition(jointInfo.m_axis);
+                    result.velocity = articulationJointRequests->GetJointVelocity(jointInfo.m_axis);
+                    const bool is_acceleration_driven = articulationJointRequests->IsAccelerationDrive(jointInfo.m_axis);
+                    if (!is_acceleration_driven)
+                    {
+                        const float stiffness = articulationJointRequests->GetDriveStiffness(jointInfo.m_axis);
+                        const float damping = articulationJointRequests->GetDriveDamping(jointInfo.m_axis);
+                        const float targetPosition = articulationJointRequests->GetDriveTarget(jointInfo.m_axis);
+                        const float targetVelocity = articulationJointRequests->GetDriveTargetVelocity(jointInfo.m_axis);
+                        const float maxEffort = articulationJointRequests->GetMaxForce(jointInfo.m_axis);
+                        result.effort = stiffness * -(result.position - targetPosition) + damping * (targetVelocity - result.velocity);
+                        result.effort = AZ::GetClamp(result.effort, -maxEffort, maxEffort);
+                    }
+                });
+        }
+        else
+        {
+            PhysX::JointRequestBus::Event(
+                jointInfo.m_entityComponentIdPair,
+                [&](PhysX::JointRequests* jointRequests)
+                {
+                    result.position = jointRequests->GetPosition();
+                    result.velocity = jointRequests->GetVelocity();
+                });
+        }
+        return result;
+    }
+} // namespace ROS2::Utils

Gems/ROS2/Code/Source/Manipulation/ManipulationUtils.h

@@ -0,0 +1,25 @@
+/*
+ * 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 <ROS2/Manipulation/JointInfo.h>
+
+namespace ROS2::Utils
+{
+    struct JointStateData
+    {
+        float position{ 0.f };
+        float velocity{ 0.f };
+        float effort{ 0.f };
+    };
+
+    //! Get the current joint state
+    //! @param jointInfo Info of the joint we want to get data of.
+    //! @return Data with the current joint state.
+    JointStateData GetJointState(const JointInfo& jointInfo);
+} // namespace ROS2::Utils

Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/PidMotorControllerComponent.cpp

@@ -7,6 +7,7 @@
  */
 
 #include <AzCore/Component/ComponentBus.h>
+#include <AzCore/RTTI/BehaviorContext.h>
 #include <AzCore/Serialization/EditContext.h>
 #include <PhysX/Joint/PhysXJointRequestsBus.h>
 #include <ROS2/Manipulation/MotorizedJoints/PidMotorControllerComponent.h>
@@ -37,6 +38,16 @@ namespace ROS2
                     ->DataElement(AZ::Edit::UIHandlers::Default, &PidMotorControllerComponent::m_pidPos, "Pid Position", "Pid Position");
             }
         }
+
+        if (AZ::BehaviorContext* behaviorContext = azrtti_cast<AZ::BehaviorContext*>(context))
+        {
+            behaviorContext->EBus<PidMotorControllerRequestBus>("PidMotorControllerComponent", "PidMotorControllerRequestBus")
+                ->Attribute(AZ::Edit::Attributes::Category, "ROS2/PidMotorController")
+                ->Event("SetSetpoint", &PidMotorControllerRequestBus::Events::SetSetpoint)
+                ->Event("GetSetpoint", &PidMotorControllerRequestBus::Events::GetSetpoint)
+                ->Event("GetCurrentMeasurement", &PidMotorControllerRequestBus::Events::GetCurrentMeasurement)
+                ->Event("GetError", &PidMotorControllerRequestBus::Events::GetError);
+        }
     }
 
     void PidMotorControllerComponent::Activate()

Gems/ROS2/Code/Source/Odometry/ROS2OdometrySensorComponent.cpp

@@ -51,18 +51,25 @@ namespace ROS2
 
     void ROS2OdometrySensorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
     {
-        required.push_back(AZ_CRC_CE("PhysicsRigidBodyService"));
+        required.push_back(AZ_CRC_CE("PhysicsDynamicRigidBodyService"));
         required.push_back(AZ_CRC_CE("ROS2Frame"));
     }
 
     // ROS2SensorComponent overrides ...
     void ROS2OdometrySensorComponent::SetupRefreshLoop()
     {
-        InstallPhysicalCallback(m_entity->GetId());
+        InstallPhysicalCallback();
     }
 
     void ROS2OdometrySensorComponent::OnPhysicsInitialization(AzPhysics::SceneHandle sceneHandle)
     {
+        AzPhysics::RigidBody* rigidBody = nullptr;
+        AZ::EntityId entityId = GetEntityId();
+        Physics::RigidBodyRequestBus::EventResult(rigidBody, entityId, &Physics::RigidBodyRequests::GetRigidBody);
+        AZ_Assert(rigidBody, "Entity %s does not have rigid body.", entityId.ToString().c_str());
+
+        m_bodyHandle = rigidBody->m_bodyHandle;
+
         auto* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
         auto* simulatedBodyPtr = sceneInterface->GetSimulatedBodyFromHandle(sceneHandle, m_bodyHandle);
         auto rigidbodyPtr = azrtti_cast<AzPhysics::RigidBody*>(simulatedBodyPtr);

Gems/ROS2/Code/Source/Odometry/ROS2OdometrySensorComponent.h

@@ -13,9 +13,10 @@
 #include <AzCore/Serialization/SerializeContext.h>
 #include <AzFramework/Physics/Common/PhysicsEvents.h>
 #include <AzFramework/Physics/PhysicsScene.h>
+#include <AzFramework/Physics/PhysicsSystem.h>
 #include <AzFramework/Physics/SimulatedBodies/RigidBody.h>
 #include <ROS2/Sensor/ROS2SensorComponent.h>
-#include <Utilities/PhysicsCallbackHandler.h>
+#include <ROS2/Utilities/PhysicsCallbackHandler.h>
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/publisher.hpp>
 
@@ -53,5 +54,8 @@ namespace ROS2
         // ROS2::Utils::PhysicsCallbackHandler overrides ...
         void OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime) override;
         void OnPhysicsInitialization(AzPhysics::SceneHandle sceneHandle) override;
+
+        //! Handler to simulated physical body
+        AzPhysics::SimulatedBodyHandle m_bodyHandle = AzPhysics::InvalidSimulatedBodyHandle;
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/Odometry/ROS2WheelOdometry.cpp

@@ -67,14 +67,13 @@ namespace ROS2
 
     void ROS2WheelOdometryComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
     {
-        required.push_back(AZ_CRC_CE("PhysicsRigidBodyService"));
         required.push_back(AZ_CRC_CE("ROS2Frame"));
         required.push_back(AZ_CRC_CE("SkidSteeringModelService"));
     }
 
     void ROS2WheelOdometryComponent::SetupRefreshLoop()
     {
-        InstallPhysicalCallback(m_entity->GetId());
+        InstallPhysicalCallback();
     }
 
     void ROS2WheelOdometryComponent::OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime)

Gems/ROS2/Code/Source/Odometry/ROS2WheelOdometry.h

@@ -14,7 +14,7 @@
 #include <AzFramework/Physics/PhysicsSystem.h>
 #include <AzFramework/Physics/SimulatedBodies/RigidBody.h>
 #include <ROS2/Sensor/ROS2SensorComponent.h>
-#include <Utilities/PhysicsCallbackHandler.h>
+#include <ROS2/Utilities/PhysicsCallbackHandler.h>
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/publisher.hpp>
 

Gems/ROS2/Code/Source/ProximitySensor/ROS2ProximitySensor.cpp

@@ -15,6 +15,8 @@
 #include <AzCore/std/string/string.h>
 
 #include <ROS2/Communication/TopicConfiguration.h>
+#include <ROS2/ProximitySensor/ProximitySensorNotificationBus.h>
+#include <ROS2/ProximitySensor/ProximitySensorNotificationBusHandler.h>
 #include <ROS2/ROS2Bus.h>
 #include <ROS2/Sensor/ROS2SensorComponent.h>
 #include <ROS2/Utilities/ROS2Names.h>
@@ -49,6 +51,12 @@ namespace ROS2
                     ->Attribute(AZ::Edit::Attributes::Min, 0.);
             }
         }
+        if (AZ::BehaviorContext* behaviorContext = azrtti_cast<AZ::BehaviorContext*>(context))
+        {
+            behaviorContext->EBus<ProximitySensorNotificationBus>("ROS2ProximitySensor", "ProximitySensorNotificationBus")
+                ->Attribute(AZ::Edit::Attributes::Category, "ROS2/ProximitySensor")
+                ->Handler<ProximitySensorNotificationBusHandler>();
+        }
     }
 
     ROS2ProximitySensor::ROS2ProximitySensor()
@@ -70,7 +78,7 @@ namespace ROS2
         AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic);
         m_detectionPublisher = ros2Node->create_publisher<std_msgs::msg::Bool>(fullTopic.data(), publisherConfig.GetQoS());
 
-        if (m_sensorConfiguration.m_visualise)
+        if (m_sensorConfiguration.m_visualize)
         {
             auto* entityScene = AZ::RPI::Scene::GetSceneForEntityId(GetEntityId());
             m_drawQueue = AZ::RPI::AuxGeomFeatureProcessorInterface::GetDrawQueueForScene(entityScene);
@@ -91,7 +99,7 @@ namespace ROS2
         ROS2SensorComponent::Deactivate();
     }
 
-    void ROS2ProximitySensor::Visualise()
+    void ROS2ProximitySensor::Visualize()
     {
         if (m_drawQueue)
         {
@@ -143,8 +151,17 @@ namespace ROS2
 
             std_msgs::msg::Bool msg;
             m_position = !result.m_hits.empty() ? std::make_optional(result.m_hits.front().m_position) : std::nullopt;
-            msg.data = m_position ? true : false;;
+            msg.data = m_position ? true : false;
             m_detectionPublisher->publish(msg);
+
+            if (m_position)
+            {
+                ProximitySensorNotificationBus::Event(GetEntityId(), &ProximitySensorNotifications::OnObjectInRange);
+            }
+            else
+            {
+                ProximitySensorNotificationBus::Event(GetEntityId(), &ProximitySensorNotifications::OnObjectOutOfRange);
+            }
         }
     }
 } // namespace ROS2

Gems/ROS2/Code/Source/ProximitySensor/ROS2ProximitySensor.h

@@ -35,7 +35,7 @@ namespace ROS2
         //////////////////////////////////////////////////////////////////////////
         // ROS2SensorComponent overrides
         void FrequencyTick() override;
-        void Visualise() override;
+        void Visualize() override;
         //////////////////////////////////////////////////////////////////////////
 
         AZ::Vector3 m_detectionDirection = AZ::Vector3::CreateAxisX();