diff --git a/.gitattributes b/.gitattributes index c0c8f450f..3f0934338 100644 --- a/.gitattributes +++ b/.gitattributes @@ -1,4 +1,3 @@ - # # Git LFS (see https://git-lfs.github.com/) # @@ -126,5 +125,10 @@ Gems/AtomLyIntegration/CommonFeatures/Assets/Objects/Lucy/.wip/marmoset_bake.tbs Gems/AtomLyIntegration/CommonFeatures/Assets/Objects/Lucy/.wip/Brass/brass_bake.spp filter=lfs diff=lfs merge=lfs -text Gems/AtomLyIntegration/CommonFeatures/Assets/Objects/Lucy/.wip/stone/stone_bake.spp filter=lfs diff=lfs merge=lfs -text Gems/WarehouseAssets/docs/images/*.png filter= diff= merge= -text +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 +Templates/Ros2RoboticManipulationTemplate/Screenshots/*.png filter= diff= merge= -text + +*.pgm filter=lfs diff=lfs merge=lfs -text diff --git a/Gems/AzQtComponentsForPython/gem.json b/Gems/AzQtComponentsForPython/gem.json index 010957ec7..825eb67c0 100644 --- a/Gems/AzQtComponentsForPython/gem.json +++ b/Gems/AzQtComponentsForPython/gem.json @@ -19,5 +19,8 @@ "documentation_url": "", "dependencies": [ "EditorPythonBindings" - ] + ], + "repo_uri": "https://raw.githubusercontent.com/o3de/o3de-extras/development", + "download_source_uri": "https://github.com/o3de/o3de-extras/releases/download/2.0/azqtcomponentsforpython-1.0.0-gem.zip", + "version": "1.0.0" } diff --git a/Gems/OpenXRVk/Code/Include/OpenXRVk/OpenXRVkDevice.h b/Gems/OpenXRVk/Code/Include/OpenXRVk/OpenXRVkDevice.h index 013863e09..12a5bf0c7 100644 --- a/Gems/OpenXRVk/Code/Include/OpenXRVk/OpenXRVkDevice.h +++ b/Gems/OpenXRVk/Code/Include/OpenXRVk/OpenXRVkDevice.h @@ -11,6 +11,7 @@ #include #include #include +#include namespace OpenXRVk { @@ -27,6 +28,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 +43,11 @@ 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; + + //! Returns the graphic binding for a hardware queue + const AZ::Vulkan::XRDeviceDescriptor::GraphicsBinding& GetGraphicsBinding(AZ::RHI::HardwareQueueClass queueClass) const; //! Reserve space for appropriate number of views void InitXrViews(uint32_t numViews); @@ -67,12 +72,13 @@ namespace OpenXRVk ////////////////////////////////////////////////////////////////////////// VkDevice m_xrVkDevice = VK_NULL_HANDLE; + VkPhysicalDevice m_xrVkPhysicalDevice = VK_NULL_HANDLE; + AZStd::array m_xrQueueBinding; XrFrameState m_frameState{ XR_TYPE_FRAME_STATE }; AZStd::vector m_xrLayers; XrCompositionLayerProjection m_xrLayer{ XR_TYPE_COMPOSITION_LAYER_PROJECTION }; AZStd::vector m_projectionLayerViews; AZStd::vector m_views; uint32_t m_viewCountOutput = 0; - GladVulkanContext m_context = {}; }; } \ No newline at end of file diff --git a/Gems/OpenXRVk/Code/Include/OpenXRVk/OpenXRVkInstance.h b/Gems/OpenXRVk/Code/Include/OpenXRVk/OpenXRVkInstance.h index d12b116f3..5dd3ee127 100644 --- a/Gems/OpenXRVk/Code/Include/OpenXRVk/OpenXRVkInstance.h +++ b/Gems/OpenXRVk/Code/Include/OpenXRVk/OpenXRVkInstance.h @@ -8,11 +8,12 @@ #pragma once + #include -#include -#include #include #include +#include +#include #include 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& extensions) override; + void CollectMinMaxVulkanAPIVersions(AZStd::vector& min, AZStd::vector& max) override; + + // AZ::Vulkan::DeviceRequirementBus::Handler overrides... + void CollectAdditionalRequiredDeviceExtensions(AZStd::vector& extensions) override; + void FilterSupportedDevices(AZStd::vector& 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 m_functionLoader; + XR::StringList m_requireVulkanInstanceExtensions; + XR::StringList m_requireVulkanDeviceExtensions; AZStd::vector m_supportedXRDevices; - AZ::u32 m_physicalDeviceActiveIndex = 0; + uint32_t m_minVulkanAPIVersion = 0; + uint32_t m_maxVulkanAPIVersion = 0; }; } diff --git a/Gems/OpenXRVk/Code/Include/OpenXRVk/OpenXRVkSystemComponent.h b/Gems/OpenXRVk/Code/Include/OpenXRVk/OpenXRVkSystemComponent.h index b3346e690..e980edeaf 100644 --- a/Gems/OpenXRVk/Code/Include/OpenXRVk/OpenXRVkSystemComponent.h +++ b/Gems/OpenXRVk/Code/Include/OpenXRVk/OpenXRVkSystemComponent.h @@ -9,6 +9,7 @@ #pragma once #include +#include #include namespace OpenXRVk @@ -24,8 +25,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 +60,8 @@ namespace OpenXRVk //! Create XR::Swapchain::Image object. XR::Ptr CreateSwapChainImage() override; /////////////////////////////////////////////////////////////////// + + private: + XR::Ptr m_instance; }; } \ No newline at end of file diff --git a/Gems/OpenXRVk/Code/Include/OpenXRVk/OpenXRVkUtils.h b/Gems/OpenXRVk/Code/Include/OpenXRVk/OpenXRVkUtils.h index b09874548..29df63ad4 100644 --- a/Gems/OpenXRVk/Code/Include/OpenXRVk/OpenXRVkUtils.h +++ b/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 ParseExtensionString(char* names); - - //! Disable certain extensions because function pointers didn't load correctly. - void FilterAvailableExtensions(GladVulkanContext& context); } diff --git a/Gems/OpenXRVk/Code/Source/OpenXRVkDevice.cpp b/Gems/OpenXRVk/Code/Source/OpenXRVkDevice.cpp index b7929c64f..f37842e36 100644 --- a/Gems/OpenXRVk/Code/Source/OpenXRVkDevice.cpp +++ b/Gems/OpenXRVk/Code/Source/OpenXRVkDevice.cpp @@ -13,7 +13,7 @@ #include #include #include -#include +#include #include namespace OpenXRVk @@ -26,90 +26,9 @@ namespace OpenXRVk AZ::RHI::ResultCode Device::InitDeviceInternal(AZ::RHI::XRDeviceDescriptor* deviceDescriptor) { AZ::Vulkan::XRDeviceDescriptor* xrDeviceDescriptor = static_cast(deviceDescriptor); - Instance* xrVkInstance = static_cast(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 = nullptr; - - PFN_xrGetVulkanDeviceExtensionsKHR pfnGetVulkanDeviceExtensionsKHR = nullptr; - XrResult result = xrGetInstanceProcAddr( - xrVkInstance->GetXRInstance(), "xrGetVulkanDeviceExtensionsKHR", reinterpret_cast(&pfnGetVulkanDeviceExtensionsKHR)); - ASSERT_IF_UNSUCCESSFUL(result); - - AZ::u32 deviceExtensionNamesSize = 0; - result = pfnGetVulkanDeviceExtensionsKHR(xrVkInstance->GetXRInstance(), xrDeviceCreateInfo.systemId, 0, &deviceExtensionNamesSize, nullptr); - ASSERT_IF_UNSUCCESSFUL(result); - - AZStd::vector deviceExtensionNames(deviceExtensionNamesSize); - result = pfnGetVulkanDeviceExtensionsKHR( - xrVkInstance->GetXRInstance(), xrDeviceCreateInfo.systemId, deviceExtensionNamesSize, &deviceExtensionNamesSize, &deviceExtensionNames[0]); - ASSERT_IF_UNSUCCESSFUL(result); - - AZStd::vector 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(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; + m_xrQueueBinding = xrDeviceDescriptor->m_inputData.m_xrQueueBinding; return AZ::RHI::ResultCode::Success; } @@ -258,9 +177,14 @@ namespace OpenXRVk return m_xrVkDevice; } - const GladVulkanContext& Device::GetContext() const + VkPhysicalDevice Device::GetNativePhysicalDevice() const + { + return m_xrVkPhysicalDevice; + } + + const AZ::Vulkan::XRDeviceDescriptor::GraphicsBinding& Device::GetGraphicsBinding(AZ::RHI::HardwareQueueClass queueClass) const { - return m_context; + return m_xrQueueBinding[static_cast(queueClass)]; } AZ::RHI::ResultCode Device::GetViewFov(AZ::u32 viewIndex, AZ::RPI::FovData& outFovData) const @@ -304,10 +228,7 @@ namespace OpenXRVk m_projectionLayerViews.clear(); m_views.clear(); m_xrLayers.clear(); - if (m_xrVkDevice != VK_NULL_HANDLE) - { - m_context.DestroyDevice(m_xrVkDevice, nullptr); - m_xrVkDevice = VK_NULL_HANDLE; - } + m_xrVkDevice = VK_NULL_HANDLE; + m_xrVkPhysicalDevice = VK_NULL_HANDLE; } } \ No newline at end of file diff --git a/Gems/OpenXRVk/Code/Source/OpenXRVkInstance.cpp b/Gems/OpenXRVk/Code/Source/OpenXRVkInstance.cpp index 272ec85e0..7209afc3f 100644 --- a/Gems/OpenXRVk/Code/Source/OpenXRVkInstance.cpp +++ b/Gems/OpenXRVk/Code/Source/OpenXRVkInstance.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -190,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", @@ -205,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(&pfnGetVulkanInstanceExtensionsKHR)); + ASSERT_IF_UNSUCCESSFUL(result); + + AZ::u32 extensionNamesSize = 0; + result = pfnGetVulkanInstanceExtensionsKHR(m_xrInstance, m_xrSystemId, 0, &extensionNamesSize, nullptr); + ASSERT_IF_UNSUCCESSFUL(result); + + AZStd::vector extensionNames(extensionNamesSize); + result = pfnGetVulkanInstanceExtensionsKHR(m_xrInstance, m_xrSystemId, extensionNamesSize, &extensionNamesSize, &extensionNames[0]); + ASSERT_IF_UNSUCCESSFUL(result); + + AZStd::vector 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(&pfnGetVulkanDeviceExtensionsKHR)); + ASSERT_IF_UNSUCCESSFUL(result); + + AZ::u32 deviceExtensionNamesSize = 0; + result = pfnGetVulkanDeviceExtensionsKHR(m_xrInstance, m_xrSystemId, 0, &deviceExtensionNamesSize, nullptr); + ASSERT_IF_UNSUCCESSFUL(result); + + AZStd::vector deviceExtensionNames(deviceExtensionNamesSize); + result = pfnGetVulkanDeviceExtensionsKHR( + m_xrInstance, m_xrSystemId, deviceExtensionNamesSize, &deviceExtensionNamesSize, &deviceExtensionNames[0]); + ASSERT_IF_UNSUCCESSFUL(result); + + AZStd::vector 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; } @@ -288,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(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 = nullptr; + 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(&pfnGetVulkanInstanceExtensionsKHR)); - ASSERT_IF_UNSUCCESSFUL(result); - - AZ::u32 extensionNamesSize = 0; - result = pfnGetVulkanInstanceExtensionsKHR(m_xrInstance, m_xrSystemId, 0, &extensionNamesSize, nullptr); - ASSERT_IF_UNSUCCESSFUL(result); - - AZStd::vector extensionNames(extensionNamesSize); - result = pfnGetVulkanInstanceExtensionsKHR(m_xrInstance, m_xrSystemId, extensionNamesSize, &extensionNamesSize, &extensionNames[0]); - ASSERT_IF_UNSUCCESSFUL(result); - - AZStd::vector 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& 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(extensions.size()); - instInfo.ppEnabledExtensionNames = extensions.empty() ? nullptr : extensions.data(); + void Instance::CollectMinMaxVulkanAPIVersions(AZStd::vector& min, AZStd::vector& max) + { + min.push_back(m_minVulkanAPIVersion); + max.push_back(m_maxVulkanAPIVersion); + } - auto pfnCreateInstance = (PFN_vkCreateInstance)createInfo.pfnGetInstanceProcAddr(nullptr, "vkCreateInstance"); - VkResult vkResult = pfnCreateInstance(&instInfo, nullptr, &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& 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& 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, nullptr); - 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) @@ -409,12 +418,6 @@ namespace OpenXRVk return aznumeric_cast(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; @@ -430,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; diff --git a/Gems/OpenXRVk/Code/Source/OpenXRVkSession.cpp b/Gems/OpenXRVk/Code/Source/OpenXRVkSession.cpp index 2bb104ba1..182002c57 100644 --- a/Gems/OpenXRVk/Code/Source/OpenXRVkSession.cpp +++ b/Gems/OpenXRVk/Code/Source/OpenXRVkSession.cpp @@ -24,19 +24,19 @@ namespace OpenXRVk return aznew Session; } - AZ::RHI::ResultCode Session::InitInternal(AZ::RHI::XRSessionDescriptor* descriptor) + AZ::RHI::ResultCode Session::InitInternal([[maybe_unused]] AZ::RHI::XRSessionDescriptor* descriptor) { - AZ::Vulkan::XRSessionDescriptor* sessionDescriptor = static_cast(descriptor); Instance* xrVkInstance = static_cast(GetDescriptor().m_instance.get()); Device* xrVkDevice = static_cast(GetDescriptor().m_device.get()); + auto graphicBinding = xrVkDevice->GetGraphicsBinding(AZ::RHI::HardwareQueueClass::Graphics); 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; + m_graphicsBinding.queueFamilyIndex = graphicBinding.m_queueFamilyIndex; + m_graphicsBinding.queueIndex = graphicBinding.m_queueIndex; AZ_Assert(m_xrInstance != XR_NULL_HANDLE, "XR instance is null."); AZ_Assert(m_session == XR_NULL_HANDLE, "XR session is already initialized."); diff --git a/Gems/OpenXRVk/Code/Source/OpenXRVkSwapChain.cpp b/Gems/OpenXRVk/Code/Source/OpenXRVkSwapChain.cpp index d1dbb1dfc..1e6eb9544 100644 --- a/Gems/OpenXRVk/Code/Source/OpenXRVkSwapChain.cpp +++ b/Gems/OpenXRVk/Code/Source/OpenXRVkSwapChain.cpp @@ -241,7 +241,7 @@ namespace OpenXRVk VkFormat SwapChain::SelectColorSwapChainFormat(const AZStd::vector& runtimeFormats) const { // List of supported color swapchain formats. - constexpr int64_t SupportedColorSwapchainFormats[] = { VK_FORMAT_B8G8R8A8_UNORM, VK_FORMAT_R8G8B8A8_UNORM }; + constexpr int64_t SupportedColorSwapchainFormats[] = { VK_FORMAT_B8G8R8A8_UNORM, VK_FORMAT_R8G8B8A8_UNORM, VK_FORMAT_R8G8B8A8_SRGB }; auto swapchainFormatIt = AZStd::find_first_of(runtimeFormats.begin(), runtimeFormats.end(), AZStd::begin(SupportedColorSwapchainFormats), diff --git a/Gems/OpenXRVk/Code/Source/OpenXRVkSystemComponent.cpp b/Gems/OpenXRVk/Code/Source/OpenXRVkSystemComponent.cpp index e826317ff..478074807 100644 --- a/Gems/OpenXRVk/Code/Source/OpenXRVkSystemComponent.cpp +++ b/Gems/OpenXRVk/Code/Source/OpenXRVkSystemComponent.cpp @@ -9,6 +9,8 @@ #include +#include + #include #include #include @@ -17,11 +19,14 @@ #include #include +#include + 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 SystemComponent::CreateInstance() { return Instance::Create(); @@ -88,9 +82,33 @@ namespace OpenXRVk void SystemComponent::Activate() { + if (XR::IsOpenXREnabled()) + { + m_instance = AZStd::static_pointer_cast(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::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::Unregister(m_instance.get()); + m_instance = nullptr; + } } } \ No newline at end of file diff --git a/Gems/OpenXRVk/Code/Source/OpenXRVkUtils.cpp b/Gems/OpenXRVk/Code/Source/OpenXRVkUtils.cpp index bf066e2e4..32215b376 100644 --- a/Gems/OpenXRVk/Code/Source/OpenXRVkUtils.cpp +++ b/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; - } - } } diff --git a/Gems/OpenXRVk/gem.json b/Gems/OpenXRVk/gem.json index 1bc556e4d..ef882b555 100644 --- a/Gems/OpenXRVk/gem.json +++ b/Gems/OpenXRVk/gem.json @@ -13,7 +13,8 @@ "user_tags": [], "requirements": "", "documentation_url": "", - "dependencies": [ - "Atom_RPI" - ] + "dependencies": [], + "repo_uri": "https://raw.githubusercontent.com/o3de/o3de-extras/development", + "download_source_uri": "https://github.com/o3de/o3de-extras/releases/download/2.0/openxrvk-1.0.0-gem.zip", + "version": "1.0.0" } diff --git a/Gems/ProteusRobot/CMakeLists.txt b/Gems/ProteusRobot/CMakeLists.txt index e604ecccf..798cc8678 100644 --- a/Gems/ProteusRobot/CMakeLists.txt +++ b/Gems/ProteusRobot/CMakeLists.txt @@ -1,22 +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 # Query the gem name from the gem.json file if possible # otherwise fallback to using ProteusRobot -o3de_find_ancestor_gem_root(gem_path gem_name "${CMAKE_CURRENT_SOURCE_DIR}") +o3de_find_ancestor_gem_root(gem_root_path gem_name "${CMAKE_CURRENT_SOURCE_DIR}") if (NOT gem_name) set(gem_name "ProteusRobot") endif() -# Fallback to using the current source CMakeLists.txt directory as the gem root path -if (NOT gem_path) - set(gem_path ${CMAKE_CURRENT_SOURCE_DIR}) -endif() - -set(gem_json ${gem_path}/gem.json) - -o3de_restricted_path(${gem_json} gem_restricted_path gem_parent_relative_path) +# This indicates to the Builders applications(AssetProcessor, AssetBuilder, AssetBundler) +# that the gem should be added to the "cmake_dependencies..assetbuilder.setreg" +# which is generated when cmake configure occurs. +# Also tooling applications such as the Editor needs the CMake alias +# to see the gem as active +if(PAL_TRAIT_BUILD_HOST_TOOLS) + ly_create_alias(NAME ${gem_name}.Builders NAMESPACE Gem) + ly_create_alias(NAME ${gem_name}.Tools NAMESPACE Gem) -o3de_pal_dir(pal_dir ${CMAKE_CURRENT_SOURCE_DIR}/Platform/${PAL_PLATFORM_NAME} "${gem_restricted_path}" "${gem_path}" "${gem_parent_relative_path}") - -ly_add_external_target_path(${CMAKE_CURRENT_SOURCE_DIR}/3rdParty) - -add_subdirectory(Code) + # Add in CMake dependencies for each gem dependency listed in this gem's gem.json file + # for the Tools and Builders gem variants + o3de_add_variant_dependencies_for_gem_dependencies(GEM_NAME ${gem_name} VARIANTS Tools Builders) +endif() diff --git a/Gems/ProteusRobot/Code/Platform/Android/PAL_android.cmake b/Gems/ProteusRobot/Code/Platform/Android/PAL_android.cmake deleted file mode 100644 index ebbf6148b..000000000 --- a/Gems/ProteusRobot/Code/Platform/Android/PAL_android.cmake +++ /dev/null @@ -1,4 +0,0 @@ - -set(PAL_TRAIT_PROTEUSROBOT_SUPPORTED TRUE) -set(PAL_TRAIT_PROTEUSROBOT_TEST_SUPPORTED FALSE) -set(PAL_TRAIT_PROTEUSROBOT_EDITOR_TEST_SUPPORTED FALSE) diff --git a/Gems/ProteusRobot/Code/Platform/Android/proteusrobot_api_files.cmake b/Gems/ProteusRobot/Code/Platform/Android/proteusrobot_api_files.cmake deleted file mode 100644 index f5526eeb3..000000000 --- a/Gems/ProteusRobot/Code/Platform/Android/proteusrobot_api_files.cmake +++ /dev/null @@ -1,3 +0,0 @@ - -set(FILES -) diff --git a/Gems/ProteusRobot/Code/Platform/Android/proteusrobot_private_files.cmake b/Gems/ProteusRobot/Code/Platform/Android/proteusrobot_private_files.cmake deleted file mode 100644 index f773e3f1d..000000000 --- a/Gems/ProteusRobot/Code/Platform/Android/proteusrobot_private_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for Android -# i.e. ../Source/Android/ProteusRobotAndroid.cpp -# ../Source/Android/ProteusRobotAndroid.h -# ../Include/Android/ProteusRobotAndroid.h - -set(FILES -) diff --git a/Gems/ProteusRobot/Code/Platform/Android/proteusrobot_shared_files.cmake b/Gems/ProteusRobot/Code/Platform/Android/proteusrobot_shared_files.cmake deleted file mode 100644 index f773e3f1d..000000000 --- a/Gems/ProteusRobot/Code/Platform/Android/proteusrobot_shared_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for Android -# i.e. ../Source/Android/ProteusRobotAndroid.cpp -# ../Source/Android/ProteusRobotAndroid.h -# ../Include/Android/ProteusRobotAndroid.h - -set(FILES -) diff --git a/Gems/ProteusRobot/Code/Platform/Linux/PAL_linux.cmake b/Gems/ProteusRobot/Code/Platform/Linux/PAL_linux.cmake deleted file mode 100644 index ebbf6148b..000000000 --- a/Gems/ProteusRobot/Code/Platform/Linux/PAL_linux.cmake +++ /dev/null @@ -1,4 +0,0 @@ - -set(PAL_TRAIT_PROTEUSROBOT_SUPPORTED TRUE) -set(PAL_TRAIT_PROTEUSROBOT_TEST_SUPPORTED FALSE) -set(PAL_TRAIT_PROTEUSROBOT_EDITOR_TEST_SUPPORTED FALSE) diff --git a/Gems/ProteusRobot/Code/Platform/Linux/proteusrobot_api_files.cmake b/Gems/ProteusRobot/Code/Platform/Linux/proteusrobot_api_files.cmake deleted file mode 100644 index f5526eeb3..000000000 --- a/Gems/ProteusRobot/Code/Platform/Linux/proteusrobot_api_files.cmake +++ /dev/null @@ -1,3 +0,0 @@ - -set(FILES -) diff --git a/Gems/ProteusRobot/Code/Platform/Linux/proteusrobot_editor_api_files.cmake b/Gems/ProteusRobot/Code/Platform/Linux/proteusrobot_editor_api_files.cmake deleted file mode 100644 index f5526eeb3..000000000 --- a/Gems/ProteusRobot/Code/Platform/Linux/proteusrobot_editor_api_files.cmake +++ /dev/null @@ -1,3 +0,0 @@ - -set(FILES -) diff --git a/Gems/ProteusRobot/Code/Platform/Linux/proteusrobot_private_files.cmake b/Gems/ProteusRobot/Code/Platform/Linux/proteusrobot_private_files.cmake deleted file mode 100644 index 65b415277..000000000 --- a/Gems/ProteusRobot/Code/Platform/Linux/proteusrobot_private_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for Linux -# i.e. ../Source/Linux/ProteusRobotLinux.cpp -# ../Source/Linux/ProteusRobotLinux.h -# ../Include/Linux/ProteusRobotLinux.h - -set(FILES -) diff --git a/Gems/ProteusRobot/Code/Platform/Linux/proteusrobot_shared_files.cmake b/Gems/ProteusRobot/Code/Platform/Linux/proteusrobot_shared_files.cmake deleted file mode 100644 index 65b415277..000000000 --- a/Gems/ProteusRobot/Code/Platform/Linux/proteusrobot_shared_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for Linux -# i.e. ../Source/Linux/ProteusRobotLinux.cpp -# ../Source/Linux/ProteusRobotLinux.h -# ../Include/Linux/ProteusRobotLinux.h - -set(FILES -) diff --git a/Gems/ProteusRobot/Code/Platform/Mac/PAL_mac.cmake b/Gems/ProteusRobot/Code/Platform/Mac/PAL_mac.cmake deleted file mode 100644 index ebbf6148b..000000000 --- a/Gems/ProteusRobot/Code/Platform/Mac/PAL_mac.cmake +++ /dev/null @@ -1,4 +0,0 @@ - -set(PAL_TRAIT_PROTEUSROBOT_SUPPORTED TRUE) -set(PAL_TRAIT_PROTEUSROBOT_TEST_SUPPORTED FALSE) -set(PAL_TRAIT_PROTEUSROBOT_EDITOR_TEST_SUPPORTED FALSE) diff --git a/Gems/ProteusRobot/Code/Platform/Mac/proteusrobot_api_files.cmake b/Gems/ProteusRobot/Code/Platform/Mac/proteusrobot_api_files.cmake deleted file mode 100644 index f5526eeb3..000000000 --- a/Gems/ProteusRobot/Code/Platform/Mac/proteusrobot_api_files.cmake +++ /dev/null @@ -1,3 +0,0 @@ - -set(FILES -) diff --git a/Gems/ProteusRobot/Code/Platform/Mac/proteusrobot_editor_api_files.cmake b/Gems/ProteusRobot/Code/Platform/Mac/proteusrobot_editor_api_files.cmake deleted file mode 100644 index f5526eeb3..000000000 --- a/Gems/ProteusRobot/Code/Platform/Mac/proteusrobot_editor_api_files.cmake +++ /dev/null @@ -1,3 +0,0 @@ - -set(FILES -) diff --git a/Gems/ProteusRobot/Code/Platform/Mac/proteusrobot_private_files.cmake b/Gems/ProteusRobot/Code/Platform/Mac/proteusrobot_private_files.cmake deleted file mode 100644 index 002dfd77d..000000000 --- a/Gems/ProteusRobot/Code/Platform/Mac/proteusrobot_private_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for Mac -# i.e. ../Source/Mac/ProteusRobotMac.cpp -# ../Source/Mac/ProteusRobotMac.h -# ../Include/Mac/ProteusRobotMac.h - -set(FILES -) diff --git a/Gems/ProteusRobot/Code/Platform/Mac/proteusrobot_shared_files.cmake b/Gems/ProteusRobot/Code/Platform/Mac/proteusrobot_shared_files.cmake deleted file mode 100644 index 002dfd77d..000000000 --- a/Gems/ProteusRobot/Code/Platform/Mac/proteusrobot_shared_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for Mac -# i.e. ../Source/Mac/ProteusRobotMac.cpp -# ../Source/Mac/ProteusRobotMac.h -# ../Include/Mac/ProteusRobotMac.h - -set(FILES -) diff --git a/Gems/ProteusRobot/Code/Platform/Windows/PAL_windows.cmake b/Gems/ProteusRobot/Code/Platform/Windows/PAL_windows.cmake deleted file mode 100644 index ebbf6148b..000000000 --- a/Gems/ProteusRobot/Code/Platform/Windows/PAL_windows.cmake +++ /dev/null @@ -1,4 +0,0 @@ - -set(PAL_TRAIT_PROTEUSROBOT_SUPPORTED TRUE) -set(PAL_TRAIT_PROTEUSROBOT_TEST_SUPPORTED FALSE) -set(PAL_TRAIT_PROTEUSROBOT_EDITOR_TEST_SUPPORTED FALSE) diff --git a/Gems/ProteusRobot/Code/Platform/Windows/proteusrobot_api_files.cmake b/Gems/ProteusRobot/Code/Platform/Windows/proteusrobot_api_files.cmake deleted file mode 100644 index f5526eeb3..000000000 --- a/Gems/ProteusRobot/Code/Platform/Windows/proteusrobot_api_files.cmake +++ /dev/null @@ -1,3 +0,0 @@ - -set(FILES -) diff --git a/Gems/ProteusRobot/Code/Platform/Windows/proteusrobot_editor_api_files.cmake b/Gems/ProteusRobot/Code/Platform/Windows/proteusrobot_editor_api_files.cmake deleted file mode 100644 index f5526eeb3..000000000 --- a/Gems/ProteusRobot/Code/Platform/Windows/proteusrobot_editor_api_files.cmake +++ /dev/null @@ -1,3 +0,0 @@ - -set(FILES -) diff --git a/Gems/ProteusRobot/Code/Platform/Windows/proteusrobot_private_files.cmake b/Gems/ProteusRobot/Code/Platform/Windows/proteusrobot_private_files.cmake deleted file mode 100644 index 730b236e2..000000000 --- a/Gems/ProteusRobot/Code/Platform/Windows/proteusrobot_private_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for Windows -# i.e. ../Source/Windows/ProteusRobotWindows.cpp -# ../Source/Windows/ProteusRobotWindows.h -# ../Include/Windows/ProteusRobotWindows.h - -set(FILES -) diff --git a/Gems/ProteusRobot/Code/Platform/Windows/proteusrobot_shared_files.cmake b/Gems/ProteusRobot/Code/Platform/Windows/proteusrobot_shared_files.cmake deleted file mode 100644 index 730b236e2..000000000 --- a/Gems/ProteusRobot/Code/Platform/Windows/proteusrobot_shared_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for Windows -# i.e. ../Source/Windows/ProteusRobotWindows.cpp -# ../Source/Windows/ProteusRobotWindows.h -# ../Include/Windows/ProteusRobotWindows.h - -set(FILES -) diff --git a/Gems/ProteusRobot/Code/Platform/iOS/PAL_ios.cmake b/Gems/ProteusRobot/Code/Platform/iOS/PAL_ios.cmake deleted file mode 100644 index ebbf6148b..000000000 --- a/Gems/ProteusRobot/Code/Platform/iOS/PAL_ios.cmake +++ /dev/null @@ -1,4 +0,0 @@ - -set(PAL_TRAIT_PROTEUSROBOT_SUPPORTED TRUE) -set(PAL_TRAIT_PROTEUSROBOT_TEST_SUPPORTED FALSE) -set(PAL_TRAIT_PROTEUSROBOT_EDITOR_TEST_SUPPORTED FALSE) diff --git a/Gems/ProteusRobot/Code/Platform/iOS/proteusrobot_api_files.cmake b/Gems/ProteusRobot/Code/Platform/iOS/proteusrobot_api_files.cmake deleted file mode 100644 index f5526eeb3..000000000 --- a/Gems/ProteusRobot/Code/Platform/iOS/proteusrobot_api_files.cmake +++ /dev/null @@ -1,3 +0,0 @@ - -set(FILES -) diff --git a/Gems/ProteusRobot/Code/Platform/iOS/proteusrobot_private_files.cmake b/Gems/ProteusRobot/Code/Platform/iOS/proteusrobot_private_files.cmake deleted file mode 100644 index 52c79c1af..000000000 --- a/Gems/ProteusRobot/Code/Platform/iOS/proteusrobot_private_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for iOS -# i.e. ../Source/iOS/ProteusRobotiOS.cpp -# ../Source/iOS/ProteusRobotiOS.h -# ../Include/iOS/ProteusRobotiOS.h - -set(FILES -) diff --git a/Gems/ProteusRobot/Code/Platform/iOS/proteusrobot_shared_files.cmake b/Gems/ProteusRobot/Code/Platform/iOS/proteusrobot_shared_files.cmake deleted file mode 100644 index 52c79c1af..000000000 --- a/Gems/ProteusRobot/Code/Platform/iOS/proteusrobot_shared_files.cmake +++ /dev/null @@ -1,8 +0,0 @@ - -# Platform specific files for iOS -# i.e. ../Source/iOS/ProteusRobotiOS.cpp -# ../Source/iOS/ProteusRobotiOS.h -# ../Include/iOS/ProteusRobotiOS.h - -set(FILES -) diff --git a/Gems/ProteusRobot/Code/Source/Clients/ProteusRobotModule.cpp b/Gems/ProteusRobot/Code/Source/Clients/ProteusRobotModule.cpp deleted file mode 100644 index b840e71b2..000000000 --- a/Gems/ProteusRobot/Code/Source/Clients/ProteusRobotModule.cpp +++ /dev/null @@ -1,17 +0,0 @@ - - -#include -#include "ProteusRobotSystemComponent.h" - -namespace ProteusRobot -{ - class ProteusRobotModule - : public ProteusRobotModuleInterface - { - public: - AZ_RTTI(ProteusRobotModule, "{F9558D3E-566B-4824-8634-015F21864F5E}", ProteusRobotModuleInterface); - AZ_CLASS_ALLOCATOR(ProteusRobotModule, AZ::SystemAllocator); - }; -}// namespace ProteusRobot - -AZ_DECLARE_MODULE_CLASS(Gem_ProteusRobot, ProteusRobot::ProteusRobotModule) diff --git a/Gems/ProteusRobot/Code/Source/Clients/ProteusRobotSystemComponent.cpp b/Gems/ProteusRobot/Code/Source/Clients/ProteusRobotSystemComponent.cpp deleted file mode 100644 index 463dd313f..000000000 --- a/Gems/ProteusRobot/Code/Source/Clients/ProteusRobotSystemComponent.cpp +++ /dev/null @@ -1,83 +0,0 @@ - -#include "ProteusRobotSystemComponent.h" - -#include -#include -#include - -namespace ProteusRobot -{ - void ProteusRobotSystemComponent::Reflect(AZ::ReflectContext* context) - { - if (AZ::SerializeContext* serialize = azrtti_cast(context)) - { - serialize->Class() - ->Version(0) - ; - - if (AZ::EditContext* ec = serialize->GetEditContext()) - { - ec->Class("ProteusRobot", "[Description of functionality provided by this System Component]") - ->ClassElement(AZ::Edit::ClassElements::EditorData, "") - ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("System")) - ->Attribute(AZ::Edit::Attributes::AutoExpand, true) - ; - } - } - } - - void ProteusRobotSystemComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) - { - provided.push_back(AZ_CRC_CE("ProteusRobotService")); - } - - void ProteusRobotSystemComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) - { - incompatible.push_back(AZ_CRC_CE("ProteusRobotService")); - } - - void ProteusRobotSystemComponent::GetRequiredServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) - { - } - - void ProteusRobotSystemComponent::GetDependentServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) - { - } - - ProteusRobotSystemComponent::ProteusRobotSystemComponent() - { - if (ProteusRobotInterface::Get() == nullptr) - { - ProteusRobotInterface::Register(this); - } - } - - ProteusRobotSystemComponent::~ProteusRobotSystemComponent() - { - if (ProteusRobotInterface::Get() == this) - { - ProteusRobotInterface::Unregister(this); - } - } - - void ProteusRobotSystemComponent::Init() - { - } - - void ProteusRobotSystemComponent::Activate() - { - ProteusRobotRequestBus::Handler::BusConnect(); - AZ::TickBus::Handler::BusConnect(); - } - - void ProteusRobotSystemComponent::Deactivate() - { - AZ::TickBus::Handler::BusDisconnect(); - ProteusRobotRequestBus::Handler::BusDisconnect(); - } - - void ProteusRobotSystemComponent::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time) - { - } - -} // namespace ProteusRobot diff --git a/Gems/ProteusRobot/Code/Source/Tools/ProteusRobotEditorModule.cpp b/Gems/ProteusRobot/Code/Source/Tools/ProteusRobotEditorModule.cpp deleted file mode 100644 index 4f3807f08..000000000 --- a/Gems/ProteusRobot/Code/Source/Tools/ProteusRobotEditorModule.cpp +++ /dev/null @@ -1,38 +0,0 @@ - -#include -#include "ProteusRobotEditorSystemComponent.h" - -namespace ProteusRobot -{ - class ProteusRobotEditorModule - : public ProteusRobotModuleInterface - { - public: - AZ_RTTI(ProteusRobotEditorModule, "{F9558D3E-566B-4824-8634-015F21864F5E}", ProteusRobotModuleInterface); - AZ_CLASS_ALLOCATOR(ProteusRobotEditorModule, AZ::SystemAllocator); - - ProteusRobotEditorModule() - { - // Push results of [MyComponent]::CreateDescriptor() into m_descriptors here. - // Add ALL components descriptors associated with this gem to m_descriptors. - // This will associate the AzTypeInfo information for the components with the the SerializeContext, BehaviorContext and EditContext. - // This happens through the [MyComponent]::Reflect() function. - m_descriptors.insert(m_descriptors.end(), { - ProteusRobotEditorSystemComponent::CreateDescriptor(), - }); - } - - /** - * Add required SystemComponents to the SystemEntity. - * Non-SystemComponents should not be added here - */ - AZ::ComponentTypeList GetRequiredSystemComponents() const override - { - return AZ::ComponentTypeList { - azrtti_typeid(), - }; - } - }; -}// namespace ProteusRobot - -AZ_DECLARE_MODULE_CLASS(Gem_ProteusRobot, ProteusRobot::ProteusRobotEditorModule) diff --git a/Gems/ProteusRobot/Code/Source/Tools/ProteusRobotEditorSystemComponent.cpp b/Gems/ProteusRobot/Code/Source/Tools/ProteusRobotEditorSystemComponent.cpp deleted file mode 100644 index 938f919d7..000000000 --- a/Gems/ProteusRobot/Code/Source/Tools/ProteusRobotEditorSystemComponent.cpp +++ /dev/null @@ -1,54 +0,0 @@ - -#include -#include "ProteusRobotEditorSystemComponent.h" - -namespace ProteusRobot -{ - void ProteusRobotEditorSystemComponent::Reflect(AZ::ReflectContext* context) - { - if (auto serializeContext = azrtti_cast(context)) - { - serializeContext->Class() - ->Version(0); - } - } - - ProteusRobotEditorSystemComponent::ProteusRobotEditorSystemComponent() = default; - - ProteusRobotEditorSystemComponent::~ProteusRobotEditorSystemComponent() = default; - - void ProteusRobotEditorSystemComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) - { - BaseSystemComponent::GetProvidedServices(provided); - provided.push_back(AZ_CRC_CE("ProteusRobotEditorService")); - } - - void ProteusRobotEditorSystemComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) - { - BaseSystemComponent::GetIncompatibleServices(incompatible); - incompatible.push_back(AZ_CRC_CE("ProteusRobotEditorService")); - } - - void ProteusRobotEditorSystemComponent::GetRequiredServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) - { - BaseSystemComponent::GetRequiredServices(required); - } - - void ProteusRobotEditorSystemComponent::GetDependentServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) - { - BaseSystemComponent::GetDependentServices(dependent); - } - - void ProteusRobotEditorSystemComponent::Activate() - { - ProteusRobotSystemComponent::Activate(); - AzToolsFramework::EditorEvents::Bus::Handler::BusConnect(); - } - - void ProteusRobotEditorSystemComponent::Deactivate() - { - AzToolsFramework::EditorEvents::Bus::Handler::BusDisconnect(); - ProteusRobotSystemComponent::Deactivate(); - } - -} // namespace ProteusRobot diff --git a/Gems/ProteusRobot/Code/Source/Tools/ProteusRobotEditorSystemComponent.h b/Gems/ProteusRobot/Code/Source/Tools/ProteusRobotEditorSystemComponent.h deleted file mode 100644 index 5da317550..000000000 --- a/Gems/ProteusRobot/Code/Source/Tools/ProteusRobotEditorSystemComponent.h +++ /dev/null @@ -1,33 +0,0 @@ - -#pragma once - -#include - -#include - -namespace ProteusRobot -{ - /// System component for ProteusRobot editor - class ProteusRobotEditorSystemComponent - : public ProteusRobotSystemComponent - , protected AzToolsFramework::EditorEvents::Bus::Handler - { - using BaseSystemComponent = ProteusRobotSystemComponent; - public: - AZ_COMPONENT(ProteusRobotEditorSystemComponent, "{1AD25C4B-B8F7-4C54-BF46-3F5A9E02E90B}", BaseSystemComponent); - static void Reflect(AZ::ReflectContext* context); - - ProteusRobotEditorSystemComponent(); - ~ProteusRobotEditorSystemComponent(); - - private: - static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); - static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible); - static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); - static void GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent); - - // AZ::Component - void Activate() override; - void Deactivate() override; - }; -} // namespace ProteusRobot diff --git a/Gems/ProteusRobot/Code/Tests/Clients/ProteusRobotTest.cpp b/Gems/ProteusRobot/Code/Tests/Clients/ProteusRobotTest.cpp deleted file mode 100644 index 274a99080..000000000 --- a/Gems/ProteusRobot/Code/Tests/Clients/ProteusRobotTest.cpp +++ /dev/null @@ -1,4 +0,0 @@ - -#include - -AZ_UNIT_TEST_HOOK(DEFAULT_UNIT_TEST_ENV); diff --git a/Gems/ProteusRobot/Code/Tests/Tools/ProteusRobotEditorTest.cpp b/Gems/ProteusRobot/Code/Tests/Tools/ProteusRobotEditorTest.cpp deleted file mode 100644 index 274a99080..000000000 --- a/Gems/ProteusRobot/Code/Tests/Tools/ProteusRobotEditorTest.cpp +++ /dev/null @@ -1,4 +0,0 @@ - -#include - -AZ_UNIT_TEST_HOOK(DEFAULT_UNIT_TEST_ENV); diff --git a/Gems/ProteusRobot/Code/proteusrobot_api_files.cmake b/Gems/ProteusRobot/Code/proteusrobot_api_files.cmake deleted file mode 100644 index 850825091..000000000 --- a/Gems/ProteusRobot/Code/proteusrobot_api_files.cmake +++ /dev/null @@ -1,4 +0,0 @@ - -set(FILES - Include/ProteusRobot/ProteusRobotBus.h -) diff --git a/Gems/ProteusRobot/Code/proteusrobot_editor_api_files.cmake b/Gems/ProteusRobot/Code/proteusrobot_editor_api_files.cmake deleted file mode 100644 index 8cd37a5cf..000000000 --- a/Gems/ProteusRobot/Code/proteusrobot_editor_api_files.cmake +++ /dev/null @@ -1,4 +0,0 @@ - - -set(FILES -) diff --git a/Gems/ProteusRobot/Code/proteusrobot_editor_private_files.cmake b/Gems/ProteusRobot/Code/proteusrobot_editor_private_files.cmake deleted file mode 100644 index f5117c474..000000000 --- a/Gems/ProteusRobot/Code/proteusrobot_editor_private_files.cmake +++ /dev/null @@ -1,5 +0,0 @@ - -set(FILES - Source/Tools/ProteusRobotEditorSystemComponent.cpp - Source/Tools/ProteusRobotEditorSystemComponent.h -) diff --git a/Gems/ProteusRobot/Code/proteusrobot_editor_shared_files.cmake b/Gems/ProteusRobot/Code/proteusrobot_editor_shared_files.cmake deleted file mode 100644 index d84920316..000000000 --- a/Gems/ProteusRobot/Code/proteusrobot_editor_shared_files.cmake +++ /dev/null @@ -1,4 +0,0 @@ - -set(FILES - Source/Tools/ProteusRobotEditorModule.cpp -) diff --git a/Gems/ProteusRobot/Code/proteusrobot_editor_tests_files.cmake b/Gems/ProteusRobot/Code/proteusrobot_editor_tests_files.cmake deleted file mode 100644 index 19d5b4ac9..000000000 --- a/Gems/ProteusRobot/Code/proteusrobot_editor_tests_files.cmake +++ /dev/null @@ -1,4 +0,0 @@ - -set(FILES - Tests/Tools/ProteusRobotEditorTest.cpp -) diff --git a/Gems/ProteusRobot/Code/proteusrobot_private_files.cmake b/Gems/ProteusRobot/Code/proteusrobot_private_files.cmake deleted file mode 100644 index 6a251865e..000000000 --- a/Gems/ProteusRobot/Code/proteusrobot_private_files.cmake +++ /dev/null @@ -1,6 +0,0 @@ - -set(FILES - Source/ProteusRobotModuleInterface.h - Source/Clients/ProteusRobotSystemComponent.cpp - Source/Clients/ProteusRobotSystemComponent.h -) diff --git a/Gems/ProteusRobot/Code/proteusrobot_shared_files.cmake b/Gems/ProteusRobot/Code/proteusrobot_shared_files.cmake deleted file mode 100644 index 819855914..000000000 --- a/Gems/ProteusRobot/Code/proteusrobot_shared_files.cmake +++ /dev/null @@ -1,4 +0,0 @@ - -set(FILES - Source/Clients/ProteusRobotModule.cpp -) diff --git a/Gems/ProteusRobot/Code/proteusrobot_tests_files.cmake b/Gems/ProteusRobot/Code/proteusrobot_tests_files.cmake deleted file mode 100644 index e55f73955..000000000 --- a/Gems/ProteusRobot/Code/proteusrobot_tests_files.cmake +++ /dev/null @@ -1,4 +0,0 @@ - -set(FILES - Tests/Clients/ProteusRobotTest.cpp -) diff --git a/Gems/ProteusRobot/gem.json b/Gems/ProteusRobot/gem.json index fde828a18..6f774e333 100644 --- a/Gems/ProteusRobot/gem.json +++ b/Gems/ProteusRobot/gem.json @@ -1,13 +1,13 @@ { "gem_name": "ProteusRobot", "version": "1.0.0", - "display_name": "ProteusRobot", - "license": "License used i.e. Apache-2.0 or MIT", - "license_url": "Link to the license web site i.e. https://opensource.org/licenses/Apache-2.0", - "origin": "The name of the originator or creator", + "display_name": "Proteus Robot", + "license": "Apache-2.0", + "license_url": "https://opensource.org/licenses/Apache-2.0", + "origin": "RobotecAI", "origin_url": "https://github.com/o3de/o3de-extras/tree/development/Gems/ProteusRobot", - "type": "Code", - "summary": "Proteus: Warehouse robot with Lidar sensor", + "type": "Asset", + "summary": "Proteus warehouse robot with Lidar sensor", "canonical_tags": [ "Gem" ], @@ -18,11 +18,15 @@ "" ], "icon_path": "preview.png", - "requirements": "Notice of any requirements for this Gem i.e. This requires X other gem", - "documentation_url": "Link to any documentation of your Gem", + "requirements": "Requires ROS 2 Gem for the Lidar to function", + "documentation_url": "https://www.o3de.org/docs/user-guide/interactivity/robotics/project-configuration/#ros-2-project-templates", "dependencies": [], - "repo_uri": "", - "compatible_engines": [], + "repo_uri": "https://raw.githubusercontent.com/o3de/o3de-extras/development", + "compatible_engines": [ + "o3de-sdk>=1.2.0", + "o3de>=1.2.0" + ], "engine_api_dependencies": [], - "restricted": "ProteusRobot" + "restricted": "ProteusRobot", + "download_source_uri": "https://github.com/o3de/o3de-extras/releases/download/2.0/proteusrobot-1.0.0-gem.zip" } diff --git a/Gems/ProteusRobot/preview.png b/Gems/ProteusRobot/preview.png index 7d6835f1d..049874ed0 100644 --- a/Gems/ProteusRobot/preview.png +++ b/Gems/ProteusRobot/preview.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:4e2408737fe2bba9016b8bfaf0e09ba7624ab298720c51307a51cec1c816ebd7 -size 125915 +oid sha256:4f1c54b47691f37f59463ff5926c44bba4783f526512485704167502d840f76c +size 19282 diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/AckermannControl.svg b/Gems/ROS2/Assets/Editor/Icons/Components/AckermannControl.svg new file mode 100644 index 000000000..b622b9826 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/AckermannControl.svg @@ -0,0 +1,6 @@ + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/AckermannVehicleModel.svg b/Gems/ROS2/Assets/Editor/Icons/Components/AckermannVehicleModel.svg new file mode 100644 index 000000000..1183ecc98 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/AckermannVehicleModel.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/FingerGripperComponent.svg b/Gems/ROS2/Assets/Editor/Icons/Components/FingerGripperComponent.svg new file mode 100644 index 000000000..12b45c14e --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/FingerGripperComponent.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/GripperActionServerComponent.svg b/Gems/ROS2/Assets/Editor/Icons/Components/GripperActionServerComponent.svg new file mode 100644 index 000000000..e9155192d --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/GripperActionServerComponent.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/JointsArticulationControllerComponent.svg b/Gems/ROS2/Assets/Editor/Icons/Components/JointsArticulationControllerComponent.svg new file mode 100644 index 000000000..e52e149f9 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/JointsArticulationControllerComponent.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/JointsManipulationEditorComponent.svg b/Gems/ROS2/Assets/Editor/Icons/Components/JointsManipulationEditorComponent.svg new file mode 100644 index 000000000..ece4c3eda --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/JointsManipulationEditorComponent.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/JointsPIDControllerComponent.svg b/Gems/ROS2/Assets/Editor/Icons/Components/JointsPIDControllerComponent.svg new file mode 100644 index 000000000..641a7000f --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/JointsPIDControllerComponent.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/JointsTrajectoryComponent.svg b/Gems/ROS2/Assets/Editor/Icons/Components/JointsTrajectoryComponent.svg new file mode 100644 index 000000000..fa789a636 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/JointsTrajectoryComponent.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/ManualMotorController.svg b/Gems/ROS2/Assets/Editor/Icons/Components/ManualMotorController.svg new file mode 100644 index 000000000..f4d13eb01 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/ManualMotorController.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/PidMotorController.svg b/Gems/ROS2/Assets/Editor/Icons/Components/PidMotorController.svg new file mode 100644 index 000000000..eee5bad55 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/PidMotorController.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/ROS2CameraSensor.svg b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2CameraSensor.svg new file mode 100644 index 000000000..aada9414a --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2CameraSensor.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/ROS2ContactSensor.svg b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2ContactSensor.svg new file mode 100644 index 000000000..3539bf51a --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2ContactSensor.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/ROS2Frame.svg b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2Frame.svg new file mode 100644 index 000000000..1c5546deb --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2Frame.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/ROS2GNSSSensor.svg b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2GNSSSensor.svg new file mode 100644 index 000000000..b5225028f --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2GNSSSensor.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/ROS2ImuSensor.svg b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2ImuSensor.svg new file mode 100644 index 000000000..c53770fe7 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2ImuSensor.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/ROS2Lidar2DSensor.svg b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2Lidar2DSensor.svg new file mode 100644 index 000000000..ccb95f3bb --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2Lidar2DSensor.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/ROS2LidarSensor.svg b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2LidarSensor.svg new file mode 100644 index 000000000..9e42fb610 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2LidarSensor.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/ROS2OdometrySensor.svg b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2OdometrySensor.svg new file mode 100644 index 000000000..a4c5c4119 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2OdometrySensor.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/ROS2RobotControl.svg b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2RobotControl.svg new file mode 100644 index 000000000..874adc8be --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2RobotControl.svg @@ -0,0 +1,5 @@ + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/ROS2SpawnPoint.svg b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2SpawnPoint.svg new file mode 100644 index 000000000..170aded46 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2SpawnPoint.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/ROS2Spawner.svg b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2Spawner.svg new file mode 100644 index 000000000..f63076742 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2Spawner.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/ROS2WheelOdometrySensor.svg b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2WheelOdometrySensor.svg new file mode 100644 index 000000000..8b5aafb3b --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/ROS2WheelOdometrySensor.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/RigidBodyTwistControl.svg b/Gems/ROS2/Assets/Editor/Icons/Components/RigidBodyTwistControl.svg new file mode 100644 index 000000000..8710e2c8c --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/RigidBodyTwistControl.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/SkidSteeringTwistControl.svg b/Gems/ROS2/Assets/Editor/Icons/Components/SkidSteeringTwistControl.svg new file mode 100644 index 000000000..b4f1470ec --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/SkidSteeringTwistControl.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/SkidSteeringVehicleModel.svg b/Gems/ROS2/Assets/Editor/Icons/Components/SkidSteeringVehicleModel.svg new file mode 100644 index 000000000..40ef129a7 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/SkidSteeringVehicleModel.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/VacuumGripperComponent.svg b/Gems/ROS2/Assets/Editor/Icons/Components/VacuumGripperComponent.svg new file mode 100644 index 000000000..f070253be --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/VacuumGripperComponent.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/AckermannControl.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/AckermannControl.svg new file mode 100644 index 000000000..89d0bc802 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/AckermannControl.svg @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/AckermannVehicleModel.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/AckermannVehicleModel.svg new file mode 100644 index 000000000..b375e3fb0 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/AckermannVehicleModel.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/FingerGripperComponent.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/FingerGripperComponent.svg new file mode 100644 index 000000000..f2709821c --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/FingerGripperComponent.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/GripperActionServerComponent.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/GripperActionServerComponent.svg new file mode 100644 index 000000000..0e78b5a3c --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/GripperActionServerComponent.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/JointsArticulationControllerComponent.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/JointsArticulationControllerComponent.svg new file mode 100644 index 000000000..23731f9d8 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/JointsArticulationControllerComponent.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/JointsManipulationEditorComponent.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/JointsManipulationEditorComponent.svg new file mode 100644 index 000000000..d901411c3 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/JointsManipulationEditorComponent.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/JointsPIDControllerComponent.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/JointsPIDControllerComponent.svg new file mode 100644 index 000000000..d72208ed8 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/JointsPIDControllerComponent.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/JointsTrajectoryComponent.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/JointsTrajectoryComponent.svg new file mode 100644 index 000000000..05858c124 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/JointsTrajectoryComponent.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ManualMotorController.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ManualMotorController.svg new file mode 100644 index 000000000..c0d80a566 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ManualMotorController.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/PidMotorController.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/PidMotorController.svg new file mode 100644 index 000000000..0b0ef2061 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/PidMotorController.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2CameraSensor.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2CameraSensor.svg new file mode 100644 index 000000000..53672114e --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2CameraSensor.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2ContactSensor.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2ContactSensor.svg new file mode 100644 index 000000000..5fa17d8f6 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2ContactSensor.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2Frame.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2Frame.svg new file mode 100644 index 000000000..7877d13e1 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2Frame.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2GNSSSensor.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2GNSSSensor.svg new file mode 100644 index 000000000..40db0b3bc --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2GNSSSensor.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2ImuSensor.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2ImuSensor.svg new file mode 100644 index 000000000..9741b1eaa --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2ImuSensor.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2Lidar2DSensor.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2Lidar2DSensor.svg new file mode 100644 index 000000000..5ab2cf536 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2Lidar2DSensor.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2LidarSensor.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2LidarSensor.svg new file mode 100644 index 000000000..c27a311db --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2LidarSensor.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2OdometrySensor.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2OdometrySensor.svg new file mode 100644 index 000000000..8f305263d --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2OdometrySensor.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2RobotControl.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2RobotControl.svg new file mode 100644 index 000000000..a066f8958 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2RobotControl.svg @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2SpawnPoint.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2SpawnPoint.svg new file mode 100644 index 000000000..e2d980fdc --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2SpawnPoint.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2Spawner.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2Spawner.svg new file mode 100644 index 000000000..7af94370b --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2Spawner.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2WheelOdometrySensor.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2WheelOdometrySensor.svg new file mode 100644 index 000000000..459be5111 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/ROS2WheelOdometrySensor.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/RigidBodyTwistControl.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/RigidBodyTwistControl.svg new file mode 100644 index 000000000..210b9c640 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/RigidBodyTwistControl.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/SkidSteeringTwistControl.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/SkidSteeringTwistControl.svg new file mode 100644 index 000000000..dbcd7b7f2 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/SkidSteeringTwistControl.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/SkidSteeringVehicleModel.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/SkidSteeringVehicleModel.svg new file mode 100644 index 000000000..b18af1cc8 --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/SkidSteeringVehicleModel.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/VacuumGripperComponent.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/VacuumGripperComponent.svg new file mode 100644 index 000000000..f9c726aea --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/VacuumGripperComponent.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/WheelController.svg b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/WheelController.svg new file mode 100644 index 000000000..928c464ab --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/Viewport/WheelController.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Gems/ROS2/Assets/Editor/Icons/Components/WheelController.svg b/Gems/ROS2/Assets/Editor/Icons/Components/WheelController.svg new file mode 100644 index 000000000..d57d3be7b --- /dev/null +++ b/Gems/ROS2/Assets/Editor/Icons/Components/WheelController.svg @@ -0,0 +1,3 @@ + + + diff --git a/Gems/ROS2/Assets/Gazebo/Black.material b/Gems/ROS2/Assets/Gazebo/Black.material new file mode 100644 index 000000000..5b59c0f47 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Black.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 0.0, + 0.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/BlackTransparent.material b/Gems/ROS2/Assets/Gazebo/BlackTransparent.material new file mode 100644 index 000000000..1f8e7fd47 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/BlackTransparent.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 0.0, + 0.0, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Blue.material b/Gems/ROS2/Assets/Gazebo/Blue.material new file mode 100644 index 000000000..4e5a9152e --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Blue.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 0.0, + 1.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/BlueGlow.material b/Gems/ROS2/Assets/Gazebo/BlueGlow.material new file mode 100644 index 000000000..fa253ccfc --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/BlueGlow.material @@ -0,0 +1,28 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 0.0, + 1.0, + 1.0 + ], + "emissive.enable": true, + "emissive.color": [ + 0.0, + 0.0, + 1.0, + 1.0 + ], + "emissive.intensity": 5.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/BlueLaser.material b/Gems/ROS2/Assets/Gazebo/BlueLaser.material new file mode 100644 index 000000000..803d0bdbf --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/BlueLaser.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 0.0, + 1.0, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.6 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/BlueTransparent.material b/Gems/ROS2/Assets/Gazebo/BlueTransparent.material new file mode 100644 index 000000000..3d671fb9a --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/BlueTransparent.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 0.0, + 1.0, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Bricks.material b/Gems/ROS2/Assets/Gazebo/Bricks.material new file mode 100644 index 000000000..b7179ebd7 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Bricks.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 1.0, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.6627, + 0.302, + 0.1568, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/CeilingTiled.material b/Gems/ROS2/Assets/Gazebo/CeilingTiled.material new file mode 100644 index 000000000..e9d6ee1d5 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/CeilingTiled.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.921, + 0.921, + 0.921, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/CloudySky.material b/Gems/ROS2/Assets/Gazebo/CloudySky.material new file mode 100644 index 000000000..9810399e8 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/CloudySky.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.1, + "metallic.factor": 0.6, + "baseColor.color": [ + 0.0, + 0.0, + 1.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/DarkGray.material b/Gems/ROS2/Assets/Gazebo/DarkGray.material new file mode 100644 index 000000000..726b549bb --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/DarkGray.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.825, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.175, + 0.175, + 0.175, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/DarkGrey.material b/Gems/ROS2/Assets/Gazebo/DarkGrey.material new file mode 100644 index 000000000..726b549bb --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/DarkGrey.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.825, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.175, + 0.175, + 0.175, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/DarkMagentaTransparent.material b/Gems/ROS2/Assets/Gazebo/DarkMagentaTransparent.material new file mode 100644 index 000000000..ddd33cac7 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/DarkMagentaTransparent.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.6, + 0.0, + 0.6, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/DarkYellow.material b/Gems/ROS2/Assets/Gazebo/DarkYellow.material new file mode 100644 index 000000000..edf802293 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/DarkYellow.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 1.0, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.7, + 0.7, + 0.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/FlatBlack.material b/Gems/ROS2/Assets/Gazebo/FlatBlack.material new file mode 100644 index 000000000..707889a7e --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/FlatBlack.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.99, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.1, + 0.1, + 0.1, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Footway.material b/Gems/ROS2/Assets/Gazebo/Footway.material new file mode 100644 index 000000000..3f75b925e --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Footway.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.95, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.607, + 0.607, + 0.5607, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Gold.material b/Gems/ROS2/Assets/Gazebo/Gold.material new file mode 100644 index 000000000..c04c701ff --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Gold.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.1, + "metallic.factor": 1.0, + "baseColor.color": [ + 0.8, + 0.64869, + 0.120759, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Grass.material b/Gems/ROS2/Assets/Gazebo/Grass.material new file mode 100644 index 000000000..ef191167a --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Grass.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 1.0, + 0.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Gray.material b/Gems/ROS2/Assets/Gazebo/Gray.material new file mode 100644 index 000000000..9fc720ed0 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Gray.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.99, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.7, + 0.7, + 0.7, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Green.material b/Gems/ROS2/Assets/Gazebo/Green.material new file mode 100644 index 000000000..ef191167a --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Green.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 1.0, + 0.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/GreenGlow.material b/Gems/ROS2/Assets/Gazebo/GreenGlow.material new file mode 100644 index 000000000..2b4193cc8 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/GreenGlow.material @@ -0,0 +1,28 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 1.0, + 0.0, + 1.0 + ], + "emissive.enable": true, + "emissive.color": [ + 0.0, + 1.0, + 0.0, + 1.0 + ], + "emissive.intensity": 5.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/GreenTransparent.material b/Gems/ROS2/Assets/Gazebo/GreenTransparent.material new file mode 100644 index 000000000..0e5045879 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/GreenTransparent.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 1.0, + 0.0, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Grey.material b/Gems/ROS2/Assets/Gazebo/Grey.material new file mode 100644 index 000000000..9fc720ed0 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Grey.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.99, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.7, + 0.7, + 0.7, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/GreyGradientSky.material b/Gems/ROS2/Assets/Gazebo/GreyGradientSky.material new file mode 100644 index 000000000..9fc720ed0 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/GreyGradientSky.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.99, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.7, + 0.7, + 0.7, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/GreyTransparent.material b/Gems/ROS2/Assets/Gazebo/GreyTransparent.material new file mode 100644 index 000000000..62014de7f --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/GreyTransparent.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.5, + 0.5, + 0.5, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Indigo.material b/Gems/ROS2/Assets/Gazebo/Indigo.material new file mode 100644 index 000000000..1c8560962 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Indigo.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.33, + 0.0, + 0.5, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/LightBlueLaser.material b/Gems/ROS2/Assets/Gazebo/LightBlueLaser.material new file mode 100644 index 000000000..2100545f2 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/LightBlueLaser.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.5, + 0.5, + 1.0, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.6 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/LightOff.material b/Gems/ROS2/Assets/Gazebo/LightOff.material new file mode 100644 index 000000000..f06493e99 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/LightOff.material @@ -0,0 +1,28 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 0.0, + 0.0, + 1.0 + ], + "emissive.enable": true, + "emissive.color": [ + 1.0, + 0.0, + 0.0, + 1.0 + ], + "emissive.intensity": 5.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/LightOn.material b/Gems/ROS2/Assets/Gazebo/LightOn.material new file mode 100644 index 000000000..2b4193cc8 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/LightOn.material @@ -0,0 +1,28 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 1.0, + 0.0, + 1.0 + ], + "emissive.enable": true, + "emissive.color": [ + 0.0, + 1.0, + 0.0, + 1.0 + ], + "emissive.intensity": 5.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Orange.material b/Gems/ROS2/Assets/Gazebo/Orange.material new file mode 100644 index 000000000..c1f496421 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Orange.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.5, + "metallic.factor": 0.125, + "baseColor.color": [ + 1.0, + 0.5088, + 0.0468, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/OrangeTransparent.material b/Gems/ROS2/Assets/Gazebo/OrangeTransparent.material new file mode 100644 index 000000000..5dc77b913 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/OrangeTransparent.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 0.44, + 0.0, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.6 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/PaintedWall.material b/Gems/ROS2/Assets/Gazebo/PaintedWall.material new file mode 100644 index 000000000..1fc08cdfb --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/PaintedWall.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 1.0, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 1.0, + 0.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Pedestrian.material b/Gems/ROS2/Assets/Gazebo/Pedestrian.material new file mode 100644 index 000000000..3f75b925e --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Pedestrian.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.95, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.607, + 0.607, + 0.5607, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Purple.material b/Gems/ROS2/Assets/Gazebo/Purple.material new file mode 100644 index 000000000..5264559ce --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Purple.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 0.0, + 1.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/PurpleGlow.material b/Gems/ROS2/Assets/Gazebo/PurpleGlow.material new file mode 100644 index 000000000..b21428140 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/PurpleGlow.material @@ -0,0 +1,28 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 0.0, + 1.0, + 1.0 + ], + "emissive.enable": true, + "emissive.color": [ + 1.0, + 0.0, + 1.0, + 1.0 + ], + "emissive.intensity": 5.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Red.material b/Gems/ROS2/Assets/Gazebo/Red.material new file mode 100644 index 000000000..6ea55fb08 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Red.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 0.0, + 0.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/RedBright.material b/Gems/ROS2/Assets/Gazebo/RedBright.material new file mode 100644 index 000000000..4f0f85fa5 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/RedBright.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.6, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.87, + 0.26, + 0.07, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/RedGlow.material b/Gems/ROS2/Assets/Gazebo/RedGlow.material new file mode 100644 index 000000000..f06493e99 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/RedGlow.material @@ -0,0 +1,28 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 0.0, + 0.0, + 1.0 + ], + "emissive.enable": true, + "emissive.color": [ + 1.0, + 0.0, + 0.0, + 1.0 + ], + "emissive.intensity": 5.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/RedTransparent.material b/Gems/ROS2/Assets/Gazebo/RedTransparent.material new file mode 100644 index 000000000..a2e72676d --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/RedTransparent.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 0.0, + 0.0, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Residential.material b/Gems/ROS2/Assets/Gazebo/Residential.material new file mode 100644 index 000000000..da4a3a865 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Residential.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.95, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.35, + 0.35, + 0.35, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Road.material b/Gems/ROS2/Assets/Gazebo/Road.material new file mode 100644 index 000000000..da4a3a865 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Road.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.95, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.35, + 0.35, + 0.35, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/SkyBlue.material b/Gems/ROS2/Assets/Gazebo/SkyBlue.material new file mode 100644 index 000000000..9810399e8 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/SkyBlue.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.1, + "metallic.factor": 0.6, + "baseColor.color": [ + 0.0, + 0.0, + 1.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Turquoise.material b/Gems/ROS2/Assets/Gazebo/Turquoise.material new file mode 100644 index 000000000..119c49828 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Turquoise.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 1.0, + 1.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/TurquoiseGlow.material b/Gems/ROS2/Assets/Gazebo/TurquoiseGlow.material new file mode 100644 index 000000000..2fceed5cd --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/TurquoiseGlow.material @@ -0,0 +1,28 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.0, + 1.0, + 1.0, + 1.0 + ], + "emissive.enable": true, + "emissive.color": [ + 0.0, + 1.0, + 1.0, + 1.0 + ], + "emissive.intensity": 5.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/White.material b/Gems/ROS2/Assets/Gazebo/White.material new file mode 100644 index 000000000..4e6330c76 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/White.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 1.0, + 1.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/WhiteGlow.material b/Gems/ROS2/Assets/Gazebo/WhiteGlow.material new file mode 100644 index 000000000..43bb814de --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/WhiteGlow.material @@ -0,0 +1,28 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 1.0, + 1.0, + 1.0 + ], + "emissive.enable": true, + "emissive.color": [ + 1.0, + 1.0, + 1.0, + 1.0 + ], + "emissive.intensity": 8.0 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Wood.material b/Gems/ROS2/Assets/Gazebo/Wood.material new file mode 100644 index 000000000..9784b3d7a --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Wood.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.1, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.698, + 0.4314, + 0.1921, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/WoodFloor.material b/Gems/ROS2/Assets/Gazebo/WoodFloor.material new file mode 100644 index 000000000..56a671b1b --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/WoodFloor.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.1, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.557, + 0.275, + 0.067, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/WoodPallet.material b/Gems/ROS2/Assets/Gazebo/WoodPallet.material new file mode 100644 index 000000000..a8e122d13 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/WoodPallet.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 0.6627, + 0.5, + 0.2784, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/Yellow.material b/Gems/ROS2/Assets/Gazebo/Yellow.material new file mode 100644 index 000000000..1fc08cdfb --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/Yellow.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 1.0, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 1.0, + 0.0, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Gazebo/YellowGlow.material b/Gems/ROS2/Assets/Gazebo/YellowGlow.material new file mode 100644 index 000000000..0830ed3b7 --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/YellowGlow.material @@ -0,0 +1,28 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 1.0, + 0.0, + 1.0 + ], + "emissive.enable": true, + "emissive.color": [ + 1.0, + 1.0, + 0.0, + 1.0 + ], + "emissive.intensity": 5.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/YellowTransparent.material b/Gems/ROS2/Assets/Gazebo/YellowTransparent.material new file mode 100644 index 000000000..24bc9eecf --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/YellowTransparent.material @@ -0,0 +1,23 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.9, + "metallic.factor": 0.0, + "baseColor.color": [ + 1.0, + 1.0, + 0.0, + 1.0 + ], + "opacity.mode": "Blended", + "opacity.alphaSource": "None", + "opacity.factor": 0.5 + } +} diff --git a/Gems/ROS2/Assets/Gazebo/ZincYellow.material b/Gems/ROS2/Assets/Gazebo/ZincYellow.material new file mode 100644 index 000000000..5a519f77a --- /dev/null +++ b/Gems/ROS2/Assets/Gazebo/ZincYellow.material @@ -0,0 +1,20 @@ +{ + "materialType": "Materials/Types/StandardPBR.materialtype", + "materialTypeVersion": 5, + "propertyValues": { + "metallic.useTexture": false, + "normal.useTexture": false, + "roughness.useTexture": false, + "specularF0.enableMultiScatterCompensation": true, + "specularF0.useTexture": false, + "baseColor.useTexture": false, + "roughness.factor": 0.05, + "metallic.factor": 1.0, + "baseColor.color": [ + 0.9725, + 0.9529, + 0.2078, + 1.0 + ] + } +} diff --git a/Gems/ROS2/Assets/Sdf/empty.sdf b/Gems/ROS2/Assets/Sdf/empty.sdf new file mode 100644 index 000000000..b9f8515a8 --- /dev/null +++ b/Gems/ROS2/Assets/Sdf/empty.sdf @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Gems/ROS2/Assets/seedList.seed b/Gems/ROS2/Assets/seedList.seed new file mode 100755 index 000000000..589ae3044 --- /dev/null +++ b/Gems/ROS2/Assets/seedList.seed @@ -0,0 +1,14 @@ +{ + "Type": "JsonSerialization", + "Version": 1, + "ClassName": "AZStd::vector", + "ClassData": [ + { + "assetId": { + "guid": "{58E05BD5-3187-58D7-9792-18DA2F6F63A0}" + }, + "platformFlags": 2, + "pathHint": "passes/rospasstemplates.azasset" + } + ] +} \ No newline at end of file diff --git a/Gems/ROS2/Code/CMakeLists.txt b/Gems/ROS2/Code/CMakeLists.txt index 980bfb0cb..7ae2e8f3b 100644 --- a/Gems/ROS2/Code/CMakeLists.txt +++ b/Gems/ROS2/Code/CMakeLists.txt @@ -19,6 +19,29 @@ if (NOT ROS2_FOUND) endif() message(DEBUG "Building ${gem_name} Gem with ros2 $ENV{ROS_DISTRO}") +# Check if ROS 2 distribution is cached +get_property(ROS_DISTRO_TYPE CACHE ROS_DISTRO PROPERTY TYPE) + +# Perform checks with cached ROS 2 distribution and sourced distribution. Save the distribution into cache if it was not cached before or if they do not match end with error. +if (NOT ROS_DISTRO_TYPE) + set(ROS_DISTRO $ENV{ROS_DISTRO} CACHE STRING "ROS 2 distribution of current configuration" FORCE) +elseif(NOT "$ENV{ROS_DISTRO}" STREQUAL "${ROS_DISTRO}") + get_cmake_property(CACHED_VARIABLES CACHE_VARIABLES) + set(ROS_DISTRO_COPY ${ROS_DISTRO}) + # Iterate over cached variables and unset them + foreach(CACHED_VARIABLE ${CACHED_VARIABLES}) + unset(${CACHED_VARIABLE} CACHE) + endforeach() + message(FATAL_ERROR "ROS 2 distribution does not match. Configuration created for: ${ROS_DISTRO_COPY}, sourced distribution: $ENV{ROS_DISTRO}. Removed invalid configuration, please reconfigure project.") +endif() + +# Add a custom target to always check during build if sourced and cached distributions match. +add_custom_target( + 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 +) + # Add the ROS2.Static target # Note: We include the common files and the platform specific files which are set in ros2_common_files.cmake # and in ${pal_dir}/ros2_${PAL_PLATFORM_NAME_LOWERCASE}_files.cmake @@ -43,6 +66,7 @@ ly_add_target( Gem::Atom_Component_DebugCamera.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) @@ -110,11 +134,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 @@ -137,8 +161,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() @@ -190,6 +214,7 @@ if(PAL_TRAIT_BUILD_TESTS_SUPPORTED) PRIVATE AZ::AzTest Gem::${gem_name}.Editor + 3rdParty::sdformat ) # Add ROS2.Editor.Tests to googletest @@ -199,3 +224,5 @@ if(PAL_TRAIT_BUILD_TESTS_SUPPORTED) endif() endif() endif() + +add_subdirectory(PythonTests) diff --git a/Gems/ROS2/Code/FindROS2.cmake b/Gems/ROS2/Code/FindROS2.cmake index 04d969d27..0a6472d60 100644 --- a/Gems/ROS2/Code/FindROS2.cmake +++ b/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) \ No newline at end of file +set(ROS2_FOUND TRUE) diff --git a/Gems/ROS2/Code/Include/ROS2/Camera/CameraCalibrationRequestBus.h b/Gems/ROS2/Code/Include/ROS2/Camera/CameraCalibrationRequestBus.h new file mode 100644 index 000000000..b6343f849 --- /dev/null +++ b/Gems/ROS2/Code/Include/ROS2/Camera/CameraCalibrationRequestBus.h @@ -0,0 +1,44 @@ +/* + * 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 +#include +#include +#include + +namespace ROS2 +{ + //! Interface allows to obtain intrinsic parameters of the camera. To obtain extrinsic parameters use TransformProviderRequestBus. + class CameraCalibrationRequest : public AZ::EBusTraits + { + public: + using BusIdType = AZ::EntityId; + static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById; + + //! Returns the intrinsic calibration matrix of the camera as: + //! [fx 0 cx] + //! [ 0 fy cy] + //! [ 0 0 1] + //! where: + //! - fx, fy : the focal lengths in meters + //! - cx, cy : principal point in pixels. + virtual AZ::Matrix3x3 GetCameraMatrix() const = 0; + + //! Returns the width of the camera sensor in pixels + virtual int GetWidth() const = 0; + + //! Returns the height of the camera sensor in pixels + virtual int GetHeight() const = 0; + + //! Returns the vertical field of view of the camera in degrees + virtual float GetVerticalFOV() const = 0; + }; + + using CameraCalibrationRequestBus = AZ::EBus; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Camera/CameraPostProcessingRequestBus.h b/Gems/ROS2/Code/Include/ROS2/Camera/CameraPostProcessingRequestBus.h index 3e277e9d3..5745a4169 100644 --- a/Gems/ROS2/Code/Include/ROS2/Camera/CameraPostProcessingRequestBus.h +++ b/Gems/ROS2/Code/Include/ROS2/Camera/CameraPostProcessingRequestBus.h @@ -7,28 +7,26 @@ */ #pragma once +#include "AzCore/EBus/EBusSharedDispatchTraits.h" #include -#include #include -#include #include - #include namespace ROS2 { //! Interface class that allows to add post-processing to the pipeline - class CameraPostProcessingRequests : public AZ::EBusTraits + //! + //! Each function call can be processed without blocking Bus for other dispatches. + //! Do not use connects / disconnects to this bus during event dispatch, as they are not allowed for this concurrency model. + //! Those constraints allow for processing multiple camera frames in the same time. + //! Bus implementations should allow for asynchronous execution of provided functions. + class CameraPostProcessingRequests : public AZ::EBusSharedDispatchTraits { public: using BusIdType = AZ::EntityId; static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById; static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Multiple; - using MutexType = AZStd::recursive_mutex; - - //! Set to true to allow multiple threads to call ApplyPostProcessing at the same time. - //! Otherwise, the bus will be locked to a single thread for each entity. - static constexpr bool LocklessDispatch = true; //! Apply post-processing function, if any implementations to the bus are in the entity. //! @param image standard image message passed as a reference. It will be changed through post-processing. diff --git a/Gems/ROS2/Code/Include/ROS2/Communication/PublisherConfiguration.h b/Gems/ROS2/Code/Include/ROS2/Communication/PublisherConfiguration.h new file mode 100644 index 000000000..ff0bdc87e --- /dev/null +++ b/Gems/ROS2/Code/Include/ROS2/Communication/PublisherConfiguration.h @@ -0,0 +1,27 @@ +/* +* 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 +#include +#include +#include + +namespace ROS2 +{ + struct PublisherConfiguration + { + AZ_TYPE_INFO(PublisherConfiguration, "{E50D1926-0322-4832-BD72-C48F854131C8}"); + static void Reflect(AZ::ReflectContext* context); + + bool m_publish = true; //!< A switch controlling whether publishing happens. + TopicConfiguration m_topicConfiguration; //!< Configuration of the published topic. + float m_frequency = 10.0f; //!< Frequency of the published topic (Hz) + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Frame/NamespaceConfiguration.h b/Gems/ROS2/Code/Include/ROS2/Frame/NamespaceConfiguration.h index 5c07070ba..207dc8278 100644 --- a/Gems/ROS2/Code/Include/ROS2/Frame/NamespaceConfiguration.h +++ b/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; diff --git a/Gems/ROS2/Code/Include/ROS2/Frame/ROS2FrameComponent.h b/Gems/ROS2/Code/Include/ROS2/Frame/ROS2FrameComponent.h index b5b7e658e..6aea1dced 100644 --- a/Gems/ROS2/Code/Include/ROS2/Frame/ROS2FrameComponent.h +++ b/Gems/ROS2/Code/Include/ROS2/Frame/ROS2FrameComponent.h @@ -50,6 +50,16 @@ namespace ROS2 //! Set a above-mentioned frame id void SetFrameID(const AZStd::string& frameId); + //! Get the joint name including the namespace + //! @note Supplementary metadata for Joint components, necessary in some cases for joints addressed by name in ROS 2 + //! @return The namespaced joint name, ready to send in a ROS2 message + AZ::Name GetJointName() const; + + //! Set the joint name + //! @note May be populated during URDF import or set by the user in the Editor view + //! @param jointNameString does not include the namespace. The namespace prefix is added automatically. + void SetJointName(const AZStd::string& jointNameString); + //! Get a namespace, which should be used for any publisher or subscriber in the same entity. //! @return A complete namespace (including parent namespaces) AZStd::string GetNamespace() const; @@ -64,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 @@ -85,6 +100,7 @@ namespace ROS2 NamespaceConfiguration m_namespaceConfiguration; AZStd::string m_frameName = "sensor_frame"; + AZStd::string m_jointNameString; bool m_publishTransform = true; bool m_isDynamic = false; diff --git a/Gems/ROS2/Code/Include/ROS2/Gripper/GripperRequestBus.h b/Gems/ROS2/Code/Include/ROS2/Gripper/GripperRequestBus.h new file mode 100644 index 000000000..77ba8ed62 --- /dev/null +++ b/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 +#include +#include + +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 GripperCommand + 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 GripperCommand(float position, float maxEffort) = 0; + + //! Cancel the current command to the gripper. + //! @return Nothing on success, error message if failed. + virtual AZ::Outcome 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; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h b/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h index bc8fb559a..40ef759b7 100644 --- a/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h +++ b/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h @@ -11,6 +11,7 @@ #include #include #include +#include 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& 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; diff --git a/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRegistrarBus.h b/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRegistrarBus.h index 2c11a3b9b..4ad411d69 100644 --- a/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRegistrarBus.h +++ b/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. diff --git a/Gems/ROS2/Code/Include/ROS2/Lidar/LidarSystemBus.h b/Gems/ROS2/Code/Include/ROS2/Lidar/LidarSystemBus.h index 4355cf34b..f8490669c 100644 --- a/Gems/ROS2/Code/Include/ROS2/Lidar/LidarSystemBus.h +++ b/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; }; diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/Controllers/JointsPositionControllerRequests.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/Controllers/JointsPositionControllerRequests.h new file mode 100644 index 000000000..54f6d06fa --- /dev/null +++ b/Gems/ROS2/Code/Include/ROS2/Manipulation/Controllers/JointsPositionControllerRequests.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 +#include +#include +#include + +namespace ROS2 +{ + //! Interface for controllers that execute the simple movement between two positions one step at a time. + class JointsPositionControllerRequests : public AZ::EBusTraits + { + public: + using BusIdType = AZ::EntityId; + static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById; + + //! Whether the implementing controller component is based on articulations. + //! @return true if the controller works with articulations, false otherwise. + virtual bool SupportsArticulation() + { + return false; + }; + + //! Whether the implementing controller component is based on articulations. + //! @return true if the controller works with classic joints, false otherwise. + virtual bool SupportsClassicJoints() + { + return false; + }; + + //! Control a joint through specification of its target position. + //! @param jointName name of the joint to move. + //! @param joint specification of the joint. + //! @param currentPosition current position of the joint. + //! @param targetPosition target position of the joint. + //! @param deltaTime how much time elapsed in simulation the movement should represent. + //! @return nothing on success, error message if the command cannot be realize due to controller or entity configuration. + virtual AZ::Outcome PositionControl( + const AZStd::string& jointName, + JointInfo joint, + JointPosition currentPosition, + JointPosition targetPosition, + float deltaTime) = 0; + }; + using JointsPositionControllerRequestBus = AZ::EBus; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/JointInfo.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/JointInfo.h new file mode 100644 index 000000000..1e495a9e0 --- /dev/null +++ b/Gems/ROS2/Code/Include/ROS2/Manipulation/JointInfo.h @@ -0,0 +1,33 @@ +/* + * 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 +#include +#include +#include +#include +#include + +namespace ROS2 +{ + using JointPosition = float; + using JointVelocity = float; + using JointEffort = float; + struct JointInfo + { + AZ_TYPE_INFO(JointInfo, "{2E33E4D0-78DD-436D-B3AB-F752E744F421}"); + static void Reflect(AZ::ReflectContext* context); + + bool m_isArticulation = false; + PhysX::ArticulationJointAxis m_axis = PhysX::ArticulationJointAxis::Twist; + AZ::EntityComponentIdPair m_entityComponentIdPair; + JointPosition m_restPosition = 0.0f; //!< Keeps this position if no commands are given (for example, opposing gravity). + }; + using ManipulationJoints = AZStd::unordered_map; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/JointsManipulationRequests.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/JointsManipulationRequests.h new file mode 100644 index 000000000..74563a1c4 --- /dev/null +++ b/Gems/ROS2/Code/Include/ROS2/Manipulation/JointsManipulationRequests.h @@ -0,0 +1,92 @@ +/* + * 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 +#include +#include +#include +#include + +namespace ROS2 +{ + //! Interface for general requests for joint systems such as manipulator arms. + //! This interface supports only systems with joints or articulation links with a single degree of freedom (DOF) each. + class JointsManipulationRequests : public AZ::EBusTraits + { + public: + using BusIdType = AZ::EntityId; + static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById; + + using JointsPositionsMap = AZStd::unordered_map; + using JointsVelocitiesMap = AZStd::unordered_map; + using JointsEffortsMap = AZStd::unordered_map; + + //! Get all entity tree joints, including joint or articulation component hierarchy. + //! @return An unordered map of joint names to joint info structure. + //! @note Only free joints are returned (no fixed ones). + virtual ManipulationJoints GetJoints() = 0; + + //! Get position 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 relative position in degree of motion range if joint exists. + //! If it does not exist or some other error happened, error message is returned. + virtual AZ::Outcome 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 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 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. + //! @note the movement is realized by a specific controller and not instant. The joints will then keep these positions. + virtual AZ::Outcome MoveJointsToPositions(const JointsPositionsMap& positions) = 0; + + //! Move a single joint into desired relative position. + //! @param jointName name of the joint. Use names acquired from GetJoints() query. + //! @param position relative position in degree of motion range to achieve. + //! @return nothing on success, error message on failure. + //! @note the movement is realized by a specific controller and not instant. The joints will then keep this position. + virtual AZ::Outcome 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 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; + }; + using JointsManipulationRequestBus = AZ::EBus; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/JointsTrajectoryRequests.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/JointsTrajectoryRequests.h new file mode 100644 index 000000000..d47edd9c4 --- /dev/null +++ b/Gems/ROS2/Code/Include/ROS2/Manipulation/JointsTrajectoryRequests.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 +#include +#include +#include + +namespace ROS2 +{ + //! Interface for commanding a system of joints such as robotic arm (manipulator) through FollowJointTrajectory actions. + //@see FollowJointTrajectory + class JointsTrajectoryRequests : public AZ::EBusTraits + { + public: + using BusIdType = AZ::EntityId; + static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById; + + using TrajectoryGoal = control_msgs::action::FollowJointTrajectory::Goal; + using TrajectoryGoalPtr = std::shared_ptr; + using TrajectoryResult = control_msgs::action::FollowJointTrajectory::Result; + using TrajectoryResultPtr = std::shared_ptr; + + //! Status of trajectory action. + enum class TrajectoryActionStatus + { + Idle, //!< No action has been ordered yet. + Executing, //!< Ongoing trajectory goal. + Cancelled, //!< Cancelled goal, either by the client user or simulation side. + Succeeded //!< Goal reached. + }; + + //! Validate and, if validation is successful, start a trajectory goal. + //! @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 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 CancelTrajectoryGoal() = 0; + + //! Retrieve current trajectory goal status. + //! @return Status of trajectory goal. + virtual TrajectoryActionStatus GetGoalStatus() = 0; + }; + + using JointsTrajectoryRequestBus = AZ::EBus; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/JointMotorControllerComponent.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/JointMotorControllerComponent.h new file mode 100644 index 000000000..3880684d6 --- /dev/null +++ b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/JointMotorControllerComponent.h @@ -0,0 +1,62 @@ +/* + * 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 +#include +#include +#include +#include +#include + +namespace ROS2 +{ + class JointMotorControllerComponent + : public AZ::Component + , public AZ::TickBus::Handler + , public ImGui::ImGuiUpdateListenerBus::Handler + , public AZ::EntityBus::Handler + { + public: + JointMotorControllerComponent() = default; + JointMotorControllerComponent(const JointMotorControllerComponent& other) = default; + JointMotorControllerComponent(JointMotorControllerComponent&& other) = default; + ~JointMotorControllerComponent() = default; + AZ_COMPONENT(JointMotorControllerComponent, "{88e725fc-29d8-45b9-b3e8-bd268ad9f413}"); + + // Component overrides + void Activate() override; + void Deactivate() override; + + static void Reflect(AZ::ReflectContext* context); + + // ImGui::ImGuiUpdateListenerBus overrides + void OnImGuiUpdate() override; + + // EntityBus overrides + void OnEntityActivated(const AZ::EntityId& entityId) override; + + protected: + AZ::EntityComponentIdPair m_jointComponentIdPair; //!< Joint component managed by the motorized joint. + float m_currentPosition{ 0.0f }; //!< Last measured position. + float m_currentSpeed{ 0.0f }; //!< Last measured speed. + + JointMotorControllerConfiguration m_jointMotorControllerConfiguration; + + private: + virtual float CalculateMotorSpeed([[maybe_unused]] float deltaTime) + { + return 0.0f; + }; + + virtual void DisplayControllerParameters(){}; + + // AZ::TickBus overrides + void OnTick(float deltaTime, AZ::ScriptTimePoint time) override; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/JointMotorControllerConfiguration.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/JointMotorControllerConfiguration.h new file mode 100644 index 000000000..60cdd3ac5 --- /dev/null +++ b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/JointMotorControllerConfiguration.h @@ -0,0 +1,27 @@ +/* + * 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 +#include + +namespace ROS2 +{ + struct JointMotorControllerConfiguration + { + AZ_TYPE_INFO(JointMotorControllerConfiguration, "{4358971c-36bd-4e13-8427-74ebba6c6760}"); + static void Reflect(AZ::ReflectContext* context); + + bool IsDebugModeVisible() const; + + bool m_isDebugController{ false }; //!< Is it a debug controller. + + bool m_debugMode{ false }; //!< Is debug mode activated. + }; + +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/ManualMotorControllerComponent.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/ManualMotorControllerComponent.h new file mode 100644 index 000000000..7c8808f09 --- /dev/null +++ b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/ManualMotorControllerComponent.h @@ -0,0 +1,30 @@ +/* + * 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 +#include + +namespace ROS2 +{ + class ManualMotorControllerComponent : public JointMotorControllerComponent + { + public: + AZ_COMPONENT(ManualMotorControllerComponent, "{0817634e-4862-4245-a66e-72d1a6939705}", JointMotorControllerComponent); + + ManualMotorControllerComponent(); + ~ManualMotorControllerComponent() = default; + static void Reflect(AZ::ReflectContext* context); + + private: + float m_setSpeed{ 0.0f }; + + float CalculateMotorSpeed([[maybe_unused]] float deltaTime) override; + void DisplayControllerParameters() override; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulator/MotorizedJointBus.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/PidMotorControllerBus.h similarity index 85% rename from Gems/ROS2/Code/Include/ROS2/Manipulator/MotorizedJointBus.h rename to Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/PidMotorControllerBus.h index 385dceaad..dae109b5f 100644 --- a/Gems/ROS2/Code/Include/ROS2/Manipulator/MotorizedJointBus.h +++ b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/PidMotorControllerBus.h @@ -13,15 +13,15 @@ namespace ROS2 { - //! Interface to communicate with motorized joints. - //! It allows to apply setpoint and tracking performance of the PID controller. - class MotorizedJointRequest : public AZ::EBusTraits + //! Interface to communicate with a PID motor controller. + //! It allows to apply a setpoint and track performance of the controller. + class PidMotorControllerRequests : public AZ::EBusTraits { public: static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById; using BusIdType = AZ::EntityId; - virtual ~MotorizedJointRequest() = default; + virtual ~PidMotorControllerRequests() = default; //! Set current setpoint value for position controller. //! The setpoint is the desired position for a simulated actuator. @@ -53,5 +53,5 @@ namespace ROS2 virtual float GetError() = 0; }; - using MotorizedJointRequestBus = AZ::EBus; -} // namespace ROS2 \ No newline at end of file + using PidMotorControllerRequestBus = AZ::EBus; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/PidMotorControllerComponent.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/PidMotorControllerComponent.h new file mode 100644 index 000000000..018d2781a --- /dev/null +++ b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/PidMotorControllerComponent.h @@ -0,0 +1,49 @@ +/* + * 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 +#include +#include +#include +#include + +namespace ROS2 +{ + class PidMotorControllerComponent + : public JointMotorControllerComponent + , public PidMotorControllerRequestBus::Handler + { + public: + AZ_COMPONENT(PidMotorControllerComponent, "{ac1d057f-a6ad-4a26-b44f-0ebda2f5f526}", JointMotorControllerComponent); + + PidMotorControllerComponent() = default; + ~PidMotorControllerComponent() = default; + static void Reflect(AZ::ReflectContext* context); + + // Component overrides + void Activate() override; + void Deactivate() override; + + // PidMotorControllerBus overrides + void SetSetpoint(float setpoint) override; + float GetSetpoint() override; + float GetCurrentMeasurement() override; + float GetError() override; + + private: + Controllers::PidConfiguration m_pidPos; //!< PID controller for position. + float m_zeroOffset{ 0.0f }; //!< Offset added to setpoint. + float m_setPoint{ 0.0f }; //!< Desired local position. + float m_error{ 0.0f }; //!< Current error (difference between control value and measurement). + + // JointMotorControllerComponent overrides + float CalculateMotorSpeed([[maybe_unused]] float deltaTime) override; + void DisplayControllerParameters() override; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulator/MotorizedJointComponent.h b/Gems/ROS2/Code/Include/ROS2/Manipulator/MotorizedJointComponent.h deleted file mode 100644 index 9eab938e0..000000000 --- a/Gems/ROS2/Code/Include/ROS2/Manipulator/MotorizedJointComponent.h +++ /dev/null @@ -1,93 +0,0 @@ -/* - * 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 "MotorizedJointBus.h" -#include -#include -#include -#include -#include - -namespace ROS2 -{ - //! A prototype component for simulated joint with a motor. - //! It works with either TransformBus or RigidBodyBus. - //! TransformBus mode, called `AnimationMode` changes local transform. In this mode, you cannot have a rigid body - //! controller enabled. With RigidBodyBus it applies forces and torque according to PID control. - //! @note This class is already used through ROS2FrameComponent. - class MotorizedJointComponent - : public AZ::Component - , public AZ::TickBus::Handler - , public MotorizedJointRequestBus::Handler - { - public: - AZ_COMPONENT(MotorizedJointComponent, "{AE9207DB-5B7E-4F70-A7DD-C4EAD8DD9403}", AZ::Component); - - MotorizedJointComponent() = default; - ~MotorizedJointComponent() = default; - ////////////////////////////////////////////////////////////////////////// - // Component overrides - void Activate() override; - void Deactivate() override; - static void Reflect(AZ::ReflectContext* context); - ////////////////////////////////////////////////////////////////////////// - - //////////////////////////////////////////////////////////////////////// - // MotorizedJointRequestBus::Handler overrides - void SetSetpoint(float setpoint) override; - float GetSetpoint() override; - float GetError() override; - float GetCurrentMeasurement() override; - //////////////////////////////////////////////////////////////////////// - - //! Get a degree of freedom direction. - //! @returns direction of joint movement in global coordinates. - AZ::Vector3 GetDir() const - { - return m_jointDir; - }; - - private: - float ComputeMeasurement(AZ::ScriptTimePoint time); - void SetVelocity(float velocity /* m/s */, float deltaTime /* seconds */); - void ApplyLinVelAnimation(float velocity /* m/s */, float deltaTime /* seconds */); - void ApplyLinVelRigidBodyImpulse(float velocity /* m/s */, float deltaTime /* seconds */); - void ApplyLinVelRigidBody(float velocity /* m/s */, float deltaTime /* seconds */); - ////////////////////////////////////////////////////////////////////////// - // AZ::TickBus::Handler overrides - void OnTick(float deltaTime, AZ::ScriptTimePoint time) override; - ////////////////////////////////////////////////////////////////////////// - - AZ::Vector3 m_jointDir{ 0.f, 0.f, 1.f }; //!< Direction of joint movement in parent frame of reference, used to compute measurement. - AZ::Vector3 m_effortAxis{ 0.f, 0.f, 1.f }; //!< Direction of force or torque application in owning entity frame of reference. - AZStd::pair m_limits{ -0.5f, 0.5f }; //!< limits of joint, the force is applied only when joint is within limits. - Controllers::PidConfiguration m_pidPos; //!< PID controller for position. - - bool m_linear{ true }; //!< Linear mode. The force is applied through RigidBodyBus. - bool m_animationMode{ true }; //!< Use TransformBus (animation mode, no physics) instead of RigidBodyBus. - - bool m_testSinusoidal{ true }; //!< Enable sinusoidal signal generator to setpoint (for tuning). - float m_sinAmplitude{ 0.5 }; //!< Amplitude of test signal generator. - float m_sinDC{ 0.25 }; //!< DC of test signal generator. - float m_sinFreq{ 0.1 }; //!< Frequency of test signal generator. - - float m_zeroOffset{ 0.f }; //!< offset added to setpoint. - float m_setpoint{ 0 }; //!< Desired local position. - float m_error{ 0 }; //!< Current error (difference between control value and measurement). - float m_currentPosition{ 0 }; //!< Last measured position. - float m_currentVelocity{ 0 }; //!< Last measured velocity. - double m_lastMeasurementTime{ 0 }; //!< Last measurement time in seconds. - - AZ::EntityId m_debugDrawEntity; //!< Optional Entity that allows to visualize desired setpoint value. - AZ::Transform m_debugDrawEntityInitialTransform; //!< Initial transform of m_debugDrawEntity. - bool m_debugPrint{ false }; //!< Print debug info to the console. - - AZ::EntityId m_measurementReferenceEntity; //!< Entity used for reference for measurements. Defaults to parent entity. - }; -} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/RobotImporter/RobotImporterBus.h b/Gems/ROS2/Code/Include/ROS2/RobotImporter/RobotImporterBus.h new file mode 100644 index 000000000..66bffb989 --- /dev/null +++ b/Gems/ROS2/Code/Include/ROS2/RobotImporter/RobotImporterBus.h @@ -0,0 +1,35 @@ +/* + * 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 +#include +#include +#include + +namespace ROS2 +{ + class RobotImporterRequest : public AZ::EBusTraits + { + public: + static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Single; + static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::Single; + + //! Generate prefab from the urdf file. + //! @param filePath The path of the urdf file + //! @param importAssetWithUrdf If true, the assets referenced in the urdf file will be imported + //! @param useArticulation If true, the prefab will be generated with articulation + virtual bool GeneratePrefabFromFile(const AZStd::string_view filePath, bool importAssetWithUrdf, bool useArticulation) = 0; + + //! Return the reference to the list of sensor importer hooks + virtual const SDFormat::SensorImporterHooksStorage& GetSensorHooks() const = 0; + }; + + using RobotImporterRequestBus = AZ::EBus; + +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/RobotImporter/SDFormatSensorImporterHook.h b/Gems/ROS2/Code/Include/ROS2/RobotImporter/SDFormatSensorImporterHook.h new file mode 100644 index 000000000..26ccdbc84 --- /dev/null +++ b/Gems/ROS2/Code/Include/ROS2/RobotImporter/SDFormatSensorImporterHook.h @@ -0,0 +1,62 @@ +/* + * 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 +#include +#include +#include +#include + +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 m_sensorTypes; + + //! Names of the sensor's parameters in SDFormat description that are supported by the particular O3DE component. + AZStd::unordered_set 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 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 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; + using ConvertSDFSensorCallback = AZStd::function; + ConvertSDFSensorCallback m_sdfSensorToComponentCallback; + }; + + //! Buffer for SensorImporterHook data + using SensorImporterHooksStorage = AZStd::vector; +} // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Include/ROS2/Sensor/Events/EventSourceAdapter.h b/Gems/ROS2/Code/Include/ROS2/Sensor/Events/EventSourceAdapter.h new file mode 100644 index 000000000..f45118393 --- /dev/null +++ b/Gems/ROS2/Code/Include/ROS2/Sensor/Events/EventSourceAdapter.h @@ -0,0 +1,193 @@ +/* + * 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 +#include +#include +#include + +namespace ROS2 +{ + namespace Internal + { + template< + template< + template + class, // EventType + template + class, // EventHandlerType + typename...> // Event parameters + class C, + class T> + struct is_specialization_of : AZStd::false_type + { + }; + + template< + template< + template + class, // EventType + template + class, // EventHandlerType + typename...> // Event parameters + class Base, + template + class EventType, + template + class EventHandlerType, + typename... Args> + struct is_specialization_of> : AZStd::true_type + { + }; + } // namespace Internal + + //! Class adapting event source (ROS2::SensorEventSource) to configurable working frequency. This is handled via adapted event, in + //! a similar manner like it is done in SensorEventSource. EventSourceAdapter has its internal handler that connects to + //! SensorEventSource source event, and signals adapted event according to frequency set (ROS2::EventSourceAdapter::SetFrequency). + //! User can connect to this event using ROS2::EventSourceAdapter::ConnectToAdaptedEvent method. This class should be used, instead + //! of using directly a class derived from SensorEventSource, when specific working frequency is required. Following this path, user can + //! still use source event - ROS2::EventSourceAdapter::ConnectToSourceEvent. This template has to be resolved using a class derived from + //! SensorEventSource specialization. + //! @see ROS2::SensorEventSource + template + class EventSourceAdapter + { + public: + // Require non-abstract type derived from SensorEventSource specialization. + static_assert(Internal::is_specialization_of::value); + static_assert(AZStd::is_base_of::value); + static_assert(AZStd::is_abstract::value == false); + + static void Reflect(AZ::ReflectContext* context) + { + EventSourceT::Reflect(context); + + if (auto* serializeContext = azrtti_cast(context)) + { + serializeContext->Class>() + ->Version(1) + ->Field("Adapted frequency", &EventSourceAdapter::m_adaptedFrequency) + ->Field("Event source configuration", &EventSourceAdapter::m_eventSource); + + if (auto* editContext = serializeContext->GetEditContext()) + { + editContext + ->Class>( + "Event Source Adapter", "Adapts sensor event source to specified working frequency") + ->ClassElement(AZ::Edit::ClassElements::EditorData, "") + ->ElementAttribute(AZ::Edit::Attributes::AutoExpand, true) + ->DataElement( + AZ::Edit::UIHandlers::Default, + &EventSourceAdapter::m_adaptedFrequency, + "Adapted frequency", + "Adapter event signalling frequency"); + } + } + } + + //! Starts event source adapter - assigns internal adapted event handler and starts managed event source. Adapted frequency can be + //! set using ROS2::EventSourceAdapter::SetFrequency method. + void Start() + { + m_sourceAdaptingEventHandler = typename EventSourceT::SourceEventHandlerType( + [this](auto&&... args) + { + const float sourceDeltaTime = m_eventSource.GetDeltaTime(AZStd::forward(args)...); + m_adaptedDeltaTime += sourceDeltaTime; + + if (!IsPublicationDeadline(sourceDeltaTime)) + { + return; + } + + m_sensorAdaptedEvent.Signal(m_adaptedDeltaTime, AZStd::forward(args)...); + m_adaptedDeltaTime = 0.0f; + }); + m_eventSource.ConnectToSourceEvent(m_sourceAdaptingEventHandler); + m_eventSource.Start(); + } + + //! Stops event source adapter - stops event source and disconnects internal adapted event handler from source event. If it will be + //! necessary, this implementation allows multiple consecutive calls of Start / Stop, however user must also investigate specific + //! event source implementation with such case in mind. + void Stop() + { + m_eventSource.Stop(); + m_sourceAdaptingEventHandler.Disconnect(); + } + + //! Sets adapter working frequency. By design, adapter will not work correctly, if this frequency will be greater than used event + //! source frequency - e.g. adapter will be requested to work in 60Hz, when using event source working in 30Hz. In general, adapted + //! frequency should be equal or lower than event source frequency - this is forced internally + //! (ROS2::EventSourceAdapter::IsPublicationDeadline). Optimal (highest precision in timing events) working conditions take place + //! when event source frequency is an integer multiple of adapted frequency. + //! @param adaptedFrequency Adapter working frequency. When set to zero or less adapter will be assumed to work in 1Hz. + void SetFrequency(float adaptedFrequency) + { + m_adaptedFrequency = adaptedFrequency; + } + + //! Connects given event handler to source event (ROS2::SensorEventSource). That event is signalled regardless of adapted frequency + //! set for event source adapter (ROS2::EventSourceAdapter::SetFrequency). Its frequency depends only on specific event source + //! implementation. If different working frequency is required (main purpose of ROS2::EventSourceAdapter), user should see + //! ROS2::EventSourceAdapter::ConnectToAdaptedEvent method. + //! @param sourceEventHandler Event handler for source event (frequency not managed by event source adapter). + //! @see ROS2::SensorEventSource + void ConnectToSourceEvent(typename EventSourceT::SourceEventHandlerType& sourceEventHandler) + { + m_eventSource.ConnectToSourceEvent(sourceEventHandler); + } + + //! Connects given event handler to adapted event (ROS2::EventSourceAdapter). This event is signalled with a frequency set with + //! ROS2::EventSourceAdapter::SetFrequency method. + //! @param adaptedEventHandler Event handler for adapted event. + void ConnectToAdaptedEvent(typename EventSourceT::AdaptedEventHandlerType& adaptedEventHandler) + { + adaptedEventHandler.Connect(m_sensorAdaptedEvent); + } + + private: + //! Uses: + //! - internal tick counter, + //! - last delta time of event source and + //! - frequency set for adapter + //! to support managing calls from event source. In other words, uses delta time of event source to calculate average number of + //! source event calls per adapted event call. + //! @param sourceDeltaTime Delta time of event source. + //! @return Whether it is time to signal adapted event. + [[nodiscard]] bool IsPublicationDeadline(float sourceDeltaTime) + { + if (--m_tickCounter > 0) + { + return false; + } + + const float sourceFrequencyEstimation = 1.0f / sourceDeltaTime; + const float numberOfFrames = + m_adaptedFrequency <= sourceFrequencyEstimation ? (sourceFrequencyEstimation / m_adaptedFrequency) : 1.0f; + m_tickCounter = aznumeric_cast(AZStd::round(numberOfFrames)); + return true; + } + + EventSourceT m_eventSource{}; ///< Event source managed by this adapter. + + //! Event handler for adapting event source to specific frequency. + typename EventSourceT::SourceEventHandlerType m_sourceAdaptingEventHandler{}; + + //! Adapted event that is called with specific frequency. + typename EventSourceT::AdaptedEventType m_sensorAdaptedEvent{}; + + float m_adaptedFrequency{ 30.0f }; ///< Adapted frequency value. + float m_adaptedDeltaTime{ 0.0f }; ///< Accumulator for calculating adapted delta time. + int m_tickCounter{ 0 }; ///< Internal counter for controlling adapter frequency. + }; + + AZ_TYPE_INFO_TEMPLATE(EventSourceAdapter, "{DC8BB5F7-8E0E-42A1-BD82-5FCD9D31B9DD}", AZ_TYPE_INFO_CLASS) +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Sensor/Events/PhysicsBasedSource.h b/Gems/ROS2/Code/Include/ROS2/Sensor/Events/PhysicsBasedSource.h new file mode 100644 index 000000000..2e7d79b69 --- /dev/null +++ b/Gems/ROS2/Code/Include/ROS2/Sensor/Events/PhysicsBasedSource.h @@ -0,0 +1,36 @@ +/* + * 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 +#include +#include +#include + +namespace ROS2 +{ + //! Class implementing physics callbacks as sensor event source. Source event (ROS2::SensorEventSource) is signalled based on scene + //! simulation finish event. + //! @note The working frequency of this event source can be changed through engine settings (physics simulation delta). + //! @see ROS2::SensorEventSource + class PhysicsBasedSource final : public SensorEventSource + { + public: + AZ_TYPE_INFO(PhysicsBasedSource, "{48BB21A8-F14E-4869-95DC-28EEA279Cf53}"); + static void Reflect(AZ::ReflectContext* context); + + // Overrides of ROS2::SensorEventSource. + void Start() override; + void Stop() override; + [[nodiscard]] float GetDeltaTime(AzPhysics::SceneHandle sceneHandle, float deltaTime) const override; + + private: + AzPhysics::SceneEvents::OnSceneSimulationFinishHandler m_onSceneSimulationEventHandler; ///< Handler for physics callback. + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Sensor/Events/SensorEventSource.h b/Gems/ROS2/Code/Include/ROS2/Sensor/Events/SensorEventSource.h new file mode 100644 index 000000000..b23a1da6e --- /dev/null +++ b/Gems/ROS2/Code/Include/ROS2/Sensor/Events/SensorEventSource.h @@ -0,0 +1,72 @@ +/* + * 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 + +namespace ROS2 +{ + struct SensorConfiguration; + + //! Base class template for sensor event sources (based e.g. on TickBus or engine physics callback). Developer that wants to implement + //! new event source should derive from this class and signal m_sensorEvent based on chosen approach. Derived classes can be used + //! directly as unconstrained source of frequency - e.g. TickBasedSource, when synchronization with draw calls is necessary. However, in + //! most common case they will be used as source of events for sensors, through EventSourceAdapter object, where their m_sensorEvent + //! will be additionally adapted to some sensor-specific frequency. + //! @see ROS2::EventSourceAdapter + //! @see ROS2::TickBasedSource + //! @see ROS2::PhysicsBasedSource + template class EventT, template class EventHandlerT, typename... EventArgs> + class SensorEventSource + { + public: + using SourceBaseType = SensorEventSource; + + using SourceCallbackType = AZStd::function; + using AdaptedCallbackType = AZStd::function; + + using SourceEventType = EventT; + using SourceEventHandlerType = EventHandlerT; + + using AdaptedEventType = EventT; + using AdaptedEventHandlerType = EventHandlerT; + + virtual ~SensorEventSource() = default; + + //! Starts event source - see specific event source description for more details. After call to this method event source is supposed + //! to start signalling source event. + virtual void Start() + { + } + + //! Stops event source - see specific event source description for more details. After call to this method event source is supposed + //! to stop signalling source event. + virtual void Stop() + { + } + + //! Returns delta time from set of source event parameters (passed as variadic up to some point). The purpose of this method is to + //! give developers access to event source delta time, without additional variables or helpers. + //! @param args Set of source event parameters. + //! @return Delta time in seconds. + [[nodiscard]] virtual float GetDeltaTime(EventArgs... args) const = 0; + + //! Connects given event handler to sensor source event. By design, source event is not restricted by frequency settings. This event + //! should be signalled based on event source internal logic (e.g. on draw call). For more details check event source + //! specializations. + //! @param sourceEventHandler Event handler that will be connected to source event. + void ConnectToSourceEvent(SourceEventHandlerType& sourceEventHandler) + { + sourceEventHandler.Connect(m_sourceEvent); + } + + protected: + SensorEventSource() = default; + + SourceEventType m_sourceEvent{}; ///< This event should be signalled based on specific event source specialization. + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Sensor/Events/TickBasedSource.h b/Gems/ROS2/Code/Include/ROS2/Sensor/Events/TickBasedSource.h new file mode 100644 index 000000000..93a09b44a --- /dev/null +++ b/Gems/ROS2/Code/Include/ROS2/Sensor/Events/TickBasedSource.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 +#include +#include + +namespace ROS2 +{ + //! Class implementing system TickBus (draw calls) as sensor event source. Source event (ROS2::SensorEventSource) is signalled based on + //! system ticks. + //! @see ROS2::SensorEventSource + class TickBasedSource final + : public SensorEventSource + , protected AZ::TickBus::Handler + { + public: + AZ_TYPE_INFO(TickBasedSource, "{AD3CC041-5F7C-45E8-AA2D-5D8A1D4CC466}"); + static void Reflect(AZ::ReflectContext* context); + + // Overrides of ROS2::SensorEventSource. + void Start() override; + void Stop() override; + [[nodiscard]] float GetDeltaTime(float deltaTime, AZ::ScriptTimePoint time) const override; + + private: + // Override of AZ::TickBus::Handler. + void OnTick(float deltaTime, AZ::ScriptTimePoint time) override; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Sensor/ROS2SensorComponent.h b/Gems/ROS2/Code/Include/ROS2/Sensor/ROS2SensorComponent.h deleted file mode 100644 index 28afa20f4..000000000 --- a/Gems/ROS2/Code/Include/ROS2/Sensor/ROS2SensorComponent.h +++ /dev/null @@ -1,74 +0,0 @@ -/* - * 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 "SensorConfiguration.h" -#include -#include -#include -#include - -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. - class ROS2SensorComponent - : public AZ::Component - , public AZ::TickBus::Handler - { - public: - ROS2SensorComponent() = default; - virtual ~ROS2SensorComponent() = default; - AZ_COMPONENT(ROS2SensorComponent, "{91BCC1E9-6D93-4466-9CDB-E73D497C6B5E}"); - - ////////////////////////////////////////////////////////////////////////// - // Component overrides - void Activate() override; - void Deactivate() override; - ////////////////////////////////////////////////////////////////////////// - - static void Reflect(AZ::ReflectContext* context); - void OnTick(float deltaTime, AZ::ScriptTimePoint time) override; - static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); - - protected: - AZStd::string GetNamespace() const; //!< Get a complete namespace for this sensor topics and frame ids. - AZStd::string GetFrameID() const; //!< Returns this sensor frame ID. The ID contains namespace. - - SensorConfiguration m_sensorConfiguration; - - //! Check if execution deadline has arrived. - //! This function needs to be called every loop's iteration (eg TickBus::Handler::OnTick). - //! @param expectedLoopTime the expected time to loop call in seconds. - //! @returns if measurement should be done/published. - bool IsPublicationDeadline(float expectedLoopTime); - - //! Virtual function that setup refresh loop for the sensor. - //! Default implementation is calling \ref FrequencyTick periodically in AZ::TickBus::Handler::OnTick. - //! This function can be overridden to subscribe to higher frequency loops or to spawn sensor threads. - virtual void SetupRefreshLoop(); - - //! Executes the sensor action (acquire data -> publish) according to frequency. - //! Override to implement a specific sensor behavior. - virtual void FrequencyTick(){}; - - private: - //! Visualise 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(){}; - - //! The number of ticks that are expected to pass to trigger next measurement. - AZ::s32 m_tickCountDown{ 0 }; - - //! Optional callback that will be called in overridden onTick method. - //! Used in default implementation of \ref SetupRefreshLoop - AZStd::function m_onTickCall; - }; -} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Sensor/ROS2SensorComponentBase.h b/Gems/ROS2/Code/Include/ROS2/Sensor/ROS2SensorComponentBase.h new file mode 100644 index 000000000..c9a471c17 --- /dev/null +++ b/Gems/ROS2/Code/Include/ROS2/Sensor/ROS2SensorComponentBase.h @@ -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 + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace ROS2 +{ + //! Base sensor component class for all specific sensor implementations. Developer working on the new sensor should derive from this + //! class, defining necessary event source type (EventSourceT template parameter). Available sources are e.g. TickBasedSource or + //! PhysicsBasedSource. Chosen event source is wrapped into EventSourceAdapter, making it possible to work with specific frequency. + //! Derived implementation should call ROS2::ROS2SensorComponentBase::StartSensor at the end of Activate (or whenever sensor + //! configuration is already set up) and StopSensor in Deactivate. Starting sensor base requires passing two parameters: + //! - sensor working frequency - how often sensor logic should be processed, + //! - adapted event callback - what should be done in sensor logic processing. + //! Optionally, user can pass third parameter, which is source event callback - this will be called with source event frequency (check + //! chosen event source implementation). + //! @see ROS2::TickBasedSource + //! @see ROS2::PhysicsBasedSource + template + class ROS2SensorComponentBase : public AZ::Component + { + public: + using SensorBaseType = ROS2SensorComponentBase; + + AZ_COMPONENT_DECL((ROS2SensorComponentBase, AZ_CLASS)); + + static void Reflect(AZ::ReflectContext* context) + { + if (auto* serializeContext = azrtti_cast(context)) + { + serializeContext->Class, AZ::Component>() + ->Version(1) + ->Field("SensorConfiguration", &ROS2SensorComponentBase::m_sensorConfiguration); + + if (auto* editContext = serializeContext->GetEditContext()) + { + editContext->Class>("ROS2 Sensor Component Base", "Base component for sensors") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &ROS2SensorComponentBase::m_sensorConfiguration, + "Sensor configuration", + "Sensor configuration"); + } + } + } + + static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required) + { + required.push_back(AZ_CRC_CE("ROS2Frame")); + } + + virtual ~ROS2SensorComponentBase() = default; + + void Activate() override + { + } + + void Deactivate() override + { + } + + protected: + //! Starts sensor with passed frequency and adapted event callback. Optionally, user can pass source event callback, that will be + //! called with event source frequency. + //! @param sensorFrequency Sensor working frequency. + //! @param adaptedCallback Adapted event callback - called with sensor working frequency. + //! @param sourceCallback Source event callback - called with event source frequency. + void StartSensor( + float sensorFrequency, + typename EventSourceT::AdaptedCallbackType adaptedCallback, + typename EventSourceT::SourceCallbackType sourceCallback = nullptr) + { + m_eventSourceAdapter.SetFrequency(sensorFrequency); + + m_adaptedEventHandler.Disconnect(); + m_adaptedEventHandler = decltype(m_adaptedEventHandler)(adaptedCallback); + m_eventSourceAdapter.ConnectToAdaptedEvent(m_adaptedEventHandler); + + m_sourceEventHandler.Disconnect(); + if (sourceCallback) + { + m_sourceEventHandler = decltype(m_sourceEventHandler)(sourceCallback); + m_eventSourceAdapter.ConnectToSourceEvent(m_sourceEventHandler); + } + + m_eventSourceAdapter.Start(); + } + + //! Stops sensor and disconnects event callbacks passed through RSO2::ROS2SensorComponentBase::StartSensor. + void StopSensor() + { + m_eventSourceAdapter.Stop(); + m_sourceEventHandler.Disconnect(); + m_adaptedEventHandler.Disconnect(); + } + + //! Returns a complete namespace for this sensor topics and frame ids. + [[nodiscard]] AZStd::string GetNamespace() const + { + auto* ros2Frame = Utils::GetGameOrEditorComponent(GetEntity()); + return ros2Frame->GetNamespace(); + } + + //! Returns this sensor frame ID. The ID contains namespace. + [[nodiscard]] AZStd::string GetFrameID() const + { + auto* ros2Frame = Utils::GetGameOrEditorComponent(GetEntity()); + return ros2Frame->GetFrameID(); + } + + SensorConfiguration m_sensorConfiguration; ///< Basic sensor configuration. + EventSourceAdapter m_eventSourceAdapter; ///< Adapter for selected event source (see this class documentation). + + //! Handler for source event. Requires manual assignment and connecting to source event in derived class. + typename EventSourceT::SourceEventHandlerType m_sourceEventHandler; + + //! Handler for adapted event. Requires manual assignment and connecting to adapted event in derived class. + typename EventSourceT::AdaptedEventHandlerType m_adaptedEventHandler; + }; + + AZ_COMPONENT_IMPL_INLINE( + (ROS2SensorComponentBase, AZ_CLASS), "ROS2SensorComponentBase", "{2DF9A652-DF5D-43B1-932F-B6A838E36E97}", AZ::Component) +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Sensor/SensorConfiguration.h b/Gems/ROS2/Code/Include/ROS2/Sensor/SensorConfiguration.h index 074195ab0..d807c47ef 100644 --- a/Gems/ROS2/Code/Include/ROS2/Sensor/SensorConfiguration.h +++ b/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: @@ -31,9 +31,12 @@ namespace ROS2 //! Frequency in Hz (1/s). //! Applies both to data acquisition and publishing. - float m_frequency = 10; + float m_frequency = 10.f; 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::epsilon(); }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Spawner/SpawnerBus.h b/Gems/ROS2/Code/Include/ROS2/Spawner/SpawnerBus.h index f40aac22e..24dde1575 100644 --- a/Gems/ROS2/Code/Include/ROS2/Spawner/SpawnerBus.h +++ b/Gems/ROS2/Code/Include/ROS2/Spawner/SpawnerBus.h @@ -11,6 +11,7 @@ #include #include #include +#include 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 GetAllSpawnPointInfos() const = 0; }; using SpawnerRequestsBus = AZ::EBus; - using SpawnerInterface = AZ::Interface; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Spawner/SpawnerInfo.h b/Gems/ROS2/Code/Include/ROS2/Spawner/SpawnerInfo.h new file mode 100644 index 000000000..6276cd9d2 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +namespace ROS2 +{ + struct SpawnPointInfo + { + AZStd::string info; + AZ::Transform pose; + }; + + using SpawnPointInfoMap = AZStd::unordered_map; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Utilities/ROS2Conversions.h b/Gems/ROS2/Code/Include/ROS2/Utilities/ROS2Conversions.h index c8063db53..216dabbd0 100644 --- a/Gems/ROS2/Code/Include/ROS2/Utilities/ROS2Conversions.h +++ b/Gems/ROS2/Code/Include/ROS2/Utilities/ROS2Conversions.h @@ -27,5 +27,6 @@ namespace ROS2 AZ::Transform FromROS2Pose(const geometry_msgs::msg::Pose& ros2pose); AZ::Vector3 FromROS2Point(const geometry_msgs::msg::Point& ros2point); AZ::Quaternion FromROS2Quaternion(const geometry_msgs::msg::Quaternion& ros2quaternion); + std::array ToROS2Covariance(const AZ::Matrix3x3& covariance); }; // namespace ROS2Conversions } // namespace ROS2 diff --git a/Gems/ROS2/Code/Platform/Linux/PAL_linux.cmake b/Gems/ROS2/Code/Platform/Linux/PAL_linux.cmake index b278d0179..1bf09bc01 100644 --- a/Gems/ROS2/Code/Platform/Linux/PAL_linux.cmake +++ b/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-rev2-linux + TARGETS sdformat + PACKAGE_HASH b8e988954c07f41b99ba0950da3f4d3e8489ffabaaff157f79ab0c716e2142e0) +elseif(${CMAKE_SYSTEM_PROCESSOR} STREQUAL "aarch64") + ly_associate_package(PACKAGE_NAME sdformat-13.5.0-rev2-linux-aarch64 + TARGETS sdformat + PACKAGE_HASH 7e51cc60c61a058c1f8aba7277574946ab974af3ff4601884e72380e8585c0ea) +endif() diff --git a/Gems/ROS2/Code/PythonTests/CMakeLists.txt b/Gems/ROS2/Code/PythonTests/CMakeLists.txt new file mode 100644 index 000000000..851171d69 --- /dev/null +++ b/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 () diff --git a/Gems/ROS2/Code/PythonTests/SmokeTests_Periodic.py b/Gems/ROS2/Code/PythonTests/SmokeTests_Periodic.py new file mode 100644 index 000000000..e29628128 --- /dev/null +++ b/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 + + +@pytest.mark.SUITE_periodic # Marks the test suite as being part of a Periodic test suite +@pytest.mark.parametrize("launcher_platform", ['linux_editor']) # This test works on Linux editor +@pytest.mark.parametrize("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 diff --git a/Gems/ROS2/Code/PythonTests/__init__.py b/Gems/ROS2/Code/PythonTests/__init__.py new file mode 100755 index 000000000..f5193b300 --- /dev/null +++ b/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 +""" diff --git a/Gems/ROS2/Code/PythonTests/tests/SmokeTests_EnterGameModeWorks.py b/Gems/ROS2/Code/PythonTests/tests/SmokeTests_EnterGameModeWorks.py new file mode 100644 index 000000000..e5e7ca694 --- /dev/null +++ b/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) diff --git a/Gems/ROS2/Code/Source/Camera/CameraConstants.h b/Gems/ROS2/Code/Source/Camera/CameraConstants.h new file mode 100644 index 000000000..24facbe0c --- /dev/null +++ b/Gems/ROS2/Code/Source/Camera/CameraConstants.h @@ -0,0 +1,21 @@ +/* + * 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 + +namespace ROS2 +{ + namespace CameraConstants + { + inline constexpr char ImageMessageType[] = "sensor_msgs::msg::Image"; + inline constexpr char DepthImageConfig[] = "Depth Image"; + inline constexpr char ColorImageConfig[] = "Color Image"; + inline constexpr char DepthInfoConfig[] = "Depth Camera Info"; + inline constexpr char ColorInfoConfig[] = "Color Camera Info"; + inline constexpr char CameraInfoMessageType[] = "sensor_msgs::msg::CameraInfo"; + } // namespace CameraConstants +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Camera/CameraPublishers.cpp b/Gems/ROS2/Code/Source/Camera/CameraPublishers.cpp new file mode 100644 index 000000000..3a6fa68c7 --- /dev/null +++ b/Gems/ROS2/Code/Source/Camera/CameraPublishers.cpp @@ -0,0 +1,130 @@ +/* + * 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 "CameraPublishers.h" +#include "CameraConstants.h" +#include "CameraSensor.h" +#include +#include +#include +#include + +namespace ROS2 +{ + namespace Internal + { + using TopicConfigurations = AZStd::unordered_map; + + TopicConfiguration GetTopicConfiguration(const SensorConfiguration& sensorConfiguration, const AZStd::string& key) + { + auto ipos = sensorConfiguration.m_publishersConfigurations.find(key); + AZ_Assert(ipos != sensorConfiguration.m_publishersConfigurations.end(), "Missing key in topic configuration!"); + return ipos != sensorConfiguration.m_publishersConfigurations.end() ? ipos->second : TopicConfiguration{}; + } + + template + TopicConfigurations GetCameraTopicConfiguration([[maybe_unused]] const SensorConfiguration& sensorConfiguration) + { + AZ_Error("GetCameraTopicConfiguration", false, "Invalid camera template type!"); + return TopicConfigurations(); + } + + template + TopicConfigurations GetCameraInfoTopicConfiguration([[maybe_unused]] const SensorConfiguration& sensorConfiguration) + { + AZ_Error("GetCameraInfoTopicConfiguration", false, "Invalid camera template type!"); + return TopicConfigurations(); + } + + template<> + TopicConfigurations GetCameraTopicConfiguration(const SensorConfiguration& sensorConfiguration) + { + return { { CameraSensorDescription::CameraChannelType::RGB, + GetTopicConfiguration(sensorConfiguration, CameraConstants::ColorImageConfig) } }; + } + + template<> + TopicConfigurations GetCameraInfoTopicConfiguration(const SensorConfiguration& sensorConfiguration) + { + return { { CameraSensorDescription::CameraChannelType::RGB, + GetTopicConfiguration(sensorConfiguration, CameraConstants::ColorInfoConfig) } }; + } + + template<> + TopicConfigurations GetCameraTopicConfiguration(const SensorConfiguration& sensorConfiguration) + { + return { { CameraSensorDescription::CameraChannelType::DEPTH, + GetTopicConfiguration(sensorConfiguration, CameraConstants::DepthImageConfig) } }; + } + + template<> + TopicConfigurations GetCameraInfoTopicConfiguration(const SensorConfiguration& sensorConfiguration) + { + return { { CameraSensorDescription::CameraChannelType::DEPTH, + GetTopicConfiguration(sensorConfiguration, CameraConstants::DepthInfoConfig) } }; + } + + //! Helper that adds publishers based on predefined configuration. + template + void AddPublishersFromConfiguration( + const AZStd::string& cameraNamespace, + const TopicConfigurations configurations, + AZStd::unordered_map>>& publishers) + { + for (const auto& [channel, configuration] : configurations) + { + AZStd::string fullTopic = ROS2Names::GetNamespacedName(cameraNamespace, configuration.m_topic); + auto ros2Node = ROS2Interface::Get()->GetNode(); + auto publisher = ros2Node->create_publisher(fullTopic.data(), configuration.GetQoS()); + publishers[channel] = publisher; + } + } + + //! Helper that adds publishers for a camera type. + //! @tparam CameraType type of camera sensor (eg 'CameraColorSensor'). + //! @param cameraDescription complete information about camera configuration. + //! @param imagePublishers publishers of raw image formats (color image, depth image, ..). + //! @param infoPublishers publishers of camera_info messages for each image topic. + template + void AddCameraPublishers( + const CameraSensorDescription& cameraDescription, + AZStd::unordered_map& imagePublishers, + AZStd::unordered_map& infoPublishers) + { + const auto cameraImagePublisherConfigs = GetCameraTopicConfiguration(cameraDescription.m_sensorConfiguration); + AddPublishersFromConfiguration(cameraDescription.m_cameraNamespace, cameraImagePublisherConfigs, imagePublishers); + const auto cameraInfoPublisherConfigs = GetCameraInfoTopicConfiguration(cameraDescription.m_sensorConfiguration); + AddPublishersFromConfiguration(cameraDescription.m_cameraNamespace, cameraInfoPublisherConfigs, infoPublishers); + } + } // namespace Internal + + CameraPublishers::CameraPublishers(const CameraSensorDescription& cameraDescription) + { + if (cameraDescription.m_cameraConfiguration.m_colorCamera) + { + Internal::AddCameraPublishers(cameraDescription, m_imagePublishers, m_infoPublishers); + } + + if (cameraDescription.m_cameraConfiguration.m_depthCamera) + { + Internal::AddCameraPublishers(cameraDescription, m_imagePublishers, m_infoPublishers); + } + } + + CameraPublishers::ImagePublisherPtrType CameraPublishers::GetImagePublisher(CameraSensorDescription::CameraChannelType type) + { + AZ_Error("GetImagePublisher", m_imagePublishers.count(type) == 1, "No publisher of this type, logic error!"); + return m_imagePublishers.at(type); + } + + CameraPublishers::CameraInfoPublisherPtrType CameraPublishers::GetInfoPublisher(CameraSensorDescription::CameraChannelType type) + { + AZ_Error("GetInfoPublisher", m_infoPublishers.count(type) == 1, "No publisher of this type, logic error!"); + return m_infoPublishers.at(type); + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Camera/CameraPublishers.h b/Gems/ROS2/Code/Source/Camera/CameraPublishers.h new file mode 100644 index 000000000..0a31b284c --- /dev/null +++ b/Gems/ROS2/Code/Source/Camera/CameraPublishers.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 "CameraSensorDescription.h" + +#include + +#include +#include +#include +#include + +namespace ROS2 +{ + //! Handles all the ROS publishing related to a single camera. + //! This includes 1-2 CameraInfo topics as well as 1-2 Image topics. + class CameraPublishers + { + public: + //! ROS2 image publisher type. + using ImagePublisherPtrType = std::shared_ptr>; + + //! ROS2 camera sensor publisher type. + using CameraInfoPublisherPtrType = std::shared_ptr>; + + CameraPublishers(const CameraSensorDescription& cameraDescription); + + ImagePublisherPtrType GetImagePublisher(CameraSensorDescription::CameraChannelType type); + CameraInfoPublisherPtrType GetInfoPublisher(CameraSensorDescription::CameraChannelType type); + + private: + AZStd::unordered_map m_imagePublishers; + AZStd::unordered_map m_infoPublishers; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Camera/CameraSensor.cpp b/Gems/ROS2/Code/Source/Camera/CameraSensor.cpp index 93de60378..baa459bfd 100644 --- a/Gems/ROS2/Code/Source/Camera/CameraSensor.cpp +++ b/Gems/ROS2/Code/Source/Camera/CameraSensor.cpp @@ -21,6 +21,8 @@ #include #include +#include + namespace ROS2 { namespace Internal @@ -45,58 +47,75 @@ namespace ROS2 { AZ::RHI::Format::R16_UNORM, sizeof(uint16_t) }, { AZ::RHI::Format::R32_FLOAT, sizeof(float) }, }; - } // namespace Internal - - CameraSensorDescription::CameraSensorDescription(const AZStd::string& cameraName, const CameraConfiguration& configuration) - : m_cameraConfiguration(configuration) - , m_cameraName(cameraName) - , m_aspectRatio(static_cast(configuration.m_width) / static_cast(configuration.m_height)) - , m_viewToClipMatrix(MakeViewToClipMatrix()) - , m_cameraIntrinsics(MakeCameraIntrinsics()) - { - ValidateParameters(); - } - AZ::Matrix4x4 CameraSensorDescription::MakeViewToClipMatrix() const - { - const float nearDist = 0.1f, farDist = 100.0f; - AZ::Matrix4x4 localViewToClipMatrix; - AZ::MakePerspectiveFovMatrixRH( - localViewToClipMatrix, AZ::DegToRad(m_cameraConfiguration.m_verticalFieldOfViewDeg), m_aspectRatio, nearDist, farDist, true); - return localViewToClipMatrix; - } + //! Create a CameraImage message from the read-back result and a header. + sensor_msgs::msg::Image CreateImageMessageFromReadBackResult( + const AZ::EntityId& entityId, const AZ::RPI::AttachmentReadback::ReadbackResult& result, const std_msgs::msg::Header& header) + { + const AZ::RHI::ImageDescriptor& descriptor = result.m_imageDescriptor; + const auto format = descriptor.m_format; + AZ_Assert(Internal::FormatMappings.contains(format), "Unknown format in result %u", static_cast(format)); + sensor_msgs::msg::Image imageMessage; + imageMessage.encoding = Internal::FormatMappings.at(format); + imageMessage.width = descriptor.m_size.m_width; + imageMessage.height = descriptor.m_size.m_height; + imageMessage.step = imageMessage.width * Internal::BitDepth.at(format); + imageMessage.data = + std::vector(result.m_dataBuffer->data(), result.m_dataBuffer->data() + result.m_dataBuffer->size()); + imageMessage.header = header; + bool registeredPostProcessingSupportsEncoding = false; + CameraPostProcessingRequestBus::EventResult( + registeredPostProcessingSupportsEncoding, + entityId, + &CameraPostProcessingRequests::SupportsFormat, + AZStd::string(Internal::FormatMappings.at(format))); + if (registeredPostProcessingSupportsEncoding) + { + CameraPostProcessingRequestBus::Event(entityId, &CameraPostProcessingRequests::ApplyPostProcessing, imageMessage); + } + return imageMessage; + } - void CameraSensorDescription::ValidateParameters() const - { - AZ_Assert( - m_cameraConfiguration.m_verticalFieldOfViewDeg > 0.0f && m_cameraConfiguration.m_verticalFieldOfViewDeg < 180.0f, - "Vertical fov should be in range 0.0 < FoV < 180.0 degrees"); - AZ_Assert(!m_cameraName.empty(), "Camera name cannot be empty"); - } + //! Prepare a CameraInfo message from sensor description and a header. + sensor_msgs::msg::CameraInfo CreateCameraInfoMessage( + const CameraSensorDescription& cameraDescription, const std_msgs::msg::Header& header) + { + const auto& cameraIntrinsics = cameraDescription.m_cameraIntrinsics; + sensor_msgs::msg::CameraInfo cameraInfo; + cameraInfo.width = cameraDescription.m_cameraConfiguration.m_width; + cameraInfo.height = cameraDescription.m_cameraConfiguration.m_height; + cameraInfo.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; + + [[maybe_unused]] constexpr size_t expectedMatrixSize = 9; + AZ_Assert(cameraInfo.k.size() == expectedMatrixSize, "camera matrix should have %d elements", expectedMatrixSize); + cameraInfo.k = { cameraIntrinsics.GetElement(0, 0), + 0, + cameraIntrinsics.GetElement(0, 2), + 0, + cameraIntrinsics.GetElement(1, 1), + cameraIntrinsics.GetElement(1, 2), + 0, + 0, + 1 }; + cameraInfo.p = { cameraInfo.k[0], cameraInfo.k[1], cameraInfo.k[2], 0, cameraInfo.k[3], cameraInfo.k[4], cameraInfo.k[5], 0, + cameraInfo.k[6], cameraInfo.k[7], cameraInfo.k[8], 0 }; + cameraInfo.header = header; + return cameraInfo; + } - AZStd::array CameraSensorDescription::MakeCameraIntrinsics() const - { - /* Intrinsic camera matrix of the camera image is being created here - It is based on other parameters available in the structure - they must be initialized before this function is called - Matrix is row-major and has the following form: - [fx 0 cx] - [ 0 fy cy] - [ 0 0 1] - Projects 3D points in the camera coordinate frame to 2D pixel - coordinates using the focal lengths (fx, fy) and principal point - (cx, cy). - */ - const auto w = static_cast(m_cameraConfiguration.m_width); - const auto h = static_cast(m_cameraConfiguration.m_height); - const double verticalFieldOfView = AZ::DegToRad(m_cameraConfiguration.m_verticalFieldOfViewDeg); - const double horizontalFoV = 2.0 * AZStd::atan(AZStd::tan(verticalFieldOfView / 2.0) * m_aspectRatio); - const double focalLengthX = w / (2.0 * AZStd::tan(horizontalFoV / 2.0)); - const double focalLengthY = h / (2.0 * AZStd::tan(verticalFieldOfView / 2.0)); - return { focalLengthX, 0.0, w / 2.0, 0.0, focalLengthY, h / 2.0, 0.0, 0.0, 1.0 }; - } + AZStd::string PipelineNameFromChannelType(CameraSensorDescription::CameraChannelType channel) + { + static const AZStd::unordered_map channelNameMap = { + { CameraSensorDescription::CameraChannelType::RGB, "Color" }, { CameraSensorDescription::CameraChannelType::DEPTH, "Depth" } + }; + AZ_Assert(channelNameMap.count(channel) == 1, "Channel type not found in the dictionary!"); + return channelNameMap.at(channel); + } + } // namespace Internal CameraSensor::CameraSensor(const CameraSensorDescription& cameraSensorDescription, const AZ::EntityId& entityId) - : m_cameraSensorDescription(cameraSensorDescription) + : m_cameraPublishers(cameraSensorDescription) + , m_cameraSensorDescription(cameraSensorDescription) , m_entityId(entityId) { } @@ -110,8 +129,13 @@ namespace ROS2 m_view->SetViewToClipMatrix(m_cameraSensorDescription.m_viewToClipMatrix); m_scene = AZ::RPI::RPISystemInterface::Get()->GetSceneByName(AZ::Name("Main")); + auto cameraPipelineTypeName = Internal::PipelineNameFromChannelType(GetChannelType()); + m_pipelineName = AZStd::string::format( - "%sPipeline%s%s", m_cameraSensorDescription.m_cameraName.c_str(), GetPipelineTypeName().c_str(), m_entityId.ToString().c_str()); + "%sPipeline%s%s", + m_cameraSensorDescription.m_cameraName.c_str(), + cameraPipelineTypeName.c_str(), + m_entityId.ToString().c_str()); AZ::RPI::RenderPipelineDescriptor pipelineDesc; pipelineDesc.m_mainViewTagName = "MainCamera"; pipelineDesc.m_name = m_pipelineName; @@ -186,55 +210,33 @@ namespace ROS2 return m_cameraSensorDescription; } - void CameraSensor::RequestMessagePublication( - std::shared_ptr> publisher, - const AZ::Transform& cameraPose, - const std_msgs::msg::Header& header) + void CameraSensor::RequestMessagePublication(const AZ::Transform& cameraPose, const std_msgs::msg::Header& header) { + auto imagePublisher = m_cameraPublishers.GetImagePublisher(GetChannelType()); + auto infoPublisher = m_cameraPublishers.GetInfoPublisher(GetChannelType()); + if (!imagePublisher || !infoPublisher) + { + AZ_Error("CameraSensor::RequestMessagePublication", false, "Missing publisher for the Camera sensor"); + return; + } + + auto infoMessage = Internal::CreateCameraInfoMessage(m_cameraSensorDescription, header); RequestFrame( cameraPose, - [header, publisher, entityId = m_entityId](const AZ::RPI::AttachmentReadback::ReadbackResult& result) + [header, imagePublisher, infoPublisher, infoMessage, entityId = m_entityId]( + const AZ::RPI::AttachmentReadback::ReadbackResult& result) { if (result.m_state != AZ::RPI::AttachmentReadback::ReadbackState::Success) { return; } - const AZ::RHI::ImageDescriptor& descriptor = result.m_imageDescriptor; - const auto format = descriptor.m_format; - AZ_Assert(Internal::FormatMappings.contains(format), "Unknown format in result %u", static_cast(format)); - sensor_msgs::msg::Image message; - message.encoding = Internal::FormatMappings.at(format); - message.width = descriptor.m_size.m_width; - message.height = descriptor.m_size.m_height; - message.step = message.width * Internal::BitDepth.at(format); - message.data = std::vector(result.m_dataBuffer->data(), result.m_dataBuffer->data() + result.m_dataBuffer->size()); - message.header = header; - bool registeredPostProcessingSupportsEncoding = false; - CameraPostProcessingRequestBus::EventResult( - registeredPostProcessingSupportsEncoding, - entityId, - &CameraPostProcessingRequests::SupportsFormat, - AZStd::string(Internal::FormatMappings.at(format))); - if (registeredPostProcessingSupportsEncoding) - { - CameraPostProcessingRequestBus::Event(entityId, &CameraPostProcessingRequests::ApplyPostProcessing, message); - } - publisher->publish(message); + auto imageMessage = Internal::CreateImageMessageFromReadBackResult(entityId, result, header); + imagePublisher->publish(imageMessage); + infoPublisher->publish(infoMessage); }); } - void CameraSensor::RequestMessagePublication( - AZStd::span>> publishers, - const AZ::Transform& cameraPose, - const std_msgs::msg::Header& header) - { - if (!publishers.empty()) - { - RequestMessagePublication(publishers.front(), cameraPose, header); - } - } - CameraDepthSensor::CameraDepthSensor(const CameraSensorDescription& cameraSensorDescription, const AZ::EntityId& entityId) : CameraSensor(cameraSensorDescription, entityId) { @@ -246,9 +248,9 @@ namespace ROS2 return "PipelineRenderToTextureROSDepth"; }; - AZStd::string CameraDepthSensor::GetPipelineTypeName() const + CameraSensorDescription::CameraChannelType CameraDepthSensor::GetChannelType() const { - return "Depth"; + return CameraSensorDescription::CameraChannelType::DEPTH; }; CameraColorSensor::CameraColorSensor(const CameraSensorDescription& cameraSensorDescription, const AZ::EntityId& entityId) @@ -262,9 +264,9 @@ namespace ROS2 return "PipelineRenderToTextureROSColor"; }; - AZStd::string CameraColorSensor::GetPipelineTypeName() const + CameraSensorDescription::CameraChannelType CameraColorSensor::GetChannelType() const { - return "Color"; + return CameraSensorDescription::CameraChannelType::RGB; }; CameraRGBDSensor::CameraRGBDSensor(const CameraSensorDescription& cameraSensorDescription, const AZ::EntityId& entityId) @@ -285,43 +287,32 @@ namespace ROS2 AZ::RPI::PassAttachmentReadbackOption::Output); } - void CameraRGBDSensor::RequestMessagePublication( - AZStd::span>> publishers, - const AZ::Transform& cameraPose, - const std_msgs::msg::Header& header) + void CameraRGBDSensor::RequestMessagePublication(const AZ::Transform& cameraPose, const std_msgs::msg::Header& header) { - AZ_Assert(publishers.size() == 2, "RequestMessagePublication for CameraRGBDSensor should be called with exactly two publishers"); - const auto publisherDepth = publishers.back(); + auto imagePublisher = m_cameraPublishers.GetImagePublisher(CameraSensorDescription::CameraChannelType::DEPTH); + auto infoPublisher = m_cameraPublishers.GetInfoPublisher(CameraSensorDescription::CameraChannelType::DEPTH); + if (!imagePublisher || !infoPublisher) + { + AZ_Error("CameraRGBDSensor::RequestMessagePublication", false, "Missing publisher for the Camera sensor"); + return; + } + + auto infoMessage = Internal::CreateCameraInfoMessage(m_cameraSensorDescription, header); + // Process the Depth part. ReadBackDepth( - [header, publisherDepth, entityId = m_entityId](const AZ::RPI::AttachmentReadback::ReadbackResult& result) + [header, imagePublisher, infoPublisher, infoMessage, entityId = m_entityId]( + const AZ::RPI::AttachmentReadback::ReadbackResult& result) { - if (result.m_state == AZ::RPI::AttachmentReadback::ReadbackState::Success) + if (result.m_state != AZ::RPI::AttachmentReadback::ReadbackState::Success) { - const AZ::RHI::ImageDescriptor& descriptor = result.m_imageDescriptor; - const auto format = descriptor.m_format; - AZ_Assert(Internal::FormatMappings.contains(format), "Unknown format in result %u", static_cast(format)); - sensor_msgs::msg::Image message; - message.encoding = Internal::FormatMappings.at(format); - message.width = descriptor.m_size.m_width; - message.height = descriptor.m_size.m_height; - message.step = message.width * Internal::BitDepth.at(format); - message.data = - std::vector(result.m_dataBuffer->data(), result.m_dataBuffer->data() + result.m_dataBuffer->size()); - message.header = header; - bool registeredPostProcessingSupportsEncoding = false; - CameraPostProcessingRequestBus::EventResult( - registeredPostProcessingSupportsEncoding, - entityId, - &CameraPostProcessingRequests::SupportsFormat, - AZStd::string(Internal::FormatMappings.at(format))); - if (registeredPostProcessingSupportsEncoding) - { - CameraPostProcessingRequestBus::Event(entityId, &CameraPostProcessingRequests::ApplyPostProcessing, message); - } - publisherDepth->publish(message); + return; } + auto imageMessage = Internal::CreateImageMessageFromReadBackResult(entityId, result, header); + imagePublisher->publish(imageMessage); + infoPublisher->publish(infoMessage); }); - CameraSensor::RequestMessagePublication(publishers, cameraPose, header); - } + // Process the Color part. + CameraSensor::RequestMessagePublication(cameraPose, header); + } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Camera/CameraSensor.h b/Gems/ROS2/Code/Source/Camera/CameraSensor.h index cdf9ca553..dc232d3d9 100644 --- a/Gems/ROS2/Code/Source/Camera/CameraSensor.h +++ b/Gems/ROS2/Code/Source/Camera/CameraSensor.h @@ -9,42 +9,18 @@ #include #include + +#include "CameraPublishers.h" #include + #include #include +#include #include #include namespace ROS2 { - struct CameraConfiguration - { - float m_verticalFieldOfViewDeg = 90.0f; //!< Vertical field of view of camera sensor. - int m_width = 640; //!< Camera image width in pixels. - int m_height = 480; //!< Camera image height in pixels. - }; - - //! Structure containing all information required to create the camera sensor. - struct CameraSensorDescription - { - //! Constructor to create the description - //! @param cameraName - name of the camera; used to differentiate cameras in a multi-camera setup. - //! @param configuration - configuration structure for the camera, defining its characteristics. - CameraSensorDescription(const AZStd::string& cameraName, const CameraConfiguration& configuration); - - const CameraConfiguration m_cameraConfiguration; //!< Configuration of the camera. - const AZStd::string m_cameraName; //!< Camera name to differentiate cameras in a multi-camera setup. - - const float m_aspectRatio; //!< Camera image aspect ratio; equal to (width / height). - const AZ::Matrix4x4 m_viewToClipMatrix; //!< Camera view to clip space transform matrix; derived from other parameters. - const AZStd::array m_cameraIntrinsics; //!< Camera intrinsics; derived from other parameters. - - private: - AZ::Matrix4x4 MakeViewToClipMatrix() const; - AZStd::array MakeCameraIntrinsics() const; - void ValidateParameters() const; - }; - //! Class to create camera sensor using Atom renderer //! It creates dedicated rendering pipeline for each camera class CameraSensor @@ -59,37 +35,26 @@ namespace ROS2 virtual ~CameraSensor(); //! Publish Image Message frame from rendering pipeline - //! @param publishers - ROS2 publishers to publish image in future : color, depth ... - //! @param header - header with filled message information (frame, timestamp, seq) //! @param cameraPose - current camera pose from which the rendering should take place - virtual void RequestMessagePublication( - AZStd::span>> publishers, - const AZ::Transform& cameraPose, - const std_msgs::msg::Header& header); + //! @param header - header with filled message information (frame, timestamp, seq) + virtual void RequestMessagePublication(const AZ::Transform& cameraPose, const std_msgs::msg::Header& header); //! Get the camera sensor description [[nodiscard]] const CameraSensorDescription& GetCameraSensorDescription() const; private: - //! Publish Image Message frame from rendering pipeline - //! @param publisher - ROS2 publisher to publish image in future - //! @param header - header with filled message information (frame, timestamp, seq) - //! @param cameraPose - current camera pose from which the rendering should take place - void RequestMessagePublication( - std::shared_ptr> publisher, - const AZ::Transform& cameraPose, - const std_msgs::msg::Header& header); - - CameraSensorDescription m_cameraSensorDescription; AZStd::vector m_passHierarchy; AZ::RPI::ViewPtr m_view; AZ::RPI::Scene* m_scene = nullptr; const AZ::Transform AtomToRos{ AZ::Transform::CreateFromQuaternion( AZ::Quaternion::CreateFromMatrix3x3(AZ::Matrix3x3::CreateFromRows({ 1, 0, 0 }, { 0, -1, 0 }, { 0, 0, -1 }))) }; virtual AZStd::string GetPipelineTemplateName() const = 0; //! Returns name of pass template to use in pipeline - virtual AZStd::string GetPipelineTypeName() const = 0; //! Type of returned data eg Color, Depth, Optical flow + virtual CameraSensorDescription::CameraChannelType GetChannelType() + const = 0; //! Type of returned data eg Color, Depth, Optical flow protected: + CameraSensorDescription m_cameraSensorDescription; + CameraPublishers m_cameraPublishers; AZ::EntityId m_entityId; AZ::RPI::RenderPipelinePtr m_pipeline; AZStd::string m_pipelineName; @@ -101,16 +66,6 @@ namespace ROS2 void RequestFrame( const AZ::Transform& cameraPose, AZStd::function callback); - //! Request an additional frame from the rendering pipeline - //! @param callback - callback function object that will be called when capture is ready. - //! It's argument is readback structure containing, among other things, a captured image - //! @param passName - pass name in pipeline, eg `DepthToLinearDepthPass` - //! @param slotName - slot name in selected pass, eg `Output` - void RequestAdditionalFrame( - AZStd::function callback, - const AZStd::string& passName, - const AZStd::string& slotName); - //! Read and setup Atom Passes void SetupPasses(); }; @@ -123,7 +78,7 @@ namespace ROS2 private: AZStd::string GetPipelineTemplateName() const override; - AZStd::string GetPipelineTypeName() const override; + CameraSensorDescription::CameraChannelType GetChannelType() const override; }; //! Implementation of camera sensors that runs pipeline which produces color image @@ -134,7 +89,7 @@ namespace ROS2 private: AZStd::string GetPipelineTemplateName() const override; - AZStd::string GetPipelineTypeName() const override; + CameraSensorDescription::CameraChannelType GetChannelType() const override; }; //! Implementation of camera sensors that runs pipeline which produces color image and readbacks a depth image from pipeline @@ -144,10 +99,7 @@ namespace ROS2 CameraRGBDSensor(const CameraSensorDescription& cameraSensorDescription, const AZ::EntityId& entityId); // CameraSensor overrides - void RequestMessagePublication( - AZStd::span>> publishers, - const AZ::Transform& cameraPose, - const std_msgs::msg::Header& header) override; + void RequestMessagePublication(const AZ::Transform& cameraPose, const std_msgs::msg::Header& header) override; private: void ReadBackDepth(AZStd::function callback); diff --git a/Gems/ROS2/Code/Source/Camera/CameraSensorConfiguration.cpp b/Gems/ROS2/Code/Source/Camera/CameraSensorConfiguration.cpp index c951fdafe..ece6cacc2 100644 --- a/Gems/ROS2/Code/Source/Camera/CameraSensorConfiguration.cpp +++ b/Gems/ROS2/Code/Source/Camera/CameraSensorConfiguration.cpp @@ -17,12 +17,14 @@ namespace ROS2 if (auto serializeContext = azrtti_cast(context)) { serializeContext->Class() - ->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()) { @@ -32,10 +34,24 @@ namespace ROS2 &CameraSensorConfiguration::m_verticalFieldOfViewDeg, "Vertical field of view", "Camera's vertical (y axis) field of view in degrees.") + ->Attribute(AZ::Edit::Attributes::Min, &CameraSensorConfiguration::m_minVerticalFieldOfViewDeg) + ->Attribute(AZ::Edit::Attributes::Max, &CameraSensorConfiguration::m_maxVerticalFieldOfViewDeg) ->DataElement(AZ::Edit::UIHandlers::Default, &CameraSensorConfiguration::m_width, "Image width", "Image width") + ->Attribute(AZ::Edit::Attributes::Min, CameraSensorConfiguration::m_minWidth) ->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"); } } } diff --git a/Gems/ROS2/Code/Source/Camera/CameraSensorConfiguration.h b/Gems/ROS2/Code/Source/Camera/CameraSensorConfiguration.h index 371f861df..705173481 100644 --- a/Gems/ROS2/Code/Source/Camera/CameraSensorConfiguration.h +++ b/Gems/ROS2/Code/Source/Camera/CameraSensorConfiguration.h @@ -19,10 +19,17 @@ namespace ROS2 AZ_TYPE_INFO(CameraSensorConfiguration, "{386A2640-442B-473D-BC2A-665D049D7EF5}"); static void Reflect(AZ::ReflectContext* context); + static constexpr int m_minWidth = 1; + static constexpr int m_minHeight = 1; + float m_verticalFieldOfViewDeg = 90.0f; //!< Vertical field of view of camera sensor. + float m_minVerticalFieldOfViewDeg = 0.0f; + float m_maxVerticalFieldOfViewDeg = 360.0f; int m_width = 640; //!< Camera image width in pixels. 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 diff --git a/Gems/ROS2/Code/Source/Camera/CameraSensorDescription.cpp b/Gems/ROS2/Code/Source/Camera/CameraSensorDescription.cpp new file mode 100644 index 000000000..89c5bda25 --- /dev/null +++ b/Gems/ROS2/Code/Source/Camera/CameraSensorDescription.cpp @@ -0,0 +1,51 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#include "CameraSensorDescription.h" +#include "CameraUtilities.h" +#include + +namespace ROS2 +{ + CameraSensorDescription::CameraSensorDescription( + const AZStd::string& cameraName, + const AZStd::string& effectiveNamespace, + const CameraSensorConfiguration& configuration, + const SensorConfiguration& sensorConfiguration) + : m_cameraConfiguration(configuration) + , m_sensorConfiguration(sensorConfiguration) + , 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_nearClipDistance, + m_cameraConfiguration.m_farClipDistance)) + , m_cameraIntrinsics(CameraUtils::MakeCameraIntrinsics( + m_cameraConfiguration.m_width, m_cameraConfiguration.m_height, m_cameraConfiguration.m_verticalFieldOfViewDeg)) + { + ValidateParameters(); + } + + void CameraSensorDescription::ValidateParameters() const + { + AZ_Assert( + m_cameraConfiguration.m_verticalFieldOfViewDeg > 0.0f && m_cameraConfiguration.m_verticalFieldOfViewDeg < 180.0f, + "Vertical fov should be in range 0.0 < FoV < 180.0 degrees"); + 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 diff --git a/Gems/ROS2/Code/Source/Camera/CameraSensorDescription.h b/Gems/ROS2/Code/Source/Camera/CameraSensorDescription.h new file mode 100644 index 000000000..8f678b25b --- /dev/null +++ b/Gems/ROS2/Code/Source/Camera/CameraSensorDescription.h @@ -0,0 +1,51 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ +#pragma once + +#include "CameraSensorConfiguration.h" +#include + +#include +#include +#include + +namespace ROS2 +{ + //! Structure containing all information required to create the camera sensor. + struct CameraSensorDescription + { + //! Type of camera channel. + enum class CameraChannelType + { + RGB = 0, + DEPTH = 1 + }; + + //! Constructor to create the description + //! @param cameraName - name of the camera; used to differentiate cameras in a multi-camera setup. + //! @param effectiveNamespace - namespace for camera frames and topics. + //! @param configuration - configuration structure for the camera, defining its characteristics. + //! @param sensorConfiguration - generic configuration for this sensor. + CameraSensorDescription( + const AZStd::string& cameraName, + const AZStd::string& effectiveNamespace, + const CameraSensorConfiguration& configuration, + const SensorConfiguration& sensorConfiguration); + + const CameraSensorConfiguration m_cameraConfiguration; //!< Configuration of the camera. + const SensorConfiguration m_sensorConfiguration; //!< Generic sensor configuration. + const AZStd::string m_cameraName; //!< Camera name to differentiate cameras in a multi-camera setup. + const AZStd::string m_cameraNamespace; //!< Effective camera namespace for frames and topics. + + const AZ::Matrix4x4 m_viewToClipMatrix; //!< Camera view to clip space transform matrix; derived from other parameters. + const AZ::Matrix3x3 m_cameraIntrinsics; //!< Camera intrinsics; derived from other parameters. + + private: + void ValidateParameters() const; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Camera/CameraUtilities.cpp b/Gems/ROS2/Code/Source/Camera/CameraUtilities.cpp new file mode 100644 index 000000000..09d314393 --- /dev/null +++ b/Gems/ROS2/Code/Source/Camera/CameraUtilities.cpp @@ -0,0 +1,43 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#include +#include + +namespace ROS2::CameraUtils +{ + float GetAspectRatio(float width, float height) + { + return width / height; + }; + + AZ::Matrix3x3 MakeCameraIntrinsics(int width, int height, float verticalFieldOfViewDeg) + { + const auto w = static_cast(width); + const auto h = static_cast(height); + const float verticalFieldOfView = AZ::DegToRad(verticalFieldOfViewDeg); + const float horizontalFoV = 2.0 * AZStd::atan(AZStd::tan(verticalFieldOfView / 2.0) * GetAspectRatio(width, height)); + const float focalLengthX = w / (2.0 * AZStd::tan(horizontalFoV / 2.0)); + const float focalLengthY = h / (2.0 * AZStd::tan(verticalFieldOfView / 2.0)); + return AZ::Matrix3x3::CreateFromRows({ focalLengthX, 0.f, w / 2.f }, { 0.f, focalLengthY, h / 2.f }, { 0.f, 0.f, 1.f }); + } + + AZ::Matrix4x4 MakeClipMatrix(int width, int height, float verticalFieldOfViewDeg, float nearDist, float farDist) + { + AZ::Matrix4x4 localViewToClipMatrix; + AZ::MakePerspectiveFovMatrixRH( + localViewToClipMatrix, + AZ::DegToRad(verticalFieldOfViewDeg), + CameraUtils::GetAspectRatio(width, height), + nearDist, + farDist, + true); + return localViewToClipMatrix; + } + +} // namespace ROS2::CameraUtils \ No newline at end of file diff --git a/Gems/ROS2/Code/Source/Camera/CameraUtilities.h b/Gems/ROS2/Code/Source/Camera/CameraUtilities.h new file mode 100644 index 000000000..391554833 --- /dev/null +++ b/Gems/ROS2/Code/Source/Camera/CameraUtilities.h @@ -0,0 +1,36 @@ +/* + * 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 + +//! Namespace contains utility functions for camera. +namespace ROS2::CameraUtils +{ + //! Function computes aspect ratio of the image. + //! @param width Width of the image in pixels + //! @param height Height of the image in pixels. + //! @return Aspect ratio of the image. + float GetAspectRatio(float width, float height); + + //! Function computes 3x3 projection matrix (pinhole model) from camera config. + //! @param height Height of the image in pixels. + //! @param width Width of the image in pixels + //! @param verticalFieldOfViewDeg Vertical field of view of the camera in degrees. + //! @return projection matrix for computer vision applications. + AZ::Matrix3x3 MakeCameraIntrinsics(int width, int height, float verticalFieldOfViewDeg); + + //! Function computes 4x4 projection matrix (frustum model) from camera config. + //! @param height Height of the image in pixels. + //! @param height Height of the image in pixels. + //! @param verticalFieldOfViewDeg Vertical field of view of the camera in degrees. + //! @param farDist Far clipping plane distance in meters. + //! @param nearDist Near clipping plane distance in meters. + //! @return projection matrix for the rendering. + AZ::Matrix4x4 MakeClipMatrix(int width, int height, float verticalFieldOfViewDeg, float nearDist = 0.1f, float farDist = 100.0f); +} // namespace ROS2::CameraUtils diff --git a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.cpp b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.cpp index b47f594cc..f348ffc7f 100644 --- a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.cpp @@ -7,16 +7,9 @@ */ #include "ROS2CameraSensorComponent.h" -#include +#include "CameraUtilities.h" #include -#include -#include -#include -#include - -#include - namespace ROS2 { ROS2CameraSensorComponent::ROS2CameraSensorComponent( @@ -28,71 +21,105 @@ namespace ROS2 void ROS2CameraSensorComponent::Reflect(AZ::ReflectContext* context) { + CameraSensorConfiguration::Reflect(context); + auto* serialize = azrtti_cast(context); if (serialize) { - serialize->Class()->Version(4)->Field( + serialize->Class()->Version(5)->Field( "CameraSensorConfig", &ROS2CameraSensorComponent::m_cameraConfiguration); } } void ROS2CameraSensorComponent::Activate() { - ROS2SensorComponent::Activate(); - if (m_cameraConfiguration.m_colorCamera && m_cameraConfiguration.m_depthCamera) { - AddImageSource(); + SetImageSource(); } else if (m_cameraConfiguration.m_colorCamera) { - AddImageSource(); + SetImageSource(); } else if (m_cameraConfiguration.m_depthCamera) { - AddImageSource(); + SetImageSource(); } const auto* component = Utils::GetGameOrEditorComponent(GetEntity()); AZ_Assert(component, "Entity has no ROS2FrameComponent"); m_frameName = component->GetFrameID(); + ROS2::CameraCalibrationRequestBus::Handler::BusConnect(GetEntityId()); + + StartSensor( + m_sensorConfiguration.m_frequency, + [this]([[maybe_unused]] auto&&... args) + { + if (!m_sensorConfiguration.m_publishingEnabled) + { + return; + } + FrequencyTick(); + }); } void ROS2CameraSensorComponent::Deactivate() { + StopSensor(); m_cameraSensor.reset(); - m_imagePublishers.clear(); - m_cameraInfoPublishers.clear(); - ROS2SensorComponent::Deactivate(); + ROS2::CameraCalibrationRequestBus::Handler::BusDisconnect(GetEntityId()); + } + + AZ::Matrix3x3 ROS2CameraSensorComponent::GetCameraMatrix() const + { + return CameraUtils::MakeCameraIntrinsics( + m_cameraConfiguration.m_width, m_cameraConfiguration.m_height, m_cameraConfiguration.m_verticalFieldOfViewDeg); + } + + int ROS2CameraSensorComponent::GetWidth() const + { + return m_cameraConfiguration.m_width; + } + + int ROS2CameraSensorComponent::GetHeight() const + { + return m_cameraConfiguration.m_height; + } + + float ROS2CameraSensorComponent::GetVerticalFOV() const + { + return m_cameraConfiguration.m_verticalFieldOfViewDeg; + } + + void ROS2CameraSensorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required) + { + required.push_back(AZ_CRC("ROS2Frame")); + } + + void ROS2CameraSensorComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) + { + incompatible.push_back(AZ_CRC_CE("ROS2CameraSensor")); + } + + void ROS2CameraSensorComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) + { + provided.push_back(AZ_CRC_CE("ROS2CameraSensor")); } void ROS2CameraSensorComponent::FrequencyTick() { - const AZ::Transform transform = GetEntity()->GetTransform()->GetWorldTM(); - const auto timestamp = ROS2Interface::Get()->GetROSTimestamp(); - std_msgs::msg::Header ros_header; - if (!m_imagePublishers.empty() && m_cameraSensor) + if (!m_cameraSensor) { - const auto& cameraDescription = m_cameraSensor->GetCameraSensorDescription(); - const auto& cameraIntrinsics = cameraDescription.m_cameraIntrinsics; - sensor_msgs::msg::CameraInfo cameraInfo; - ros_header.stamp = timestamp; - ros_header.frame_id = m_frameName.c_str(); - cameraInfo.header = ros_header; - cameraInfo.width = m_cameraConfiguration.m_width; - cameraInfo.height = m_cameraConfiguration.m_height; - cameraInfo.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; - AZ_Assert(cameraIntrinsics.size() == 9, "camera matrix should have 9 elements"); - AZ_Assert(cameraInfo.k.size() == 9, "camera matrix should have 9 elements"); - AZStd::copy(cameraIntrinsics.begin(), cameraIntrinsics.end(), cameraInfo.k.begin()); - cameraInfo.p = { cameraInfo.k[0], cameraInfo.k[1], cameraInfo.k[2], 0, cameraInfo.k[3], cameraInfo.k[4], cameraInfo.k[5], 0, - cameraInfo.k[6], cameraInfo.k[7], cameraInfo.k[8], 0 }; - for (CameraInfoPublisherPtrType& cameraInfoPublisher : m_cameraInfoPublishers) - { - cameraInfoPublisher->publish(cameraInfo); - } - m_cameraSensor->RequestMessagePublication(m_imagePublishers, transform, ros_header); + return; } + + const AZ::Transform& transform = GetEntity()->GetTransform()->GetWorldTM(); + const auto timestamp = ROS2Interface::Get()->GetROSTimestamp(); + + std_msgs::msg::Header messageHeader; + messageHeader.stamp = timestamp; + messageHeader.frame_id = m_frameName.c_str(); + m_cameraSensor->RequestMessagePublication(transform, messageHeader); } AZStd::string ROS2CameraSensorComponent::GetCameraNameFromFrame(const AZ::Entity* entity) const diff --git a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.h b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.h index 0ff4d9b5f..645d6a17a 100644 --- a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.h +++ b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.h @@ -12,29 +12,17 @@ #include #include +#include #include "CameraSensor.h" #include "CameraSensorConfiguration.h" -#include "ROS2/Communication/TopicConfiguration.h" -#include -#include -#include +#include #include -#include -#include +#include +#include namespace ROS2 { - namespace CameraConstants - { - inline constexpr char ImageMessageType[] = "sensor_msgs::msg::Image"; - inline constexpr char DepthImageConfig[] = "Depth Image"; - inline constexpr char ColorImageConfig[] = "Color Image"; - inline constexpr char DepthInfoConfig[] = "Depth Camera Info"; - inline constexpr char ColorInfoConfig[] = "Color Camera Info"; - inline constexpr char CameraInfoMessageType[] = "sensor_msgs::msg::CameraInfo"; - } // namespace CameraConstants - //! ROS2 Camera sensor component class //! Allows turning an entity into a camera sensor //! Can be parametrized with following values: @@ -42,123 +30,50 @@ namespace ROS2 //! - camera image width and height in pixels //! - camera vertical field of view in degrees //! Camera frustum is facing negative Z axis; image plane is parallel to X,Y plane: X - right, Y - up - class ROS2CameraSensorComponent : public ROS2SensorComponent + class ROS2CameraSensorComponent + : public ROS2SensorComponentBase + , public CameraCalibrationRequestBus::Handler { public: ROS2CameraSensorComponent() = default; ROS2CameraSensorComponent(const SensorConfiguration& sensorConfiguration, const CameraSensorConfiguration& cameraConfiguration); - + static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); + static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible); + static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); ~ROS2CameraSensorComponent() override = default; - AZ_COMPONENT(ROS2CameraSensorComponent, "{3C6B8AE6-9721-4639-B8F9-D8D28FD7A071}", ROS2SensorComponent); + + AZ_COMPONENT(ROS2CameraSensorComponent, "{3C6B8AE6-9721-4639-B8F9-D8D28FD7A071}", SensorBaseType); static void Reflect(AZ::ReflectContext* context); + // AzToolsFramework::Components::EditorComponentBase overrides .. void Activate() override; void Deactivate() override; - private: - //! Pointer to ROS2 image publisher type - using ImagePublisherPtrType = std::shared_ptr>; - - //! Pointer to ROS2 camera sensor publisher type - using CameraInfoPublisherPtrType = std::shared_ptr>; - - template - AZStd::vector GetCameraTopicConfiguration() - { - TopicConfiguration defaultConfig; - return { defaultConfig }; - } - - template - AZStd::vector GetCameraInfoTopicConfiguration() - { - TopicConfiguration defaultConfig; - return { defaultConfig }; - } - - template<> - AZStd::vector GetCameraTopicConfiguration() - { - return { m_sensorConfiguration.m_publishersConfigurations[CameraConstants::ColorImageConfig] }; - } - - template<> - AZStd::vector GetCameraInfoTopicConfiguration() - { - return { m_sensorConfiguration.m_publishersConfigurations[CameraConstants::ColorInfoConfig] }; - } - - template<> - AZStd::vector GetCameraTopicConfiguration() - { - return { m_sensorConfiguration.m_publishersConfigurations[CameraConstants::DepthImageConfig] }; - } - - template<> - AZStd::vector GetCameraInfoTopicConfiguration() - { - return { m_sensorConfiguration.m_publishersConfigurations[CameraConstants::DepthInfoConfig] }; - } - - template<> - AZStd::vector GetCameraTopicConfiguration() - { - // Note: the order matters. - return { m_sensorConfiguration.m_publishersConfigurations[CameraConstants::ColorImageConfig], - m_sensorConfiguration.m_publishersConfigurations[CameraConstants::DepthImageConfig] }; - } - - template<> - AZStd::vector GetCameraInfoTopicConfiguration() - { - // Note: the order matters. - return { m_sensorConfiguration.m_publishersConfigurations[CameraConstants::ColorInfoConfig], - m_sensorConfiguration.m_publishersConfigurations[CameraConstants::DepthInfoConfig] }; - } - - //! Helper that adds publishers based on predefined configuration. - template - void AddPublishersFromConfiguration( - const AZStd::vector configurations, - AZStd::vector>>& publishers) - { - for (const auto& configuration : configurations) - { - AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), configuration.m_topic); - auto ros2Node = ROS2Interface::Get()->GetNode(); - auto publisher = ros2Node->create_publisher(fullTopic.data(), configuration.GetQoS()); - publishers.emplace_back(publisher); - } - } + // CameraCalibrationRequestBus::Handler overrides ... + AZ::Matrix3x3 GetCameraMatrix() const override; + int GetWidth() const override; + int GetHeight() const override; + float GetVerticalFOV() const override; + private: //! Helper that adds an image source. //! @tparam CameraType type of camera sensor (eg 'CameraColorSensor') template - void AddImageSource() + void SetImageSource() { - CameraConfiguration cameraConfiguration = { m_cameraConfiguration.m_verticalFieldOfViewDeg, - m_cameraConfiguration.m_width, - m_cameraConfiguration.m_height }; - const CameraSensorDescription description{ GetCameraNameFromFrame(GetEntity()), cameraConfiguration }; - const auto cameraImagePublisherConfigs = GetCameraTopicConfiguration(); - AddPublishersFromConfiguration(cameraImagePublisherConfigs, m_imagePublishers); - const auto cameraInfoPublisherConfigs = GetCameraInfoTopicConfiguration(); - AddPublishersFromConfiguration(cameraInfoPublisherConfigs, m_cameraInfoPublishers); + const auto cameraName = GetCameraNameFromFrame(GetEntity()); + const CameraSensorDescription description{ cameraName, GetNamespace(), m_cameraConfiguration, m_sensorConfiguration }; m_cameraSensor = AZStd::make_shared(description, GetEntityId()); } - //! Retrieve camera name from ROS2FrameComponent's FrameID. - //! @param entity pointer entity that has ROS2FrameComponent - //! @returns FrameID from ROS2FrameComponent - AZStd::string GetCameraNameFromFrame(const AZ::Entity* entity) const; + //! @param entity pointer entity that has ROS2FrameComponent. + [[nodiscard]] AZStd::string GetCameraNameFromFrame(const AZ::Entity* entity) const; - void FrequencyTick() override; + ///! Requests message publication from camera sensor. + void FrequencyTick(); CameraSensorConfiguration m_cameraConfiguration; AZStd::string m_frameName; - - AZStd::vector m_imagePublishers; AZStd::shared_ptr m_cameraSensor; - AZStd::vector m_cameraInfoPublishers; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp index 181f8656f..9624f9a95 100644 --- a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp +++ b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp @@ -7,6 +7,8 @@ */ #include "ROS2CameraSensorEditorComponent.h" +#include "CameraConstants.h" +#include "CameraUtilities.h" #include "ROS2CameraSensorComponent.h" #include #include @@ -26,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(context)) { serialize->Class() @@ -43,6 +50,8 @@ namespace ROS2 ->ClassElement(AZ::Edit::ClassElements::EditorData, "") ->Attribute(AZ::Edit::Attributes::Category, "ROS2") ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game")) + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/ROS2CameraSensor.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/ROS2CameraSensor.svg") ->DataElement( AZ::Edit::UIHandlers::Default, &ROS2CameraSensorEditorComponent::m_sensorConfiguration, @@ -61,12 +70,14 @@ namespace ROS2 { AzFramework::EntityDebugDisplayEventBus::Handler::BusConnect(this->GetEntityId()); AzToolsFramework::Components::EditorComponentBase::Activate(); + ROS2::CameraCalibrationRequestBus::Handler::BusConnect(GetEntityId()); } void ROS2CameraSensorEditorComponent::Deactivate() { AzToolsFramework::Components::EditorComponentBase::Deactivate(); AzFramework::EntityDebugDisplayEventBus::Handler::BusDisconnect(); + ROS2::CameraCalibrationRequestBus::Handler::BusDisconnect(GetEntityId()); } void ROS2CameraSensorEditorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required) @@ -79,9 +90,9 @@ namespace ROS2 incompatible.push_back(AZ_CRC_CE("ROS2CameraSensor")); } - void ROS2CameraSensorEditorComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& required) + void ROS2CameraSensorEditorComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) { - required.push_back(AZ_CRC_CE("ROS2CameraSensor")); + provided.push_back(AZ_CRC_CE("ROS2CameraSensor")); } void ROS2CameraSensorEditorComponent::BuildGameEntity(AZ::Entity* gameEntity) @@ -89,67 +100,86 @@ namespace ROS2 gameEntity->CreateComponent(m_sensorConfiguration, m_cameraSensorConfiguration); } + AZ::Matrix3x3 ROS2CameraSensorEditorComponent::GetCameraMatrix() const + { + return CameraUtils::MakeCameraIntrinsics( + m_cameraSensorConfiguration.m_width, + m_cameraSensorConfiguration.m_height, + m_cameraSensorConfiguration.m_verticalFieldOfViewDeg); + }; + + int ROS2CameraSensorEditorComponent::GetWidth() const + { + return m_cameraSensorConfiguration.m_width; + }; + + int ROS2CameraSensorEditorComponent::GetHeight() const + { + return m_cameraSensorConfiguration.m_height; + }; + + float ROS2CameraSensorEditorComponent::GetVerticalFOV() const + { + return m_cameraSensorConfiguration.m_verticalFieldOfViewDeg; + }; + 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(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)); @@ -164,6 +194,8 @@ namespace ROS2 debugDisplay.SetLineWidth(2); debugDisplay.DrawLine(ptz, pt0); + debugDisplay.PopMatrix(); + debugDisplay.SetState(stateBefore); } diff --git a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h index 06039deb2..787338502 100644 --- a/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h +++ b/Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h @@ -13,6 +13,7 @@ #include #include "CameraSensorConfiguration.h" +#include #include #include #include @@ -21,19 +22,22 @@ 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 , protected AzFramework::EntityDebugDisplayEventBus::Handler { 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); static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible); - static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& required); + static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); void Activate() override; void Deactivate() override; @@ -41,12 +45,19 @@ namespace ROS2 // AzToolsFramework::Components::EditorComponentBase overrides void BuildGameEntity(AZ::Entity* gameEntity) override; + // CameraCalibrationRequestBus::Handler overrides + AZ::Matrix3x3 GetCameraMatrix() const override; + int GetWidth() const override; + int GetHeight() const override; + float GetVerticalFOV() const override; + private: // EntityDebugDisplayEventBus::Handler overrides void DisplayEntityViewport(const AzFramework::ViewportInfo& viewportInfo, AzFramework::DebugDisplayRequests& debugDisplay) override; AZStd::pair MakeTopicConfigurationPair( const AZStd::string& topic, const AZStd::string& messageType, const AZStd::string& configName) const; + SensorConfiguration m_sensorConfiguration; CameraSensorConfiguration m_cameraSensorConfiguration; }; diff --git a/Gems/ROS2/Code/Source/Clock/PhysicallyStableClock.cpp b/Gems/ROS2/Code/Source/Clock/PhysicallyStableClock.cpp index 145283afc..6b6890799 100644 --- a/Gems/ROS2/Code/Source/Clock/PhysicallyStableClock.cpp +++ b/Gems/ROS2/Code/Source/Clock/PhysicallyStableClock.cpp @@ -18,6 +18,11 @@ namespace ROS2 void PhysicallyStableClock::Activate() { auto* systemInterface = AZ::Interface::Get(); + if (!systemInterface) + { + AZ_Warning("SimulationPhysicalClock", false, "Failed to get AzPhysics::SystemInterface"); + return; + } m_onSceneSimulationEvent = AzPhysics::SceneEvents::OnSceneSimulationFinishHandler( [this](AzPhysics::SceneHandle sceneHandle, float deltaTime) { diff --git a/Gems/ROS2/Code/Source/Communication/PublisherConfiguration.cpp b/Gems/ROS2/Code/Source/Communication/PublisherConfiguration.cpp new file mode 100644 index 000000000..08e2ea6d2 --- /dev/null +++ b/Gems/ROS2/Code/Source/Communication/PublisherConfiguration.cpp @@ -0,0 +1,38 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#include +#include +#include + +namespace ROS2 +{ + void PublisherConfiguration::Reflect(AZ::ReflectContext* context) + { + if (auto serializeContext = azrtti_cast(context)) + { + serializeContext->Class() + ->Version(1) + ->Field("Publish", &PublisherConfiguration::m_publish) + ->Field("Topic", &PublisherConfiguration::m_topicConfiguration) + ->Field("Frequency", &PublisherConfiguration::m_frequency); + + if (AZ::EditContext* ec = serializeContext->GetEditContext()) + { + ec->Class("Publisher", "Configuration of publisher") + ->ClassElement(AZ::Edit::ClassElements::EditorData, "") + ->DataElement( + AZ::Edit::UIHandlers::Default, &PublisherConfiguration::m_publish, "Publish", "Whether the publisher is on") + ->DataElement( + AZ::Edit::UIHandlers::Default, &PublisherConfiguration::m_topicConfiguration, "Topic", "Topic configuration") + ->DataElement( + AZ::Edit::UIHandlers::Default, &PublisherConfiguration::m_frequency, "Frequency (Hz)", "Publishing frequency"); + } + } + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Communication/TopicConfiguration.cpp b/Gems/ROS2/Code/Source/Communication/TopicConfiguration.cpp index 07853592a..ef5af72f7 100644 --- a/Gems/ROS2/Code/Source/Communication/TopicConfiguration.cpp +++ b/Gems/ROS2/Code/Source/Communication/TopicConfiguration.cpp @@ -27,6 +27,7 @@ namespace ROS2 ec->Class("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"); diff --git a/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.cpp b/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.cpp new file mode 100644 index 000000000..d7c415f9f --- /dev/null +++ b/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.cpp @@ -0,0 +1,198 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include +#include + +namespace ROS2 +{ + namespace + { + 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 (auto* serialize = azrtti_cast(context)) + { + serialize->Class()->Version(2); + + if (auto* editContext = serialize->GetEditContext()) + { + editContext->Class("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")) + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/ROS2ContactSensor.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/ROS2ContactSensor.svg"); + } + } + } + + 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(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 lock(m_activeContactsMutex); + m_activeContacts.erase(event.m_body2->GetEntityId()); + }); + + StartSensor( + m_sensorConfiguration.m_frequency, + [this](auto&&... args) + { + if (!m_sensorConfiguration.m_publishingEnabled) + { + return; + } + FrequencyTick(); + }); + } + + void ROS2ContactSensorComponent::Deactivate() + { + StopSensor(); + m_activeContacts.clear(); + m_contactsPublisher.reset(); + m_onCollisionBeginHandler.Disconnect(); + m_onCollisionPersistHandler.Disconnect(); + m_onCollisionEndHandler.Disconnect(); + } + + void ROS2ContactSensorComponent::FrequencyTick() + { + // Connects the collision handlers if not already connected + AzPhysics::SystemInterface* physicsSystem = AZ::Interface::Get(); + if (!physicsSystem) + { + return; + } + + if (!m_onCollisionBeginHandler.IsConnected() || !m_onCollisionPersistHandler.IsConnected() || + !m_onCollisionEndHandler.IsConnected()) + { + AZStd::pair 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(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 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 < 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 lock(m_activeContactsMutex); + m_activeContacts[event.m_body2->GetEntityId()] = AZStd::move(state); + } + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.h b/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.h new file mode 100644 index 000000000..a6c3344a2 --- /dev/null +++ b/Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.h @@ -0,0 +1,62 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 ROS2SensorComponentBase + { + public: + AZ_COMPONENT(ROS2ContactSensorComponent, "{91272e66-c9f1-4aa2-a9d5-98eaa4ef4e9a}", SensorBaseType); + 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: + ////////////////////////////////////////////////////////////////////////// + void FrequencyTick(); + + 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> m_contactsPublisher; + + AZStd::unordered_map m_activeContacts; + AZStd::mutex m_activeContactsMutex; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Frame/NamespaceConfiguration.cpp b/Gems/ROS2/Code/Source/Frame/NamespaceConfiguration.cpp index 196b128f1..65f9f02f4 100644 --- a/Gems/ROS2/Code/Source/Frame/NamespaceConfiguration.cpp +++ b/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; diff --git a/Gems/ROS2/Code/Source/Frame/ROS2FrameComponent.cpp b/Gems/ROS2/Code/Source/Frame/ROS2FrameComponent.cpp index b5be0b0c4..da33d84e5 100644 --- a/Gems/ROS2/Code/Source/Frame/ROS2FrameComponent.cpp +++ b/Gems/ROS2/Code/Source/Frame/ROS2FrameComponent.cpp @@ -15,7 +15,6 @@ #include #include #include - namespace ROS2 { namespace Internal @@ -93,7 +92,9 @@ namespace ROS2 m_entity, AZ::Uuid("{B01FD1D2-1D91-438D-874A-BF5EB7E919A8}")); // Physx::JointComponent; const bool hasFixedJoints = Internal::CheckIfEntityHasComponentOfType( m_entity, AZ::Uuid("{02E6C633-8F44-4CEE-AE94-DCB06DE36422}")); // Physx::FixedJointComponent - m_isDynamic = hasJoints && !hasFixedJoints; + const bool hasArticulations = Internal::CheckIfEntityHasComponentOfType( + m_entity, AZ::Uuid("{48751E98-B35F-4A2F-A908-D9CDD5230264}")); // Physx::ArticulationComponent + m_isDynamic = (hasJoints && !hasFixedJoints) || hasArticulations; } AZ_TracePrintf( @@ -138,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(); @@ -147,6 +153,7 @@ namespace ROS2 { return m_isDynamic; } + const ROS2FrameComponent* ROS2FrameComponent::GetParentROS2FrameComponent() const { return Internal::GetFirstROS2FrameAncestor(GetEntity()); @@ -199,6 +206,16 @@ namespace ROS2 return m_namespaceConfiguration.GetNamespace(parentNamespace); } + AZ::Name ROS2FrameComponent::GetJointName() const + { + return AZ::Name(ROS2Names::GetNamespacedName(GetNamespace(), m_jointNameString).c_str()); + } + + void ROS2FrameComponent::SetJointName(const AZStd::string& jointNameString) + { + m_jointNameString = jointNameString; + } + void ROS2FrameComponent::Reflect(AZ::ReflectContext* context) { NamespaceConfiguration::Reflect(context); @@ -208,6 +225,7 @@ namespace ROS2 ->Version(1) ->Field("Namespace Configuration", &ROS2FrameComponent::m_namespaceConfiguration) ->Field("Frame Name", &ROS2FrameComponent::m_frameName) + ->Field("Joint Name", &ROS2FrameComponent::m_jointNameString) ->Field("Publish Transform", &ROS2FrameComponent::m_publishTransform); if (AZ::EditContext* ec = serialize->GetEditContext()) @@ -216,12 +234,15 @@ namespace ROS2 ->ClassElement(AZ::Edit::ClassElements::EditorData, "") ->Attribute(AZ::Edit::Attributes::Category, "ROS2") ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/ROS2Frame.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/ROS2Frame.svg") ->DataElement( AZ::Edit::UIHandlers::Default, &ROS2FrameComponent::m_namespaceConfiguration, "Namespace Configuration", "Namespace Configuration") ->DataElement(AZ::Edit::UIHandlers::Default, &ROS2FrameComponent::m_frameName, "Frame Name", "Frame Name") + ->DataElement(AZ::Edit::UIHandlers::Default, &ROS2FrameComponent::m_jointNameString, "Joint Name", "Joint Name") ->DataElement( AZ::Edit::UIHandlers::Default, &ROS2FrameComponent::m_publishTransform, "Publish Transform", "Publish Transform"); } diff --git a/Gems/ROS2/Code/Source/GNSS/GNSSSensorConfiguration.cpp b/Gems/ROS2/Code/Source/GNSS/GNSSSensorConfiguration.cpp new file mode 100644 index 000000000..fa518d3a4 --- /dev/null +++ b/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 +#include + +namespace ROS2 +{ + void GNSSSensorConfiguration::Reflect(AZ::ReflectContext* context) + { + if (AZ::SerializeContext* serialize = azrtti_cast(context)) + { + serialize->Class() + ->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("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 diff --git a/Gems/ROS2/Code/Source/GNSS/GNSSSensorConfiguration.h b/Gems/ROS2/Code/Source/GNSS/GNSSSensorConfiguration.h new file mode 100644 index 000000000..f8fd2dc9f --- /dev/null +++ b/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 +#include +#include + +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 diff --git a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp index 74c8f4904..1a53332db 100644 --- a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp @@ -6,57 +6,44 @@ * */ +#include #include "ROS2GNSSSensorComponent.h" #include -#include #include -#include #include -#include -#include -#include - #include "GNSSFormatConversions.h" namespace ROS2 { - namespace Internal + namespace { - const char* kGNSSMsgType = "sensor_msgs::msg::NavSatFix"; + const char* GNSSMsgType = "sensor_msgs::msg::NavSatFix"; } void ROS2GNSSSensorComponent::Reflect(AZ::ReflectContext* context) { - if (AZ::SerializeContext* serialize = azrtti_cast(context)) + GNSSSensorConfiguration::Reflect(context); + + if (auto* serialize = azrtti_cast(context)) { - serialize->Class() - ->Version(1) - ->Field("gnssOriginLatitude", &ROS2GNSSSensorComponent::m_gnssOriginLatitudeDeg) - ->Field("gnssOriginLongitude", &ROS2GNSSSensorComponent::m_gnssOriginLongitudeDeg) - ->Field("gnssOriginAltitude", &ROS2GNSSSensorComponent::m_gnssOriginAltitude); + serialize->Class()->Version(3)->Field( + "gnssSensorConfiguration", &ROS2GNSSSensorComponent::m_gnssConfiguration); - if (AZ::EditContext* ec = serialize->GetEditContext()) + if (auto* editContext = serialize->GetEditContext()) { - ec->Class("ROS2 GNSS Sensor", "GNSS sensor component") + editContext->Class("ROS2 GNSS Sensor", "GNSS sensor component") ->ClassElement(AZ::Edit::ClassElements::EditorData, "") ->Attribute(AZ::Edit::Attributes::Category, "ROS2") ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/ROS2GNSSSensor.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/ROS2GNSSSensor.svg") ->DataElement( AZ::Edit::UIHandlers::Default, - &ROS2GNSSSensorComponent::m_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); } } } @@ -64,36 +51,54 @@ namespace ROS2 ROS2GNSSSensorComponent::ROS2GNSSSensorComponent() { TopicConfiguration pc; - pc.m_type = Internal::kGNSSMsgType; + pc.m_type = GNSSMsgType; pc.m_topic = "gnss"; m_sensorConfiguration.m_frequency = 10; - m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(Internal::kGNSSMsgType, pc)); + m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(GNSSMsgType, pc)); + } + + ROS2GNSSSensorComponent::ROS2GNSSSensorComponent( + const SensorConfiguration& sensorConfiguration, const GNSSSensorConfiguration& gnssConfiguration) + : m_gnssConfiguration(gnssConfiguration) + { + m_sensorConfiguration = sensorConfiguration; } void ROS2GNSSSensorComponent::Activate() { - ROS2SensorComponent::Activate(); auto ros2Node = ROS2Interface::Get()->GetNode(); AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for GNSS sensor"); - const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kGNSSMsgType]; + const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[GNSSMsgType]; const auto fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic); m_gnssPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig.GetQoS()); m_gnssMsg.header.frame_id = "gnss_frame_id"; + + StartSensor( + m_sensorConfiguration.m_frequency, + [this]([[maybe_unused]] auto&&... args) + { + if (!m_sensorConfiguration.m_publishingEnabled) + { + return; + } + FrequencyTick(); + }); } void ROS2GNSSSensorComponent::Deactivate() { - ROS2SensorComponent::Deactivate(); + StopSensor(); m_gnssPublisher.reset(); } 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(); diff --git a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h index ada69e45d..c706b8c51 100644 --- a/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h +++ b/Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h @@ -9,21 +9,25 @@ #include #include -#include +#include +#include #include #include +#include "GNSSSensorConfiguration.h" + namespace ROS2 { //! Global Navigation Satellite Systems (GNSS) sensor component class //! It provides NavSatFix data of sensor's position in GNSS frame which is defined by GNSS origin offset //! Offset is provided as latitude [deg], longitude [deg], altitude [m] of o3de global frame //! It is assumed that o3de global frame overlaps with ENU coordinate system - class ROS2GNSSSensorComponent : public ROS2SensorComponent + class ROS2GNSSSensorComponent : public ROS2SensorComponentBase { public: - AZ_COMPONENT(ROS2GNSSSensorComponent, "{55B4A299-7FA3-496A-88F0-764C75B0E9A7}", ROS2SensorComponent); + AZ_COMPONENT(ROS2GNSSSensorComponent, "{55B4A299-7FA3-496A-88F0-764C75B0E9A7}", SensorBaseType); ROS2GNSSSensorComponent(); + ROS2GNSSSensorComponent(const SensorConfiguration& sensorConfiguration, const GNSSSensorConfiguration& gnssConfiguration); ~ROS2GNSSSensorComponent() = default; static void Reflect(AZ::ReflectContext* context); ////////////////////////////////////////////////////////////////////////// @@ -33,17 +37,14 @@ namespace ROS2 ////////////////////////////////////////////////////////////////////////// private: - float m_gnssOriginLatitudeDeg = 0.0f; - float m_gnssOriginLongitudeDeg = 0.0f; - float m_gnssOriginAltitude = 0.0f; - - ////////////////////////////////////////////////////////////////////////// - // ROS2SensorComponent overrides - void FrequencyTick() override; - ////////////////////////////////////////////////////////////////////////// + ///! Requests gnss message publication. + void FrequencyTick(); - AZ::Transform GetCurrentPose() const; + //! Returns current entity position. + //! @return Current entity position. + [[nodiscard]] AZ::Transform GetCurrentPose() const; + GNSSSensorConfiguration m_gnssConfiguration; std::shared_ptr> m_gnssPublisher; sensor_msgs::msg::NavSatFix m_gnssMsg; }; diff --git a/Gems/ROS2/Code/Source/Gripper/FingerGripperComponent.cpp b/Gems/ROS2/Code/Source/Gripper/FingerGripperComponent.cpp new file mode 100644 index 000000000..b774d3381 --- /dev/null +++ b/Gems/ROS2/Code/Source/Gripper/FingerGripperComponent.cpp @@ -0,0 +1,283 @@ +/* + * 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 +#include + +#include +#include +#include +#include +#include + +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(context)) + { + serialize->Class() + ->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", "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") + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/FingerGripperComponent.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/FingerGripperComponent.svg") + ->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 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 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 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 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::infinity(); + } + + m_grippingInProgress = true; + m_desiredPosition = position; + m_stallingFor = 0.0f; + m_cancelled = false; + + SetPosition(position, maxEffort); + + return AZ::Success(); + } + + AZ::Outcome FingerGripperComponent::CancelGripperCommand() + { + m_grippingInProgress = false; + m_cancelled = true; + SetPosition(0.0f, AZStd::numeric_limits::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 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 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 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::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::infinity()); + } + + if (IsGripperVelocity0()) + { + m_stallingFor += delta; + } + else + { + m_stallingFor = 0.0f; + } + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Gripper/FingerGripperComponent.h b/Gems/ROS2/Code/Source/Gripper/FingerGripperComponent.h new file mode 100644 index 000000000..34eff96e5 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include + +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 GripperCommand(float position, float maxEffort) override; + AZ::Outcome 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 diff --git a/Gems/ROS2/Code/Source/Gripper/GripperActionServer.cpp b/Gems/ROS2/Code/Source/Gripper/GripperActionServer.cpp new file mode 100644 index 000000000..12b592b5d --- /dev/null +++ b/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 +#include +#include +#include +#include + +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( + 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 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 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 goal_handle) + { + AZ::Outcome 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 goal_handle) + { + AZ_Trace("GripperActionServer", "Goal accepted!\n"); + m_goalHandle = goal_handle; + } + + void GripperActionServer::CancelGoal(std::shared_ptr 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 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 feedback) + { + AZ_Assert(m_goalHandle, "Invalid goal handle!"); + if (m_goalHandle && m_goalHandle->is_executing()) + { + m_goalHandle->publish_feedback(feedback); + } + } + +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Gripper/GripperActionServer.h b/Gems/ROS2/Code/Source/Gripper/GripperActionServer.h new file mode 100644 index 000000000..f336430a0 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +namespace ROS2 +{ + //! GripperActionServer is a class responsible for managing an action server controlling a gripper + //! @see ROS 2 action + //! server documentation + //! @see GripperCommand documentation + class GripperActionServer + { + public: + using GripperCommand = control_msgs::action::GripperCommand; + using GoalHandleGripperCommand = rclcpp_action::ServerGoalHandle; + + //! 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 result); + + //! Report goal success to the action server. + //! @param result Result which contains success code. + void GoalSuccess(std::shared_ptr result); + + //! Publish feedback during an active action. + //! @param feedback An action feedback message informing about the progress. + void PublishFeedback(std::shared_ptr 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 goal); + rclcpp_action::CancelResponse GoalCancelledCallback(const std::shared_ptr goal_handle); + void GoalAcceptedCallback(const std::shared_ptr goal_handle); + + rclcpp_action::Server::SharedPtr actionServer; + std::shared_ptr m_goalHandle; + AZ::EntityId m_entityId; //! Entity that has target gripper component + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Gripper/GripperActionServerComponent.cpp b/Gems/ROS2/Code/Source/Gripper/GripperActionServerComponent.cpp new file mode 100644 index 000000000..797e8567a --- /dev/null +++ b/Gems/ROS2/Code/Source/Gripper/GripperActionServerComponent.cpp @@ -0,0 +1,137 @@ +/* + * 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 +#include +#include +#include +#include +#include + +namespace ROS2 +{ + void GripperActionServerComponent::Activate() + { + auto* ros2Frame = Utils::GetGameOrEditorComponent(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(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(context)) + { + serialize->Class() + ->Field("ActionServerName", &GripperActionServerComponent::m_gripperActionServerName) + ->Version(1); + + if (AZ::EditContext* ec = serialize->GetEditContext()) + { + ec->Class("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") + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/GripperActionServerComponent.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/GripperActionServerComponent.svg") + ->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 GripperActionServerComponent::ProduceFeedback() const + { + auto feedback = std::make_shared(); + 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 GripperActionServerComponent::ProduceResult() const + { + auto result = std::make_shared(); + 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 diff --git a/Gems/ROS2/Code/Source/Gripper/GripperActionServerComponent.h b/Gems/ROS2/Code/Source/Gripper/GripperActionServerComponent.h new file mode 100644 index 000000000..1985a8393 --- /dev/null +++ b/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 +#include +#include +#include +#include + +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; + 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 ProduceFeedback() const; + std::shared_ptr ProduceResult() const; + AZStd::string m_gripperActionServerName{ "gripper_server" }; //! name of the GripperCommand action server + AZStd::unique_ptr m_gripperActionServer; //! action server for GripperCommand + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Gripper/VacuumGripperComponent.cpp b/Gems/ROS2/Code/Source/Gripper/VacuumGripperComponent.cpp new file mode 100644 index 000000000..513904108 --- /dev/null +++ b/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 + +#include +#include +#include +#include +#include +#include +#include + +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(context)) + { + serialize->Class() + ->Field("EffectorCollider", &VacuumGripperComponent::m_gripperEffectorCollider) + ->Field("EffectorArticulation", &VacuumGripperComponent::m_gripperEffectorArticulationLink) + ->Version(1); + + if (AZ::EditContext* ec = serialize->GetEditContext()) + { + ec->Class("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") + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/VacuumGripperComponent.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/VacuumGripperComponent.svg") + ->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) + { + AZ_Assert(AZ::Interface::Get(), "No physics system."); + + AzPhysics::SceneInterface* sceneInterface = AZ::Interface::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::Get()) + { + AZStd::pair 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(); + AZStd::vector 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; + } + AZ_Assert(m_entity->FindComponent(), "No PhysX::ArticulationLinkComponent found on entity "); + + auto* sceneInterface = AZ::Interface::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::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 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 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 diff --git a/Gems/ROS2/Code/Source/Gripper/VacuumGripperComponent.h b/Gems/ROS2/Code/Source/Gripper/VacuumGripperComponent.h new file mode 100644 index 000000000..341ce5dcc --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include + +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 GripperCommand(float position, float maxEffort) override; + AZ::Outcome 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 diff --git a/Gems/ROS2/Code/Source/Imu/ImuSensorConfiguration.cpp b/Gems/ROS2/Code/Source/Imu/ImuSensorConfiguration.cpp new file mode 100644 index 000000000..df6cce9b9 --- /dev/null +++ b/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 +#include + +namespace ROS2 +{ + void ImuSensorConfiguration::Reflect(AZ::ReflectContext* context) + { + if (AZ::SerializeContext* serialize = azrtti_cast(context)) + { + serialize->Class() + ->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("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 diff --git a/Gems/ROS2/Code/Source/Imu/ImuSensorConfiguration.h b/Gems/ROS2/Code/Source/Imu/ImuSensorConfiguration.h new file mode 100644 index 000000000..6051e3aed --- /dev/null +++ b/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 +#include +#include +#include + +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 diff --git a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp index 2146572d7..9dfadf1a1 100644 --- a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp @@ -31,35 +31,27 @@ namespace ROS2 void ROS2ImuSensorComponent::Reflect(AZ::ReflectContext* context) { - if (AZ::SerializeContext* serialize = azrtti_cast(context)) + ImuSensorConfiguration::Reflect(context); + + if (auto* serializeContext = azrtti_cast(context)) { - serialize->Class() - ->Version(1) - ->Field("FilterSize", &ROS2ImuSensorComponent::m_filterSize) - ->Field("IncludeGravity", &ROS2ImuSensorComponent::m_includeGravity) - ->Field("AbsoluteRotation", &ROS2ImuSensorComponent::m_absoluteRotation); + serializeContext->Class()->Version(3)->Field( + "imuSensorConfiguration", &ROS2ImuSensorComponent::m_imuConfiguration); - if (AZ::EditContext* ec = serialize->GetEditContext()) + if (auto* editContext = serializeContext->GetEditContext()) { - ec->Class("ROS2 Imu Sensor", "Imu sensor component") + editContext->Class("ROS2 Imu Sensor", "Imu sensor component") ->ClassElement(AZ::Edit::ClassElements::EditorData, "") ->Attribute(AZ::Edit::Attributes::Category, "ROS2") ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/ROS2ImuSensor.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/ROS2ImuSensor.svg") ->DataElement( AZ::Edit::UIHandlers::Default, - &ROS2ImuSensorComponent::m_filterSize, - "Filter Length", - "Filter Length, large value allows to reduce numeric noise") - ->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."); + &ROS2ImuSensorComponent::m_imuConfiguration, + "Imu sensor configuration", + "Imu sensor configuration") + ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly); } } } @@ -74,78 +66,137 @@ 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() + void ROS2ImuSensorComponent::Activate() { - InstallPhysicalCallback(m_entity->GetId()); + auto ros2Node = ROS2Interface::Get()->GetNode(); + AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for IMU sensor"); + m_imuMsg.header.frame_id = GetFrameID().c_str(); + const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kImuMsgType]; + const auto fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic); + m_imuPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig.GetQoS()); + + m_linearAccelerationCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_linearAccelerationVariance); + m_angularVelocityCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_angularVelocityVariance); + m_orientationCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_orientationVariance); + + StartSensor( + m_sensorConfiguration.m_frequency, + [this](float imuDeltaTime, AzPhysics::SceneHandle sceneHandle, float physicsDeltaTime) + { + if (!m_sensorConfiguration.m_publishingEnabled) + { + return; + } + OnImuEvent(imuDeltaTime, sceneHandle, physicsDeltaTime); + }, + [this]([[maybe_unused]] AzPhysics::SceneHandle sceneHandle, float physicsDeltaTime) + { + OnPhysicsEvent(sceneHandle); + }); } - void ROS2ImuSensorComponent::OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime) + void ROS2ImuSensorComponent::Deactivate() { + StopSensor(); + m_bodyHandle = AzPhysics::InvalidSimulatedBodyHandle; + m_imuPublisher.reset(); + } + + void ROS2ImuSensorComponent::OnPhysicsEvent(AzPhysics::SceneHandle sceneHandle) + { + if (m_bodyHandle == AzPhysics::InvalidSimulatedBodyHandle) + { + AzPhysics::RigidBody* rigidBody = nullptr; + AZ::EntityId entityId = GetEntityId(); + Physics::RigidBodyRequestBus::EventResult(rigidBody, entityId, &Physics::RigidBodyRequests::GetRigidBody); + + if (!rigidBody) + { + AZ_Error("ROS2ImuSensorComponent", false, "Entity %s does not have a rigid body - stopping Imu sensor.", entityId.ToString().c_str()); + StopSensor(); + return; + } + + m_bodyHandle = rigidBody->m_bodyHandle; + } + auto* sceneInterface = AZ::Interface::Get(); - const auto gravity = sceneInterface->GetGravity(sceneHandle); auto* body = sceneInterface->GetSimulatedBodyFromHandle(sceneHandle, m_bodyHandle); - auto rigidbody = azrtti_cast(body); + AZ_Assert(body, "Requested simulated body does not exist"); + auto* rigidbody = azrtti_cast(body); AZ_Assert(rigidbody, "Requested simulated body is not a rigid body"); + auto inv = rigidbody->GetTransform().GetInverse(); const auto linearVelocity = inv.TransformVector(rigidbody->GetLinearVelocity()); 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(); } - if (IsPublicationDeadline(deltaTime)) - { - const AZ::Vector3 linearVelocityFilter = - AZStd::accumulate(m_filterAcceleration.begin(), m_filterAcceleration.end(), AZ::Vector3{ 0 }) / m_filterAcceleration.size(); + } - const AZ::Vector3 angularRateFiltered = - AZStd::accumulate(m_filterAngularVelocity.begin(), m_filterAngularVelocity.end(), AZ::Vector3{ 0 }) / - m_filterAngularVelocity.size(); + void ROS2ImuSensorComponent::OnImuEvent([[maybe_unused]] float imuDeltaTime, AzPhysics::SceneHandle sceneHandle, float physicsDeltaTime) + { + const AZ::Vector3 linearVelocityFilter = + AZStd::accumulate(m_filterAcceleration.begin(), m_filterAcceleration.end(), AZ::Vector3{ 0 }) / m_filterAcceleration.size(); - auto acc = (linearVelocityFilter - m_previousLinearVelocity) / deltaTime; + const AZ::Vector3 angularRateFiltered = + AZStd::accumulate(m_filterAngularVelocity.begin(), m_filterAngularVelocity.end(), AZ::Vector3{ 0 }) / + m_filterAngularVelocity.size(); - m_previousLinearVelocity = linearVelocityFilter; - m_acceleration = -acc + angularRateFiltered.Cross(linearVelocityFilter); - if (m_includeGravity) - { - m_acceleration += inv.TransformVector(gravity); - } - m_imuMsg.linear_acceleration = ROS2Conversions::ToROS2Vector3(m_acceleration); - m_imuMsg.angular_velocity = ROS2Conversions::ToROS2Vector3(angularRateFiltered); - if (m_absoluteRotation) - { - m_imuMsg.orientation = ROS2Conversions::ToROS2Quaternion(rigidbody->GetTransform().GetRotation()); - } - m_imuMsg.header.stamp = ROS2Interface::Get()->GetROSTimestamp(); - this->m_imuPublisher->publish(m_imuMsg); + // Physics delta time is used here intentionally - linear velocities are accumulated with that delta (see OnPhysicsEvent method). + auto acc = (linearVelocityFilter - m_previousLinearVelocity) / imuDeltaTime; + + auto* sceneInterface = AZ::Interface::Get(); + auto* body = sceneInterface->GetSimulatedBodyFromHandle(sceneHandle, m_bodyHandle); + auto rigidbody = azrtti_cast(body); + auto inv = rigidbody->GetTransform().GetInverse(); + + m_previousLinearVelocity = linearVelocityFilter; + m_acceleration = acc - angularRateFiltered.Cross(linearVelocityFilter); + + if (m_imuConfiguration.m_includeGravity) + { + const auto gravity = sceneInterface->GetGravity(sceneHandle); + m_acceleration -= inv.TransformVector(gravity); } - }; + m_imuMsg.linear_acceleration = ROS2Conversions::ToROS2Vector3(m_acceleration); + 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); - void ROS2ImuSensorComponent::Activate() - { - auto ros2Node = ROS2Interface::Get()->GetNode(); - AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for IMU sensor"); - m_imuMsg.header.frame_id = GetFrameID().c_str(); - const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kImuMsgType]; - const auto fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic); - m_imuPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig.GetQoS()); - ROS2SensorComponent::Activate(); + if (m_imuConfiguration.m_absoluteRotation) + { + m_imuMsg.orientation = ROS2Conversions::ToROS2Quaternion(rigidbody->GetTransform().GetRotation()); + m_imuMsg.orientation_covariance = ROS2Conversions::ToROS2Covariance(m_orientationCovariance); + } + m_imuMsg.header.stamp = ROS2Interface::Get()->GetROSTimestamp(); + this->m_imuPublisher->publish(m_imuMsg); } - void ROS2ImuSensorComponent::Deactivate() + AZ::Matrix3x3 ROS2ImuSensorComponent::ToDiagonalCovarianceMatrix(const AZ::Vector3& variance) { - RemovePhysicalCallback(); - m_bodyHandle = AzPhysics::InvalidSimulatedBodyHandle; - m_imuPublisher.reset(); + AZ::Matrix3x3 covarianceMatrix; + covarianceMatrix.SetElement(0, 0, variance.GetX()); + covarianceMatrix.SetElement(1, 1, variance.GetY()); + covarianceMatrix.SetElement(2, 2, variance.GetZ()); + return covarianceMatrix; } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h index be921d4d3..9b7e66890 100644 --- a/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h +++ b/Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h @@ -5,29 +5,32 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ + #pragma once +#include #include #include #include #include -#include -#include +#include +#include #include #include +#include "ImuSensorConfiguration.h" + namespace ROS2 { //! An IMU (Inertial Measurement Unit) sensor Component. //! IMUs typically include gyroscopes, accelerometers and magnetometers. This component encapsulates data //! acquisition and its publishing to ROS2 ecosystem. IMU Component requires ROS2FrameComponent. - class ROS2ImuSensorComponent - : public ROS2SensorComponent - , public ROS2::Utils::PhysicsCallbackHandler + class ROS2ImuSensorComponent : public ROS2SensorComponentBase { public: - AZ_COMPONENT(ROS2ImuSensorComponent, "{502A955E-7742-4E23-AD77-5E4063739DCA}", ROS2SensorComponent); + AZ_COMPONENT(ROS2ImuSensorComponent, "{502A955E-7742-4E23-AD77-5E4063739DCA}", SensorBaseType); ROS2ImuSensorComponent(); + ROS2ImuSensorComponent(const SensorConfiguration& sensorConfiguration, const ImuSensorConfiguration& imuConfiguration); ~ROS2ImuSensorComponent() = default; static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); static void Reflect(AZ::ReflectContext* context); @@ -38,15 +41,6 @@ namespace ROS2 ////////////////////////////////////////////////////////////////////////// private: - //! Length of filter that removes numerical noise - int m_filterSize{ 10 }; - - //! Include gravity acceleration - bool m_includeGravity{ true }; - - //! Measure also absolute rotation - bool m_absoluteRotation{ true }; - std::shared_ptr> m_imuPublisher; sensor_msgs::msg::Imu m_imuMsg; AZ::Vector3 m_previousLinearVelocity = AZ::Vector3::CreateZero(); @@ -55,11 +49,20 @@ namespace ROS2 AZStd::deque m_filterAcceleration; AZStd::deque m_filterAngularVelocity; + ImuSensorConfiguration m_imuConfiguration; + + AZ::Matrix3x3 m_orientationCovariance = AZ::Matrix3x3::CreateZero(); + AZ::Matrix3x3 m_angularVelocityCovariance = AZ::Matrix3x3::CreateZero(); + AZ::Matrix3x3 m_linearAccelerationCovariance = AZ::Matrix3x3::CreateZero(); + private: - // ROS2SensorComponent overrides ... - void SetupRefreshLoop() override; + void OnPhysicsEvent(AzPhysics::SceneHandle sceneHandle); + + void OnImuEvent(float imuDeltaTime, AzPhysics::SceneHandle sceneHandle, float physicsDeltaTime); + + AZ::Matrix3x3 ToDiagonalCovarianceMatrix(const AZ::Vector3& variance); - // ROS2::Utils::PhysicsCallbackHandler overrides ... - void OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime) override; + // Handle to the simulated physical body + AzPhysics::SimulatedBodyHandle m_bodyHandle = AzPhysics::InvalidSimulatedBodyHandle; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp b/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp new file mode 100644 index 000000000..3b0828c60 --- /dev/null +++ b/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp @@ -0,0 +1,179 @@ +/* + * 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 "LidarCore.h" +#include +#include +#include +#include +#include +#include +#include + +namespace ROS2 +{ + + void LidarCore::Reflect(AZ::ReflectContext* context) + { + LidarSensorConfiguration::Reflect(context); + + if (auto serialize = azrtti_cast(context)) + { + serialize->Class()->Version(1)->Field("lidarConfiguration", &LidarCore::m_lidarConfiguration); + + if (AZ::EditContext* ec = serialize->GetEditContext()) + { + ec->Class("ROS2 Lidar Sensor", "Lidar sensor component") + ->DataElement( + AZ::Edit::UIHandlers::ComboBox, &LidarCore::m_lidarConfiguration, "Lidar configuration", "Lidar configuration") + ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly); + } + } + } + + void LidarCore::ConnectToLidarRaycaster() + { + if (auto raycasterId = m_implementationToRaycasterMap.find(m_lidarConfiguration.m_lidarSystem); + raycasterId != m_implementationToRaycasterMap.end()) + { + m_lidarRaycasterId = raycasterId->second; + return; + } + + m_lidarRaycasterId = LidarId::CreateNull(); + LidarSystemRequestBus::EventResult( + m_lidarRaycasterId, AZ_CRC(m_lidarConfiguration.m_lidarSystem), &LidarSystemRequestBus::Events::CreateLidar, m_entityId); + AZ_Assert(!m_lidarRaycasterId.IsNull(), "Could not access selected Lidar System."); + + m_implementationToRaycasterMap.emplace(m_lidarConfiguration.m_lidarSystem, m_lidarRaycasterId); + } + + void LidarCore::ConfigureLidarRaycaster() + { + LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayOrientations, m_lastRotations); + LidarRaycasterRequestBus::Event( + m_lidarRaycasterId, + &LidarRaycasterRequestBus::Events::ConfigureMinimumRayRange, + m_lidarConfiguration.m_lidarParameters.m_minRange); + LidarRaycasterRequestBus::Event( + m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayRange, m_lidarConfiguration.m_lidarParameters.m_maxRange); + + 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 | RaycastResultFlags::Points; + + LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRaycastResultFlags, requestedFlags); + + if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers) + { + LidarRaycasterRequestBus::Event( + m_lidarRaycasterId, + &LidarRaycasterRequestBus::Events::ConfigureIgnoredCollisionLayers, + m_lidarConfiguration.m_ignoredCollisionLayers); + } + + if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion) + { + LidarRaycasterRequestBus::Event( + m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ExcludeEntities, m_lidarConfiguration.m_excludedEntities); + } + + if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints) + { + LidarRaycasterRequestBus::Event( + m_lidarRaycasterId, + &LidarRaycasterRequestBus::Events::ConfigureMaxRangePointAddition, + m_lidarConfiguration.m_addPointsAtMax); + } + } + + LidarCore::LidarCore(const AZStd::vector& availableModels) + : m_lidarConfiguration(availableModels) + { + } + + LidarCore::LidarCore(const LidarSensorConfiguration& lidarConfiguration) + : m_lidarConfiguration(lidarConfiguration) + { + } + + void LidarCore::VisualizeResults() const + { + if (m_lastScanResults.m_points.empty()) + { + return; + } + + if (m_drawQueue) + { + const uint8_t pixelSize = 2; + AZ::RPI::AuxGeomDraw::AuxGeomDynamicDrawArguments drawArgs; + drawArgs.m_verts = m_lastScanResults.m_points.data(); + drawArgs.m_vertCount = m_lastScanResults.m_points.size(); + drawArgs.m_colors = &AZ::Colors::Red; + drawArgs.m_colorCount = 1; + drawArgs.m_opacityType = AZ::RPI::AuxGeomDraw::OpacityType::Opaque; + drawArgs.m_size = pixelSize; + m_drawQueue->DrawPoints(drawArgs); + } + } + + void LidarCore::Init(AZ::EntityId entityId) + { + m_entityId = entityId; + + auto* entityScene = AZ::RPI::Scene::GetSceneForEntityId(m_entityId); + m_drawQueue = AZ::RPI::AuxGeomFeatureProcessorInterface::GetDrawQueueForScene(entityScene); + + m_lastRotations = LidarTemplateUtils::PopulateRayRotations(m_lidarConfiguration.m_lidarParameters); + + m_lidarConfiguration.FetchLidarImplementationFeatures(); + ConnectToLidarRaycaster(); + ConfigureLidarRaycaster(); + } + + void LidarCore::Deinit() + { + for (auto& [implementation, raycasterId] : m_implementationToRaycasterMap) + { + LidarSystemRequestBus::Event(AZ_CRC(implementation), &LidarSystemRequestBus::Events::DestroyLidar, raycasterId); + } + + m_implementationToRaycasterMap.clear(); + } + + LidarId LidarCore::GetLidarRaycasterId() const + { + return m_lidarRaycasterId; + } + + RaycastResult LidarCore::PerformRaycast() + { + AZ::Entity* entity = nullptr; + AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, m_entityId); + const auto entityTransform = entity->FindComponent(); + + LidarRaycasterRequestBus::EventResult( + m_lastScanResults, m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::PerformRaycast, entityTransform->GetWorldTM()); + if (m_lastScanResults.m_points.empty()) + { + AZ_TracePrintf("Lidar Sensor Component", "No results from raycast\n"); + return RaycastResult(); + } + return m_lastScanResults; + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarCore.h b/Gems/ROS2/Code/Source/Lidar/LidarCore.h new file mode 100644 index 000000000..5b78f41f3 --- /dev/null +++ b/Gems/ROS2/Code/Source/Lidar/LidarCore.h @@ -0,0 +1,66 @@ +/* + * 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 +#include +#include +#include +#include + +#include "LidarRaycaster.h" +#include "LidarSensorConfiguration.h" + +namespace ROS2 +{ + //! A class for executing lidar operations, such as data acquisition and visualization. + class LidarCore + { + public: + AZ_TYPE_INFO(LidarCore, "{e46126a2-7a86-bb65-367a-416f2cab393c}"); + static void Reflect(AZ::ReflectContext* context); + + LidarCore(const AZStd::vector& availableModels = {}); + LidarCore(const LidarSensorConfiguration& lidarConfiguration); + ~LidarCore() = default; + + //! Initialize when activating the lidar. + //! @param entityId Entity from which the rays are sent. + void Init(AZ::EntityId entityId); + //! Deinitialize when deactivating the lidar. + void Deinit(); + + //! Perform a raycast. + //! @return Results of the raycast. + RaycastResult PerformRaycast(); + //! Visualize the results of the last performed raycast. + void VisualizeResults() const; + + //! Get the raycaster used by this lidar. + //! @return Used raycaster's id. + LidarId GetLidarRaycasterId() const; + + //! Configuration according to which the lidar performs its raycasts. + LidarSensorConfiguration m_lidarConfiguration; + + private: + void ConnectToLidarRaycaster(); + void ConfigureLidarRaycaster(); + + //! An unordered map of lidar implementations to their raycasters created by this LidarSensorComponent. + AZStd::unordered_map m_implementationToRaycasterMap; + LidarId m_lidarRaycasterId; + + AZ::RPI::AuxGeomDrawPtr m_drawQueue; + + AZStd::vector m_lastRotations; + RaycastResult m_lastScanResults; + + AZ::EntityId m_entityId; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.cpp b/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.cpp index e93c56161..369828ce2 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.cpp +++ b/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]( + + 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 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& layerIndices) { - m_ignoreLayer = ignoreLayer; - m_ignoredLayerIndex = layerIndex; + m_ignoredCollisionLayers = layerIndices; } void LidarRaycaster::ConfigureMaxRangePointAddition(bool addMaxRangePoints) { diff --git a/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.h b/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.h index dd2293bf1..c5cbd0f07 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarRaycaster.h +++ b/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& layerIndices) override; void ConfigureMaxRangePointAddition(bool addMaxRangePoints) override; private: @@ -50,7 +50,6 @@ namespace ROS2 bool m_addMaxRangePoints{ false }; AZStd::vector m_rayRotations{ { AZ::Vector3::CreateZero() } }; - bool m_ignoreLayer{ false }; - AZ::u32 m_ignoredLayerIndex{ 0 }; + AZStd::unordered_set m_ignoredCollisionLayers; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarRegistrarSystemComponent.cpp b/Gems/ROS2/Code/Source/Lidar/LidarRegistrarSystemComponent.cpp index ce6edc253..f476a4693 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarRegistrarSystemComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/LidarRegistrarSystemComponent.cpp @@ -113,14 +113,16 @@ namespace ROS2 } AZStd::string Details::GetDefaultLidarSystem() + { + const auto& lidarInterface = LidarRegistrarInterface::Get(); + AZ_Assert(lidarInterface, "LidarRegistrarInterface is not registered."); + const auto& lidarSystemList = lidarInterface->GetRegisteredLidarSystems(); + if (lidarSystemList.empty()) { - const auto& lidarSystemList = LidarRegistrarInterface::Get()->GetRegisteredLidarSystems(); - if (lidarSystemList.empty()) - { - AZ_Warning("ROS2LidarSensorComponent", false, "No LIDAR system for the sensor to use."); - return {}; - } - return lidarSystemList.front(); + AZ_Warning("ROS2LidarSensorComponent", false, "No LIDAR system for the sensor to use."); + return {}; } + return lidarSystemList.front(); + } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.cpp b/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.cpp new file mode 100644 index 000000000..c3a122431 --- /dev/null +++ b/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.cpp @@ -0,0 +1,168 @@ +/* + * 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 +#include + +namespace ROS2 +{ + void LidarSensorConfiguration::Reflect(AZ::ReflectContext* context) + { + if (auto serializeContext = azrtti_cast(context)) + { + serializeContext->Class() + ->Version(1) + ->Field("lidarModelName", &LidarSensorConfiguration::m_lidarModelName) + ->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("Lidar Sensor configuration", "Lidar sensor configuration") + ->DataElement(AZ::Edit::UIHandlers::ComboBox, &LidarSensorConfiguration::m_lidarModelName, "Lidar Model", "Lidar model") + ->Attribute(AZ::Edit::Attributes::ChangeNotify, &LidarSensorConfiguration::OnLidarModelSelected) + ->Attribute(AZ::Edit::Attributes::StringList, &LidarSensorConfiguration::GetAvailableModels) + ->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); + } + } + } + + LidarSensorConfiguration::LidarSensorConfiguration(AZStd::vector availableModels) + : m_availableModels(AZStd::move(availableModels)) + { + if (m_availableModels.empty()) + { + AZ_Warning("LidarSensorConfiguration", false, "Lidar configuration created with an empty models list"); + return; + } + m_lidarModel = m_availableModels.front(); + m_lidarParameters = LidarTemplateUtils::GetTemplate(m_lidarModel); + m_lidarModelName = m_lidarParameters.m_name; + } + + 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; + } + } + + AZStd::vector LidarSensorConfiguration::GetAvailableModels() const + { + AZStd::vector result; + for (const auto model : m_availableModels) + { + auto templ = LidarTemplateUtils::GetTemplate(model); + result.push_back({ templ.m_name }); + } + return result; + } + + void LidarSensorConfiguration::FetchLidarModelConfiguration() + { + for (const auto model : m_availableModels) + { + auto templ = LidarTemplateUtils::GetTemplate(model); + if (m_lidarModelName == templ.m_name) + { + m_lidarModel = model; + break; + } + } + m_lidarParameters = LidarTemplateUtils::GetTemplate(m_lidarModel); + } + + bool LidarSensorConfiguration::IsConfigurationVisible() const + { + return m_lidarModel == LidarTemplate::LidarModel::Custom3DLidar || m_lidarModel == LidarTemplate::LidarModel::Custom2DLidar; + } + + 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() + { + FetchLidarModelConfiguration(); + UpdateShowNoise(); + return AZ::Edit::PropertyRefreshLevels::EntireTree; + } + + AZ::Crc32 LidarSensorConfiguration::OnLidarImplementationSelected() + { + FetchLidarImplementationFeatures(); + UpdateShowNoise(); + return AZ::Edit::PropertyRefreshLevels::EntireTree; + } + + AZStd::vector LidarSensorConfiguration::FetchLidarSystemList() + { + FetchLidarImplementationFeatures(); + UpdateShowNoise(); + return LidarRegistrarInterface::Get()->GetRegisteredLidarSystems(); + } + + void LidarSensorConfiguration::UpdateShowNoise() + { + m_lidarParameters.m_showNoiseConfig = m_lidarSystemFeatures & LidarSystemFeatures::Noise; + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.h b/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.h new file mode 100644 index 000000000..d33a32a21 --- /dev/null +++ b/Gems/ROS2/Code/Source/Lidar/LidarSensorConfiguration.h @@ -0,0 +1,66 @@ +/* + * 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 +#include +#include +#include + +#include "LidarRegistrarSystemComponent.h" +#include "LidarTemplate.h" +#include "LidarTemplateUtils.h" + +namespace ROS2 +{ + //! A structure capturing configuration of a lidar sensor (to be used with LidarCore). + class LidarSensorConfiguration + { + public: + AZ_TYPE_INFO(LidarSensorConfiguration, "{e46e75f4-1e0e-48ca-a22f-43afc8f25101}"); + static void Reflect(AZ::ReflectContext* context); + + LidarSensorConfiguration(AZStd::vector availableModels = {}); + + //! Update the lidar system features based on the current lidar system selected. + 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 m_ignoredCollisionLayers; + AZStd::vector m_excludedEntities; + + bool m_addPointsAtMax = false; + + private: + bool IsConfigurationVisible() const; + bool IsIgnoredLayerConfigurationVisible() const; + bool IsEntityExclusionVisible() const; + bool IsMaxPointsConfigurationVisible() const; + + //! Update the lidar configuration based on the current lidar model selected. + void FetchLidarModelConfiguration(); + + AZ::Crc32 OnLidarModelSelected(); + AZ::Crc32 OnLidarImplementationSelected(); + + //! Get all models this configuration can be set to (for example all 2D lidar models). + AZStd::vector GetAvailableModels() const; + //! Get all available lidar systems. + AZStd::vector FetchLidarSystemList(); + + const AZStd::vector m_availableModels; + AZStd::string m_lidarModelName = "CustomLidar2D"; + + void UpdateShowNoise(); + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarSystem.cpp b/Gems/ROS2/Code/Source/Lidar/LidarSystem.cpp index 96b1b7184..4eb7e534b 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarSystem.cpp +++ b/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 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarSystem.h b/Gems/ROS2/Code/Source/Lidar/LidarSystem.h index 77f4a8941..aa15a3f6e 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarSystem.h +++ b/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 m_lidars; + AZStd::unordered_map m_lidars; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarTemplate.cpp b/Gems/ROS2/Code/Source/Lidar/LidarTemplate.cpp index c9cf88b64..1bcd10f28 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarTemplate.cpp +++ b/Gems/ROS2/Code/Source/Lidar/LidarTemplate.cpp @@ -49,7 +49,7 @@ namespace ROS2 bool LidarTemplate::IsLayersVisible() const { - return m_model != LidarTemplate::LidarModel::Custom2DLidar; + return !m_is2D; } void LidarTemplate::Reflect(AZ::ReflectContext* context) @@ -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()) @@ -88,25 +89,39 @@ namespace ROS2 ->Attribute(AZ::Edit::Attributes::Max, 180.0f) ->DataElement( AZ::Edit::UIHandlers::Default, &LidarTemplate::m_minVAngle, "Min vertical angle [Deg]", "Downwards reach of fov") - ->Attribute(AZ::Edit::Attributes::Min, -180.0f) - ->Attribute(AZ::Edit::Attributes::Max, 180.0f) + ->Attribute(AZ::Edit::Attributes::Min, -90.0f) + ->Attribute(AZ::Edit::Attributes::Max, 90.0f) + ->Attribute(AZ::Edit::Attributes::Visibility, &LidarTemplate::IsLayersVisible) ->DataElement( AZ::Edit::UIHandlers::Default, &LidarTemplate::m_maxVAngle, "Max vertical angle [Deg]", "Upwards reach of fov") - ->Attribute(AZ::Edit::Attributes::Min, -180.0f) - ->Attribute(AZ::Edit::Attributes::Max, 180.0f) + ->Attribute(AZ::Edit::Attributes::Min, -90.0f) + ->Attribute(AZ::Edit::Attributes::Max, 90.0f) + ->Attribute(AZ::Edit::Attributes::Visibility, &LidarTemplate::IsLayersVisible) ->DataElement(AZ::Edit::UIHandlers::Default, &LidarTemplate::m_minRange, "Min range", "Minimum beam range [m]") ->Attribute(AZ::Edit::Attributes::Min, 0.0f) ->Attribute(AZ::Edit::Attributes::Max, 1000.0f) ->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 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarTemplate.h b/Gems/ROS2/Code/Source/Lidar/LidarTemplate.h index 8f590aa34..867d7df3d 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarTemplate.h +++ b/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; }; @@ -54,6 +54,9 @@ namespace ROS2 //! Name of lidar template AZStd::string m_name; + //! Whether the template is for a 2D Lidar. + //! This causes vertical parameters of the Lidar to be unmodifiable (m_minVAngle, m_maxVAngle, m_layers). + bool m_is2D = false; //! Minimum horizontal angle (altitude of the ray), in degrees float m_minHAngle = 0.0f; //! Maximum horizontal angle (altitude of the ray), in degrees @@ -72,9 +75,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 diff --git a/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.cpp b/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.cpp index a6d7f66c8..af5823def 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.cpp +++ b/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.cpp @@ -12,16 +12,17 @@ namespace ROS2 { - LidarTemplate LidarTemplateUtils::GetTemplate(LidarTemplate::LidarModel model) + namespace { using Model = LidarTemplate::LidarModel; - static const std::unordered_map templates = { + static const AZStd::map templates = { { Model::Custom3DLidar, { /*.m_model = */ Model::Custom3DLidar, /*.m_name = */ "CustomLidar", + /*.m_is2D = */ false, /*.m_minHAngle = */ -180.0f, /*.m_maxHAngle = */ 180.0f, /*.m_minVAngle = */ -35.0f, @@ -43,6 +44,7 @@ namespace ROS2 { /*.m_model = */ Model::Ouster_OS0_64, /*.m_name = */ "Ouster OS0-64", + /*.m_is2D = */ false, /*.m_minHAngle = */ -180.0f, /*.m_maxHAngle = */ 180.0f, /*.m_minVAngle = */ -45.0f, @@ -64,6 +66,7 @@ namespace ROS2 { /*.m_model = */ Model::Ouster_OS1_64, /*.m_name = */ "Ouster OS1-64", + /*.m_is2D = */ false, /*.m_minHAngle = */ -180.0f, /*.m_maxHAngle = */ 180.0f, /*.m_minVAngle = */ 22.5f, @@ -83,8 +86,9 @@ namespace ROS2 { Model::Ouster_OS2_64, { - /*.m_model = */ Model::Ouster_OS1_64, - /*.m_name = */ "Ouster OS1-64", + /*.m_model = */ Model::Ouster_OS2_64, + /*.m_name = */ "Ouster OS2-64", + /*.m_is2D = */ false, /*.m_minHAngle = */ -180.0f, /*.m_maxHAngle = */ 180.0f, /*.m_minVAngle = */ 11.25f, @@ -106,6 +110,7 @@ namespace ROS2 { /*.m_model = */ Model::Velodyne_Puck, /*.m_name = */ "Velodyne Puck (VLP-16)", + /*.m_is2D = */ false, /*.m_minHAngle = */ -180.0f, /*.m_maxHAngle = */ 180.0f, /*.m_minVAngle = */ 15.0f, @@ -127,6 +132,7 @@ namespace ROS2 { /*.m_model = */ Model::Velodyne_HDL_32E, /*.m_name = */ "Velodyne HDL-32E", + /*.m_is2D = */ false, /*.m_minHAngle = */ -180.0f, /*.m_maxHAngle = */ 180.0f, /*.m_minVAngle = */ 10.67f, @@ -148,45 +154,50 @@ namespace ROS2 { /*.m_model = */ Model::Custom2DLidar, /*.m_name = */ "CustomLidar2D", - /*.m_minHAngle = */ -180.0f, - /*.m_maxHAngle = */ 180.0f, - /*.m_minVAngle = */ 0.f, - /*.m_maxVAngle = */ 0.f, - /*.m_layers = */ 1, - /*.m_numberOfIncrements = */ 924, - /*.m_minRange = */ 0.0f, - /*.m_maxRange = */ 100.0f, - /*.m_noiseParameters = */ - { - /*.m_angularNoiseStdDev = */ 0.0f, + /*.m_is2D = */ true, + /*.m_minHAngle = */ -180.0f, + /*.m_maxHAngle = */ 180.0f, + /*.m_minVAngle = */ 0.f, + /*.m_maxVAngle = */ 0.f, + /*.m_layers = */ 1, + /*.m_numberOfIncrements = */ 924, + /*.m_minRange = */ 0.0f, + /*.m_maxRange = */ 100.0f, + /*.m_noiseParameters = */ + { + /*.m_angularNoiseStdDev = */ 0.0f, /*.m_distanceNoiseStdDevBase = */ 0.02f, /*.m_distanceNoiseStdDevRisePerMeter = */ 0.001f, }, - } + }, }, { Model::Slamtec_RPLIDAR_S1, { /*.m_model = */ Model::Slamtec_RPLIDAR_S1, /*.m_name = */ "Slamtec RPLIDAR S1", - /*.m_minHAngle = */ -180.0f, - /*.m_maxHAngle = */ 180.0f, - /*.m_minVAngle = */ 0.f, - /*.m_maxVAngle = */ 0.f, - /*.m_layers = */ 1, - /*.m_numberOfIncrements = */ 921, - /*.m_minRange = */ 0.1f, - /*.m_maxRange = */ 40.0f, - /*.m_noiseParameters = */ - { - /*.m_angularNoiseStdDev = */ 0.0f, + /*.m_is2D = */ true, + /*.m_minHAngle = */ -180.0f, + /*.m_maxHAngle = */ 180.0f, + /*.m_minVAngle = */ 0.f, + /*.m_maxVAngle = */ 0.f, + /*.m_layers = */ 1, + /*.m_numberOfIncrements = */ 921, + /*.m_minRange = */ 0.1f, + /*.m_maxRange = */ 40.0f, + /*.m_noiseParameters = */ + { + /*.m_angularNoiseStdDev = */ 0.0f, /*.m_distanceNoiseStdDevBase = */ 0.02f, /*.m_distanceNoiseStdDevRisePerMeter = */ 0.001f, }, - } - }, + }, + } }; + } // namespace + LidarTemplate LidarTemplateUtils::GetTemplate(LidarTemplate::LidarModel model) + { auto it = templates.find(model); if (it == templates.end()) { @@ -196,6 +207,34 @@ namespace ROS2 return it->second; } + AZStd::vector LidarTemplateUtils::Get2DModels() + { + AZStd::vector result; + + for (const auto& it : templates) + { + if (it.second.m_is2D) + { + result.push_back(it.first); + } + } + return result; + } + + AZStd::vector LidarTemplateUtils::Get3DModels() + { + AZStd::vector result; + + for (const auto& it : templates) + { + if (!it.second.m_is2D) + { + result.push_back(it.first); + } + } + return result; + } + size_t LidarTemplateUtils::TotalPointCount(const LidarTemplate& t) { return t.m_layers * t.m_numberOfIncrements; diff --git a/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.h b/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.h index 54837fbff..440e35815 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.h +++ b/Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.h @@ -21,6 +21,14 @@ namespace ROS2 //! @return the matching template which describes parameters for the model. LidarTemplate GetTemplate(LidarTemplate::LidarModel model); + //! Get all 2D lidar models. + //! @return 2D lidar models. + AZStd::vector Get2DModels(); + + //! Get all 3D lidar models. + //! @return 3D lidar models. + AZStd::vector Get3DModels(); + //! Get total point count for a given template. //! @param t lidar template. //! @return total count of points that the lidar specified by the template would produce on each scan. diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp index bdb637ace..468f11eda 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp @@ -9,292 +9,122 @@ #include #include #include +#include #include #include -#include #include -#include namespace ROS2 { - namespace Internal + namespace { - const char* kLaserScanType = "sensor_msgs::msg::LaserScan"; + const char* LaserScanType = "sensor_msgs::msg::LaserScan"; } void ROS2Lidar2DSensorComponent::Reflect(AZ::ReflectContext* context) { - if (auto* serialize = azrtti_cast(context)) + if (auto* serializeContext = azrtti_cast(context)) { - serialize->Class() - ->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); + serializeContext->Class()->Version(3)->Field( + "lidarCore", &ROS2Lidar2DSensorComponent::m_lidarCore); - if (AZ::EditContext* ec = serialize->GetEditContext()) + if (auto* editContext = serializeContext->GetEditContext()) { - ec->Class("ROS2 Lidar 2D Sensor", "Lidar 2D sensor component") + editContext->Class("ROS2 Lidar 2D Sensor", "Lidar 2D sensor component") ->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) + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/ROS2Lidar2DSensor.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/ROS2Lidar2DSensor.svg") ->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_lidarCore, + "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 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()) - { - m_lidarRaycasterId = raycasterId->second; - return; - } - - m_lidarRaycasterId = LidarId::CreateNull(); - LidarSystemRequestBus::EventResult( - m_lidarRaycasterId, AZ_CRC(m_lidarSystem), &LidarSystemRequestBus::Events::CreateLidar, GetEntityId()); - AZ_Assert(!m_lidarRaycasterId.IsNull(), "Could not access selected Lidar System."); - - m_implementationToRaycasterMap.emplace(m_lidarSystem, m_lidarRaycasterId); - } - - void ROS2Lidar2DSensorComponent::ConfigureLidarRaycaster() - { - FetchLidarImplementationFeatures(); - LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayOrientations, m_lastRotations); - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayRange, m_lidarParameters.m_maxRange); - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureMinimumRayRange, m_lidarParameters.m_minRange); - - RaycastResultFlags requestedFlags = RaycastResultFlags::Ranges; - if (m_sensorConfiguration.m_visualise) - { - requestedFlags |= RaycastResultFlags::Points; - } - LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRaycastResultFlags, requestedFlags); - - if (m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers) - { - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureLayerIgnoring, m_ignoreLayer, m_ignoredLayerIndex); - } - - if (m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion) - { - LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ExcludeEntities, m_excludedEntities); - } - - if (m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints) - { - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureMaxRangePointAddition, m_addPointsAtMax); - } - } - ROS2Lidar2DSensorComponent::ROS2Lidar2DSensorComponent() + : m_lidarCore(LidarTemplateUtils::Get2DModels()) { TopicConfiguration ls; - AZStd::string type = Internal::kLaserScanType; + AZStd::string type = LaserScanType; ls.m_type = type; - ls.m_topic = "ls"; - m_sensorConfiguration.m_frequency = 10; + ls.m_topic = "scan"; + m_sensorConfiguration.m_frequency = 10.f; m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(type, ls)); } - - void ROS2Lidar2DSensorComponent::Visualise() + ROS2Lidar2DSensorComponent::ROS2Lidar2DSensorComponent( + const SensorConfiguration& sensorConfiguration, const LidarSensorConfiguration& lidarConfiguration) + : m_lidarCore(lidarConfiguration) { - if (m_visualisationPoints.empty()) - { - return; - } - - if (m_drawQueue) - { - const uint8_t pixelSize = 2; - AZ::RPI::AuxGeomDraw::AuxGeomDynamicDrawArguments drawArgs; - drawArgs.m_verts = m_visualisationPoints.data(); - drawArgs.m_vertCount = m_visualisationPoints.size(); - drawArgs.m_colors = &AZ::Colors::Red; - drawArgs.m_colorCount = 1; - drawArgs.m_opacityType = AZ::RPI::AuxGeomDraw::OpacityType::Opaque; - drawArgs.m_size = pixelSize; - m_drawQueue->DrawPoints(drawArgs); - } + m_sensorConfiguration = sensorConfiguration; } void ROS2Lidar2DSensorComponent::Activate() { + m_lidarCore.Init(GetEntityId()); + 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::kLaserScanType]; + const TopicConfiguration& publisherConfig = m_sensorConfiguration.m_publishersConfigurations[LaserScanType]; AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic); m_laserScanPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig.GetQoS()); - if (m_sensorConfiguration.m_visualise) - { - auto* entityScene = AZ::RPI::Scene::GetSceneForEntityId(GetEntityId()); - m_drawQueue = AZ::RPI::AuxGeomFeatureProcessorInterface::GetDrawQueueForScene(entityScene); - } - - m_lastRotations = LidarTemplateUtils::PopulateRayRotations(m_lidarParameters); - - FetchLidarImplementationFeatures(); - ConnectToLidarRaycaster(); - ConfigureLidarRaycaster(); - - ROS2SensorComponent::Activate(); + StartSensor( + m_sensorConfiguration.m_frequency, + [this]([[maybe_unused]] auto&&... args) + { + if (!m_sensorConfiguration.m_publishingEnabled) + { + return; + } + FrequencyTick(); + }, + [this]([[maybe_unused]] auto&&... args) + { + if (!m_sensorConfiguration.m_visualize) + { + return; + } + m_lidarCore.VisualizeResults(); + }); } void ROS2Lidar2DSensorComponent::Deactivate() { - ROS2SensorComponent::Deactivate(); + StopSensor(); m_laserScanPublisher.reset(); + m_lidarCore.Deinit(); } - void ROS2Lidar2DSensorComponent::FrequencyTick() + void ROS2Lidar2DSensorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required) { - auto entityTransform = GetEntity()->FindComponent(); - - RaycastResultFlags requestedFlags = RaycastResultFlags::Ranges; - if (m_sensorConfiguration.m_visualise) - { - requestedFlags |= RaycastResultFlags::Points; - } - LidarRaycasterRequestBus::EventResult( - m_lastScanResults, m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::PerformRaycast, entityTransform->GetWorldTM()); - if (m_lastScanResults.m_ranges.empty()) - { - AZ_TracePrintf("Lidar Sensor Component", "No results from raycast\n"); - return; - } - - if (m_sensorConfiguration.m_visualise) - { // Store points for visualisation purposes, in global frame - m_visualisationPoints = m_lastScanResults.m_points; - } + required.push_back(AZ_CRC_CE("ROS2Frame")); + } - // TODO(mzak): possibly unneccessary post-proc? - const auto inverseLidarTM = entityTransform->GetWorldTM().GetInverse(); - for (auto& point : m_lastScanResults.m_points) - { - point = inverseLidarTM.TransformPoint(point); - } + void ROS2Lidar2DSensorComponent::FrequencyTick() + { + RaycastResult lastScanResults = m_lidarCore.PerformRaycast(); auto* ros2Frame = Utils::GetGameOrEditorComponent(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(m_lidarParameters.m_numberOfIncrements); + message.angle_min = AZ::DegToRad(m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_minHAngle); + message.angle_max = AZ::DegToRad(m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_maxHAngle); + message.angle_increment = (message.angle_max - message.angle_min) / + aznumeric_cast(m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_numberOfIncrements); - message.range_min = m_lidarParameters.m_minRange; - message.range_max = m_lidarParameters.m_maxRange; + message.range_min = m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_minRange; + message.range_max = m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_maxRange; message.scan_time = 1.f / m_sensorConfiguration.m_frequency; message.time_increment = 0.0f; - message.ranges.assign(m_lastScanResults.m_ranges.begin(), m_lastScanResults.m_ranges.end()); + message.ranges.assign(lastScanResults.m_ranges.begin(), lastScanResults.m_ranges.end()); m_laserScanPublisher->publish(message); } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h index e4173db56..e06adb6b8 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h +++ b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h @@ -9,27 +9,30 @@ #include #include -#include -#include -#include #include #include -#include +#include +#include #include #include +#include "LidarCore.h" +#include "LidarRaycaster.h" + namespace ROS2 { //! Lidar 2D sensor Component. //! Lidars (Light Detection and Ranging) emit laser light and measure it after reflection. //! Lidar Component allows customization of lidar type and behavior and encapsulates both simulation //! and data publishing. It requires ROS2FrameComponent. - class ROS2Lidar2DSensorComponent : public ROS2SensorComponent + class ROS2Lidar2DSensorComponent : public ROS2SensorComponentBase { public: - AZ_COMPONENT(ROS2Lidar2DSensorComponent, "{F4C2D970-1D69-40F2-9D4D-B52DCFDD2704}", ROS2SensorComponent); + AZ_COMPONENT(ROS2Lidar2DSensorComponent, "{F4C2D970-1D69-40F2-9D4D-B52DCFDD2704}", SensorBaseType); ROS2Lidar2DSensorComponent(); + ROS2Lidar2DSensorComponent(const SensorConfiguration& sensorConfiguration, const LidarSensorConfiguration& lidarConfiguration); ~ROS2Lidar2DSensorComponent() = default; + static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); static void Reflect(AZ::ReflectContext* context); ////////////////////////////////////////////////////////////////////////// // Component overrides @@ -39,43 +42,10 @@ namespace ROS2 private: ////////////////////////////////////////////////////////////////////////// - // ROS2SensorComponent overrides - void FrequencyTick() override; - void Visualise() override; - - bool IsConfigurationVisible() const; - bool IsIgnoredLayerConfigurationVisible() const; - bool IsEntityExclusionVisible() const; - bool IsMaxPointsConfigurationVisible() const; - - AZ::Crc32 OnLidarModelSelected(); - AZ::Crc32 OnLidarImplementationSelected(); - void FetchLidarImplementationFeatures(); - AZStd::vector 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 m_lastRotations; + void FrequencyTick(); - 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 m_implementationToRaycasterMap; - LidarId m_lidarRaycasterId; std::shared_ptr> m_laserScanPublisher; - // Used only when visualisation is on - points differ since they are in global transform as opposed to local - AZStd::vector m_visualisationPoints; - AZ::RPI::AuxGeomDrawPtr m_drawQueue; - - RaycastResult m_lastScanResults; - - AZ::u32 m_ignoredLayerIndex = 0; - bool m_ignoreLayer = false; - AZStd::vector m_excludedEntities; - - bool m_addPointsAtMax = false; + LidarCore m_lidarCore; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp index 51c03f01a..208bf6dfb 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp @@ -8,278 +8,149 @@ #include #include -#include +#include #include #include -#include #include -#include namespace ROS2 { - namespace Internal + namespace { - const char* kPointCloudType = "sensor_msgs::msg::PointCloud2"; + const char* PointCloudType = "sensor_msgs::msg::PointCloud2"; } void ROS2LidarSensorComponent::Reflect(AZ::ReflectContext* context) { - if (AZ::SerializeContext* serialize = azrtti_cast(context)) + if (auto* serializeContext = azrtti_cast(context)) { - serialize->Class() - ->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); + serializeContext->Class()->Version(3)->Field( + "lidarCore", &ROS2LidarSensorComponent::m_lidarCore); - if (AZ::EditContext* ec = serialize->GetEditContext()) + if (auto* editContext = serializeContext->GetEditContext()) { - ec->Class("ROS2 Lidar Sensor", "Lidar sensor component") + editContext->Class("ROS2 Lidar Sensor", "Lidar sensor component") ->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) + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/ROS2LidarSensor.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/ROS2LidarSensor.svg") ->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_lidarCore, + "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 + void ROS2LidarSensorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required) { - return m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion; - } - - bool ROS2LidarSensorComponent::IsMaxPointsConfigurationVisible() const - { - return m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints; - } - - AZStd::vector 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()) - { - m_lidarRaycasterId = raycasterId->second; - return; - } - - m_lidarRaycasterId = LidarId::CreateNull(); - LidarSystemRequestBus::EventResult( - m_lidarRaycasterId, AZ_CRC(m_lidarSystem), &LidarSystemRequestBus::Events::CreateLidar, GetEntityId()); - AZ_Assert(!m_lidarRaycasterId.IsNull(), "Could not access selected Lidar System."); - - m_implementationToRaycasterMap.emplace(m_lidarSystem, m_lidarRaycasterId); - } - - void ROS2LidarSensorComponent::ConfigureLidarRaycaster() - { - FetchLidarImplementationFeatures(); - LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayOrientations, m_lastRotations); - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureMinimumRayRange, m_lidarParameters.m_minRange); - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayRange, m_lidarParameters.m_maxRange); - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRaycastResultFlags, RaycastResultFlags::Points); - - if (m_lidarSystemFeatures & LidarSystemFeatures::Noise) - { - 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); - } - - if (m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers) - { - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureLayerIgnoring, m_ignoreLayer, m_ignoredLayerIndex); - } - - if (m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion) - { - LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ExcludeEntities, m_excludedEntities); - } - - if (m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints) - { - LidarRaycasterRequestBus::Event( - m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureMaxRangePointAddition, m_addPointsAtMax); - } + required.push_back(AZ_CRC_CE("ROS2Frame")); } ROS2LidarSensorComponent::ROS2LidarSensorComponent() + : m_lidarCore(LidarTemplateUtils::Get3DModels()) { TopicConfiguration pc; - AZStd::string type = Internal::kPointCloudType; + AZStd::string type = PointCloudType; pc.m_type = type; pc.m_topic = "pc"; - m_sensorConfiguration.m_frequency = 10; + m_sensorConfiguration.m_frequency = 10.f; m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(type, pc)); } - void ROS2LidarSensorComponent::Visualise() + ROS2LidarSensorComponent::ROS2LidarSensorComponent( + const SensorConfiguration& sensorConfiguration, const LidarSensorConfiguration& lidarConfiguration) + : m_lidarCore(lidarConfiguration) { - if (m_visualisationPoints.empty()) - { - return; - } - - if (m_drawQueue) - { - const uint8_t pixelSize = 2; - AZ::RPI::AuxGeomDraw::AuxGeomDynamicDrawArguments drawArgs; - drawArgs.m_verts = m_visualisationPoints.data(); - drawArgs.m_vertCount = m_visualisationPoints.size(); - drawArgs.m_colors = &AZ::Colors::Red; - drawArgs.m_colorCount = 1; - drawArgs.m_opacityType = AZ::RPI::AuxGeomDraw::OpacityType::Opaque; - drawArgs.m_size = pixelSize; - m_drawQueue->DrawPoints(drawArgs); - } + m_sensorConfiguration = sensorConfiguration; } void ROS2LidarSensorComponent::Activate() { - auto ros2Node = ROS2Interface::Get()->GetNode(); - AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for lidar sensor"); + m_lidarCore.Init(GetEntityId()); - const TopicConfiguration& publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kPointCloudType]; - AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic); - m_pointCloudPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig.GetQoS()); - - if (m_sensorConfiguration.m_visualise) + m_lidarRaycasterId = m_lidarCore.GetLidarRaycasterId(); + m_canRaycasterPublish = false; + if (m_lidarCore.m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::PointcloudPublishing) { - auto* entityScene = AZ::RPI::Scene::GetSceneForEntityId(GetEntityId()); - m_drawQueue = AZ::RPI::AuxGeomFeatureProcessorInterface::GetDrawQueueForScene(entityScene); + LidarRaycasterRequestBus::EventResult( + m_canRaycasterPublish, m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::CanHandlePublishing); } - m_lastRotations = LidarTemplateUtils::PopulateRayRotations(m_lidarParameters); + if (m_canRaycasterPublish) + { + const TopicConfiguration& publisherConfig = m_sensorConfiguration.m_publishersConfigurations[PointCloudType]; + auto* ros2Frame = Utils::GetGameOrEditorComponent(GetEntity()); + + LidarRaycasterRequestBus::Event( + m_lidarRaycasterId, + &LidarRaycasterRequestBus::Events::ConfigurePointCloudPublisher, + ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic), + ros2Frame->GetFrameID().data(), + publisherConfig.GetQoS()); + } + else + { + auto ros2Node = ROS2Interface::Get()->GetNode(); + AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for lidar sensor"); - FetchLidarImplementationFeatures(); - ConnectToLidarRaycaster(); - ConfigureLidarRaycaster(); + const TopicConfiguration& publisherConfig = m_sensorConfiguration.m_publishersConfigurations[PointCloudType]; + AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic); + m_pointCloudPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig.GetQoS()); + } - ROS2SensorComponent::Activate(); + StartSensor( + m_sensorConfiguration.m_frequency, + [this]([[maybe_unused]] auto&&... args) + { + if (!m_sensorConfiguration.m_publishingEnabled) + { + return; + } + FrequencyTick(); + }, + [this]([[maybe_unused]] auto&&... args) + { + if (!m_sensorConfiguration.m_visualize) + { + return; + } + m_lidarCore.VisualizeResults(); + }); } void ROS2LidarSensorComponent::Deactivate() { - ROS2SensorComponent::Deactivate(); + StopSensor(); m_pointCloudPublisher.reset(); + m_lidarCore.Deinit(); } void ROS2LidarSensorComponent::FrequencyTick() { auto entityTransform = GetEntity()->FindComponent(); - LidarRaycasterRequestBus::EventResult( - m_lastScanResults, m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::PerformRaycast, entityTransform->GetWorldTM()); - if (m_lastScanResults.m_points.empty()) + if (m_canRaycasterPublish) { - AZ_TracePrintf("Lidar Sensor Component", "No results from raycast\n"); - return; + const builtin_interfaces::msg::Time timestamp = ROS2Interface::Get()->GetROSTimestamp(); + LidarRaycasterRequestBus::Event( + m_lidarRaycasterId, + &LidarRaycasterRequestBus::Events::UpdatePublisherTimestamp, + aznumeric_cast(timestamp.sec) * aznumeric_cast(1.0e9f) + timestamp.nanosec); } - if (m_sensorConfiguration.m_visualise) - { // Store points for visualisation purposes, in global frame - m_visualisationPoints = m_lastScanResults.m_points; + auto lastScanResults = m_lidarCore.PerformRaycast(); + + if (m_canRaycasterPublish) + { // Skip publishing when it can be handled by the raycaster. + return; } const auto inverseLidarTM = entityTransform->GetWorldTM().GetInverse(); - for (auto& point : m_lastScanResults.m_points) + for (auto& point : lastScanResults.m_points) { point = inverseLidarTM.TransformPoint(point); } @@ -289,7 +160,7 @@ namespace ROS2 message.header.frame_id = ros2Frame->GetFrameID().data(); message.header.stamp = ROS2Interface::Get()->GetROSTimestamp(); message.height = 1; - message.width = m_lastScanResults.m_points.size(); + message.width = lastScanResults.m_points.size(); message.point_step = sizeof(AZ::Vector3); message.row_step = message.width * message.point_step; @@ -304,10 +175,10 @@ namespace ROS2 message.fields.push_back(pf); } - size_t sizeInBytes = m_lastScanResults.m_points.size() * sizeof(AZ::Vector3); + size_t sizeInBytes = lastScanResults.m_points.size() * sizeof(AZ::Vector3); message.data.resize(sizeInBytes); AZ_Assert(message.row_step * message.height == sizeInBytes, "Inconsistency in the size of point cloud data"); - memcpy(message.data.data(), m_lastScanResults.m_points.data(), sizeInBytes); + memcpy(message.data.data(), lastScanResults.m_points.data(), sizeInBytes); m_pointCloudPublisher->publish(message); } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h index dcbcfb6fe..d958c45cc 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h +++ b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h @@ -9,28 +9,32 @@ #include #include -#include -#include -#include #include #include -#include +#include +#include #include #include +#include "LidarCore.h" +#include "LidarRaycaster.h" +#include "LidarSensorConfiguration.h" + namespace ROS2 { //! Lidar sensor Component. //! Lidars (Light Detection and Ranging) emit laser light and measure it after reflection. //! Lidar Component allows customization of lidar type and behavior and encapsulates both simulation //! and data publishing. It requires ROS2FrameComponent. - class ROS2LidarSensorComponent : public ROS2SensorComponent + class ROS2LidarSensorComponent : public ROS2SensorComponentBase { public: - AZ_COMPONENT(ROS2LidarSensorComponent, "{502A955F-7742-4E23-AD77-5E4063739DCA}", ROS2SensorComponent); + AZ_COMPONENT(ROS2LidarSensorComponent, "{502A955F-7742-4E23-AD77-5E4063739DCA}", SensorBaseType); ROS2LidarSensorComponent(); + ROS2LidarSensorComponent(const SensorConfiguration& sensorConfiguration, const LidarSensorConfiguration& lidarConfiguration); ~ROS2LidarSensorComponent() = default; static void Reflect(AZ::ReflectContext* context); + static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); ////////////////////////////////////////////////////////////////////////// // Component overrides void Activate() override; @@ -39,43 +43,13 @@ namespace ROS2 private: ////////////////////////////////////////////////////////////////////////// - // ROS2SensorComponent overrides - void FrequencyTick() override; - void Visualise() override; - - bool IsConfigurationVisible() const; - bool IsIgnoredLayerConfigurationVisible() const; - bool IsEntityExclusionVisible() const; - bool IsMaxPointsConfigurationVisible() const; - - AZ::Crc32 OnLidarModelSelected(); - AZ::Crc32 OnLidarImplementationSelected(); - void FetchLidarImplementationFeatures(); - AZStd::vector 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 m_lastRotations; + void FrequencyTick(); - 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 m_implementationToRaycasterMap; - LidarId m_lidarRaycasterId; + bool m_canRaycasterPublish = false; std::shared_ptr> m_pointCloudPublisher; - // Used only when visualisation is on - points differ since they are in global transform as opposed to local - AZStd::vector m_visualisationPoints; - AZ::RPI::AuxGeomDrawPtr m_drawQueue; - - RaycastResult m_lastScanResults; + LidarCore m_lidarCore; - AZ::u32 m_ignoredLayerIndex = 0; - bool m_ignoreLayer = false; - AZStd::vector m_excludedEntities; - - bool m_addPointsAtMax = false; + LidarId m_lidarRaycasterId; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsArticulationControllerComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsArticulationControllerComponent.cpp new file mode 100644 index 000000000..e8ba2666e --- /dev/null +++ b/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsArticulationControllerComponent.cpp @@ -0,0 +1,76 @@ +/* + * 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 "JointsArticulationControllerComponent.h" +#include +#include + +namespace ROS2 +{ + void JointsArticulationControllerComponent::Activate() + { + JointsPositionControllerRequestBus::Handler::BusConnect(GetEntityId()); + } + + void JointsArticulationControllerComponent::Deactivate() + { + JointsPositionControllerRequestBus::Handler::BusDisconnect(); + } + + AZ::Outcome JointsArticulationControllerComponent::PositionControl( + const AZStd::string& jointName, + JointInfo joint, + [[maybe_unused]] JointPosition currentPosition, + JointPosition targetPosition, + [[maybe_unused]] float deltaTime) + { + if (!joint.m_isArticulation) + { + return AZ::Failure(AZStd::string::format( + "Joint %s is not an articulation link, use JointsPIDControllerComponent instead", jointName.c_str())); + } + + PhysX::ArticulationJointRequestBus::Event( + joint.m_entityComponentIdPair.GetEntityId(), &PhysX::ArticulationJointRequests::SetDriveTarget, joint.m_axis, targetPosition); + 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")); + } + + void JointsArticulationControllerComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) + { + incompatible.push_back(AZ_CRC_CE("JointsControllerService")); + } + + void JointsArticulationControllerComponent::Reflect(AZ::ReflectContext* context) + { + if (AZ::SerializeContext* serialize = azrtti_cast(context)) + { + serialize->Class()->Version(0); + + if (AZ::EditContext* ec = serialize->GetEditContext()) + { + ec->Class( + "JointsArticulationControllerComponent", "Component to move articulation joints") + ->ClassElement(AZ::Edit::ClassElements::EditorData, "") + ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game")) + ->Attribute(AZ::Edit::Attributes::Category, "ROS2") + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/JointsArticulationControllerComponent.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/JointsArticulationControllerComponent.svg"); + } + } + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsArticulationControllerComponent.h b/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsArticulationControllerComponent.h new file mode 100644 index 000000000..6a69cb36b --- /dev/null +++ b/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsArticulationControllerComponent.h @@ -0,0 +1,57 @@ +/* + * 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 +#include + +namespace ROS2 +{ + //! Handles position control commands for joints using Articulations. + class JointsArticulationControllerComponent + : public AZ::Component + , public JointsPositionControllerRequestBus::Handler + { + public: + JointsArticulationControllerComponent() = default; + ~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); + + // JointsPositionControllerRequestBus::Handler overrides ... + //! @see ROS2::JointsPositionControllerRequestBus::SupportsArticulation + bool SupportsArticulation() override + { + return true; + } + + //! @see ROS2::JointsPositionControllerRequestBus::SupportsClassicJoints + bool SupportsClassicJoints() override + { + return false; + } + + //! @see ROS2::JointsPositionControllerRequestBus::PositionControl + AZ::Outcome PositionControl( + const AZStd::string& jointName, + JointInfo joint, + JointPosition currentPosition, + JointPosition targetPosition, + float deltaTime) override; + + private: + // Component overrides ... + void Activate() override; + void Deactivate() override; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsPIDControllerComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsPIDControllerComponent.cpp new file mode 100644 index 000000000..57a106134 --- /dev/null +++ b/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsPIDControllerComponent.cpp @@ -0,0 +1,98 @@ +/* + * 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 "JointsPIDControllerComponent.h" +#include +#include + +namespace ROS2 +{ + void JointsPIDControllerComponent::Activate() + { + JointsPositionControllerRequestBus::Handler::BusConnect(GetEntityId()); + InitializePIDs(); + } + + void JointsPIDControllerComponent::Deactivate() + { + JointsPositionControllerRequestBus::Handler::BusDisconnect(); + } + + void JointsPIDControllerComponent::InitializePIDs() + { + for (auto& [jointName, pid] : m_pidConfiguration) + { + pid.InitializePid(); + } + } + + AZ::Outcome JointsPIDControllerComponent::PositionControl( + const AZStd::string& jointName, + JointInfo joint, + JointPosition currentPosition, + JointPosition targetPosition, + float deltaTime) + { + if (joint.m_isArticulation) + { + return AZ::Failure(AZStd::string::format("Joint %s is articulation link, JointsPIDControllerComponent only handles classic Hinge joints. Use " + "JointsArticulationControllerComponent instead", jointName.c_str())); + } + + bool jointPIDdefined = m_pidConfiguration.find(jointName) != m_pidConfiguration.end(); + AZ_Warning( + "JointsPIDControllerComponent", + jointPIDdefined, + "PID not defined for joint %s, using a default, the behavior is likely to be wrong for this joint", + jointName.c_str()); + + Controllers::PidConfiguration defaultConfiguration; + defaultConfiguration.InitializePid(); + auto applicablePidConfiguration = jointPIDdefined ? m_pidConfiguration.at(jointName) : defaultConfiguration; + + uint64_t deltaTimeNs = deltaTime * 1'000'000'000; + auto positionError = targetPosition - currentPosition; + float desiredVelocity = applicablePidConfiguration.ComputeCommand(positionError, deltaTimeNs); + PhysX::JointRequestBus::Event(joint.m_entityComponentIdPair, &PhysX::JointRequests::SetVelocity, desiredVelocity); + return AZ::Success(); + } + + void JointsPIDControllerComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) + { + provided.push_back(AZ_CRC_CE("JointsControllerService")); + } + + void JointsPIDControllerComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) + { + incompatible.push_back(AZ_CRC_CE("JointsControllerService")); + } + + void JointsPIDControllerComponent::Reflect(AZ::ReflectContext* context) + { + if (AZ::SerializeContext* serialize = azrtti_cast(context)) + { + serialize->Class()->Version(0)->Field( + "JointsPIDs", &JointsPIDControllerComponent::m_pidConfiguration); + + if (AZ::EditContext* ec = serialize->GetEditContext()) + { + ec->Class("JointsPIDControllerComponent", "Component to move joints") + ->ClassElement(AZ::Edit::ClassElements::EditorData, "") + ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game")) + ->Attribute(AZ::Edit::Attributes::Category, "ROS2") + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/JointsPIDControllerComponent.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/JointsPIDControllerComponent.svg") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &JointsPIDControllerComponent::m_pidConfiguration, + "Joint PIDs", + "PID configuration for each free joint in this entity hierarchy"); + } + } + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsPIDControllerComponent.h b/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsPIDControllerComponent.h new file mode 100644 index 000000000..bd83905f4 --- /dev/null +++ b/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsPIDControllerComponent.h @@ -0,0 +1,60 @@ +/* + * 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 +#include +#include + +namespace ROS2 +{ + //! Handles position control commands for joints. + class JointsPIDControllerComponent + : public AZ::Component + , public JointsPositionControllerRequestBus::Handler + { + public: + JointsPIDControllerComponent() = default; + ~JointsPIDControllerComponent() = default; + AZ_COMPONENT(JointsPIDControllerComponent, "{41A31EDB-90B0-412E-BBFA-D35D45546A8E}", AZ::Component); + + static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); + static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible); + static void Reflect(AZ::ReflectContext* context); + + // JointsPositionControllerRequestBus::Handler overrides ... + //! @see ROS2::JointsPositionControllerRequestBus::SupportsArticulation + bool SupportsArticulation() override + { + return false; + } + + //! @see ROS2::JointsPositionControllerRequestBus::SupportsClassicJoints + bool SupportsClassicJoints() override + { + return true; + } + + //! @see ROS2::JointsPositionControllerRequestBus::PositionControl + AZ::Outcome PositionControl( + const AZStd::string& jointName, + JointInfo joint, + JointPosition currentPosition, + JointPosition targetPosition, + float deltaTime) override; + + private: + // Component overrides ... + void Activate() override; + void Deactivate() override; + void InitializePIDs(); + + AZStd::unordered_map m_pidConfiguration; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.cpp b/Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.cpp new file mode 100644 index 000000000..2036385e5 --- /dev/null +++ b/Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.cpp @@ -0,0 +1,154 @@ +/* + * 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 "FollowJointTrajectoryActionServer.h" +#include +#include + +namespace ROS2 +{ + FollowJointTrajectoryActionServer::FollowJointTrajectoryActionServer(const AZStd::string& actionName, const AZ::EntityId& entityId) + : m_entityId(entityId) + { + m_actionServer = rclcpp_action::create_server( + ROS2Interface::Get()->GetNode(), + actionName.c_str(), + AZStd::bind(&FollowJointTrajectoryActionServer::GoalReceivedCallback, this, AZStd::placeholders::_1, AZStd::placeholders::_2), + AZStd::bind(&FollowJointTrajectoryActionServer::GoalCancelledCallback, this, AZStd::placeholders::_1), + AZStd::bind(&FollowJointTrajectoryActionServer::GoalAcceptedCallback, this, AZStd::placeholders::_1)); + } + + JointsTrajectoryRequests::TrajectoryActionStatus FollowJointTrajectoryActionServer::GetGoalStatus() const + { + return m_goalStatus; + } + + void FollowJointTrajectoryActionServer::SetGoalSuccess() + { + m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Succeeded; + } + + void FollowJointTrajectoryActionServer::CancelGoal(std::shared_ptr result) + { + AZ_Assert(m_goalHandle, "Invalid goal handle!"); + if (m_goalHandle && m_goalHandle->is_canceling()) + { + AZ_Trace("FollowJointTrajectoryActionServer", "Cancelling goal\n"); + m_goalHandle->canceled(result); + } + } + + void FollowJointTrajectoryActionServer::GoalSuccess(std::shared_ptr result) + { + AZ_Assert(m_goalHandle, "Invalid goal handle!"); + if (m_goalHandle && m_goalHandle->is_executing()) + { + AZ_Trace("FollowJointTrajectoryActionServer", "Goal succeeded\n"); + m_goalHandle->succeed(result); + m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Succeeded; + } + } + + void FollowJointTrajectoryActionServer::PublishFeedback(std::shared_ptr feedback) + { + AZ_Assert(m_goalHandle, "Invalid goal handle!"); + if (m_goalHandle && m_goalHandle->is_executing()) + { + m_goalHandle->publish_feedback(feedback); + } + } + + bool FollowJointTrajectoryActionServer::IsGoalActiveState() const + { + return m_goalHandle->is_active() || m_goalHandle->is_executing() || m_goalHandle->is_canceling(); + } + + bool FollowJointTrajectoryActionServer::IsReadyForExecution() const + { + // Has a goal handle yet - can be accepted. + if (!m_goalHandle) + { + return true; + } + // accept goal if previous is terminal state + return IsGoalActiveState() == false; + } + + bool FollowJointTrajectoryActionServer::IsExecuting() const + { + return m_goalHandle && m_goalHandle->is_executing(); + } + + rclcpp_action::GoalResponse FollowJointTrajectoryActionServer::GoalReceivedCallback( + [[maybe_unused]] const rclcpp_action::GoalUUID& uuid, [[maybe_unused]] std::shared_ptr 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) + { // Accept each cancel attempt + AZ::Outcome cancelOutcome; + JointsTrajectoryRequestBus::EventResult(cancelOutcome, m_entityId, &JointsTrajectoryRequests::CancelTrajectoryGoal); + + if (!cancelOutcome) + { // This will not happen in simulation unless intentionally done for behavior validation + AZ_Trace("FollowJointTrajectoryActionServer", "Cancelling could not be accepted: %s\n", cancelOutcome.GetError().c_str()); + return rclcpp_action::CancelResponse::REJECT; + } + + m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Cancelled; + return rclcpp_action::CancelResponse::ACCEPT; + } + + void FollowJointTrajectoryActionServer::GoalAcceptedCallback(const std::shared_ptr 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(); + result->error_string = "Goal aborted: server is not ready for execution!"; + result->error_code = FollowJointTrajectory::Result::INVALID_GOAL; + goalHandle->abort(result); + return; + } + + AZ::Outcome 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(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; + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.h b/Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.h new file mode 100644 index 000000000..2d3eb11f5 --- /dev/null +++ b/Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.h @@ -0,0 +1,74 @@ +/* + * 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 +#include +#include +#include +#include +#include + +namespace ROS2 +{ + //! A class wrapping ROS 2 action server for joint trajectory controller. + //! @see joint trajectory + //! controller . + class FollowJointTrajectoryActionServer + { + public: + using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory; + + //! Create an action server for FollowJointTrajectory 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 JointsTrajectoryRequestBus. + //! @see ROS 2 action + //! server documentation + FollowJointTrajectoryActionServer(const AZStd::string& actionName, const AZ::EntityId& entityId); + + //! Return trajectory action status. + //! @return Status of the trajectory execution. + JointsTrajectoryRequests::TrajectoryActionStatus GetGoalStatus() const; + + //! Cancel the current goal. + //! @param result Result to be passed to through action server to the client. + void CancelGoal(std::shared_ptr 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 result); + + //! Publish feedback during an active action. + //! @param feedback An action feedback message informing about the progress. + void PublishFeedback(std::shared_ptr feedback); + + private: + using GoalHandle = rclcpp_action::ServerGoalHandle; + using TrajectoryActionStatus = JointsTrajectoryRequests::TrajectoryActionStatus; + + AZ::EntityId m_entityId; + TrajectoryActionStatus m_goalStatus = TrajectoryActionStatus::Idle; + rclcpp_action::Server::SharedPtr m_actionServer; + std::shared_ptr m_goalHandle; + + bool IsGoalActiveState() const; + bool IsReadyForExecution() const; + bool IsExecuting() const; + + rclcpp_action::GoalResponse GoalReceivedCallback( + const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); + + rclcpp_action::CancelResponse GoalCancelledCallback(const std::shared_ptr goalHandle); + + void GoalAcceptedCallback(const std::shared_ptr goalHandle); + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointInfo.cpp b/Gems/ROS2/Code/Source/Manipulation/JointInfo.cpp new file mode 100644 index 000000000..c29df69f0 --- /dev/null +++ b/Gems/ROS2/Code/Source/Manipulation/JointInfo.cpp @@ -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 + * + */ + +#include + +namespace ROS2 +{ + void JointInfo::Reflect(AZ::ReflectContext* context) + { + if (auto serializeContext = azrtti_cast(context)) + { + serializeContext->Class() + ->Version(1) + ->Field("IsArticulation", &JointInfo::m_isArticulation) + ->Field("Axis", &JointInfo::m_axis) + ->Field("EntityComponentIdPair", &JointInfo::m_entityComponentIdPair) + ->Field("RestPosition", &JointInfo::m_restPosition); + } + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointStatePublisher.cpp b/Gems/ROS2/Code/Source/Manipulation/JointStatePublisher.cpp new file mode 100644 index 000000000..4f4e88ebc --- /dev/null +++ b/Gems/ROS2/Code/Source/Manipulation/JointStatePublisher.cpp @@ -0,0 +1,86 @@ +/* + * 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 +#include +#include + +#include "JointStatePublisher.h" +#include "ManipulationUtils.h" + +namespace ROS2 +{ + JointStatePublisher::JointStatePublisher(const PublisherConfiguration& configuration, const JointStatePublisherContext& context) + : m_configuration(configuration) + , m_context(context) + { + auto topicConfiguration = m_configuration.m_topicConfiguration; + AZStd::string topic = ROS2Names::GetNamespacedName(context.m_publisherNamespace, topicConfiguration.m_topic); + auto ros2Node = ROS2Interface::Get()->GetNode(); + m_jointStatePublisher = ros2Node->create_publisher(topic.data(), topicConfiguration.GetQoS()); + } + + JointStatePublisher::~JointStatePublisher() + { + m_eventSourceAdapter.Stop(); + m_adaptedEventHandler.Disconnect(); + } + + void JointStatePublisher::PublishMessage() + { + std_msgs::msg::Header rosHeader; + rosHeader.frame_id = ROS2Names::GetNamespacedName(m_context.m_publisherNamespace, m_context.m_frameId).data(); + 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() + { + 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()); + + m_eventSourceAdapter.SetFrequency(m_configuration.m_frequency); + m_adaptedEventHandler = decltype(m_adaptedEventHandler)( + [this](auto&&... args) + { + if (!m_configuration.m_publish) + { + return; + } + PublishMessage(); + }); + m_eventSourceAdapter.ConnectToAdaptedEvent(m_adaptedEventHandler); + m_eventSourceAdapter.Start(); + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointStatePublisher.h b/Gems/ROS2/Code/Source/Manipulation/JointStatePublisher.h new file mode 100644 index 000000000..b3c632870 --- /dev/null +++ b/Gems/ROS2/Code/Source/Manipulation/JointStatePublisher.h @@ -0,0 +1,54 @@ +/* + * 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 +#include +#include +#include +#include + +#include +#include + +namespace ROS2 +{ + struct JointStatePublisherContext + { + AZ::EntityId m_entityId; + AZStd::string m_frameId; + AZStd::string m_publisherNamespace; + }; + + //! A class responsible for publishing the joint positions on ROS2 /joint_states topic. + //!< @see jointState message. + class JointStatePublisher + { + public: + JointStatePublisher(const PublisherConfiguration& configuration, const JointStatePublisherContext& context); + virtual ~JointStatePublisher(); + + void InitializePublisher(); + + private: + void PublishMessage(); + + EventSourceAdapter m_eventSourceAdapter; + typename PhysicsBasedSource::AdaptedEventHandlerType m_adaptedEventHandler; + + PublisherConfiguration m_configuration; + JointStatePublisherContext m_context; + + std::shared_ptr> m_jointStatePublisher; + sensor_msgs::msg::JointState m_jointStateMsg; + + AZStd::vector m_jointNames; + AZStd::vector m_jointInfos; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.cpp new file mode 100644 index 000000000..3dde0ac0e --- /dev/null +++ b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.cpp @@ -0,0 +1,467 @@ +/* + * 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 "JointsManipulationComponent.h" +#include "Controllers/JointsArticulationControllerComponent.h" +#include "Controllers/JointsPIDControllerComponent.h" +#include "JointStatePublisher.h" +#include "ManipulationUtils.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ROS2 +{ + namespace Internal + { + void Add1DOFJointInfo(const AZ::EntityComponentIdPair& idPair, const AZStd::string& jointName, ManipulationJoints& joints) + { + if (joints.find(jointName) != joints.end()) + { + AZ_Assert(false, "Joint names in hierarchy need to be unique (%s is not)!", jointName.c_str()); + return; + } + AZ_Printf("JointsManipulationComponent", "Adding joint info for hinge joint %s\n", jointName.c_str()); + JointInfo jointInfo; + jointInfo.m_isArticulation = false; + jointInfo.m_axis = static_cast(0); + jointInfo.m_entityComponentIdPair = idPair; + joints[jointName] = jointInfo; + } + + bool TryGetFreeArticulationAxis(const AZ::EntityId& entityId, PhysX::ArticulationJointAxis& axis) + { + for (AZ::u8 i = 0; i <= static_cast(PhysX::ArticulationJointAxis::Z); i++) + { + PhysX::ArticulationJointMotionType type = PhysX::ArticulationJointMotionType::Locked; + axis = static_cast(i); + // Use bus to prevent compilation error without PhysX Articulation support. + PhysX::ArticulationJointRequestBus::EventResult(type, entityId, &PhysX::ArticulationJointRequests::GetMotion, axis); + if (type != PhysX::ArticulationJointMotionType::Locked) + { + return true; + } + } + return false; + } + + void AddArticulationJointInfo(const AZ::EntityComponentIdPair& idPair, const AZStd::string& jointName, ManipulationJoints& joints) + { + PhysX::ArticulationJointAxis freeAxis; + bool hasFreeAxis = TryGetFreeArticulationAxis(idPair.GetEntityId(), freeAxis); + if (!hasFreeAxis) + { // Do not add a joint since it is a fixed one + AZ_Printf("JointsManipulationComponent", "Articulation joint %s is fixed, skipping\n", jointName.c_str()); + return; + } + + if (joints.find(jointName) != joints.end()) + { + AZ_Assert(false, "Joint names in hierarchy need to be unique (%s is not)!", jointName.c_str()); + return; + } + + AZ_Printf("JointsManipulationComponent", "Adding joint info for articulation link %s\n", jointName.c_str()); + JointInfo jointInfo; + jointInfo.m_isArticulation = true; + jointInfo.m_axis = freeAxis; + jointInfo.m_entityComponentIdPair = idPair; + joints[jointName] = jointInfo; + } + + ManipulationJoints GetAllEntityHierarchyJoints(const AZ::EntityId& entityId) + { // Look for either Articulation Links or Hinge joints in entity hierarchy and collect them into a map. + // Determine kind of joints through presence of appropriate controller + bool supportsArticulation = false; + bool supportsClassicJoints = false; + JointsPositionControllerRequestBus::EventResult( + supportsArticulation, entityId, &JointsPositionControllerRequests::SupportsArticulation); + JointsPositionControllerRequestBus::EventResult( + supportsClassicJoints, entityId, &JointsPositionControllerRequests::SupportsClassicJoints); + ManipulationJoints manipulationJoints; + if (!supportsArticulation && !supportsClassicJoints) + { + AZ_Warning("JointsManipulationComponent", false, "No suitable Position Controller Component in entity!"); + return manipulationJoints; + } + if (supportsArticulation && supportsClassicJoints) + { + AZ_Warning("JointsManipulationComponent", false, "Cannot support both classic joint and articulations in one hierarchy"); + return manipulationJoints; + } + + // Get all descendants and iterate over joints + AZStd::vector descendants; + AZ::TransformBus::EventResult(descendants, entityId, &AZ::TransformInterface::GetEntityAndAllDescendants); + AZ_Warning("JointsManipulationComponent", descendants.size() > 0, "Entity %s has no descendants!", entityId.ToString().c_str()); + for (const AZ::EntityId& descendantID : descendants) + { + AZ::Entity* entity = nullptr; + AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, descendantID); + AZ_Assert(entity, "Unknown entity %s", descendantID.ToString().c_str()); + + // If there is a Frame Component, take joint name stored in it. + auto* frameComponent = Utils::GetGameOrEditorComponent(entity); + if (!frameComponent) + { // Frame Component is required for joints. + continue; + } + const AZStd::string jointName(frameComponent->GetJointName().GetCStr()); + + auto* hingeComponent = + azrtti_cast(Utils::GetGameOrEditorComponent(entity)); + auto* prismaticComponent = + azrtti_cast(Utils::GetGameOrEditorComponent(entity)); + auto* articulationComponent = Utils::GetGameOrEditorComponent(entity); + [[maybe_unused]] bool classicJoint = hingeComponent || prismaticComponent; + AZ_Warning( + "JointsManipulationComponent", + (classicJoint && supportsClassicJoints) || !classicJoint, + "Found classic joints but the controller does not support them!"); + AZ_Warning( + "JointsManipulationComponent", + (articulationComponent && supportsArticulation) || !articulationComponent, + "Found articulations but the controller does not support them!"); + + // See if there is a Hinge Joint in the entity, add it to map. + if (supportsClassicJoints && hingeComponent) + { + auto idPair = AZ::EntityComponentIdPair(hingeComponent->GetEntityId(), hingeComponent->GetId()); + 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. + if (supportsArticulation && articulationComponent) + { + auto idPair = AZ::EntityComponentIdPair(articulationComponent->GetEntityId(), articulationComponent->GetId()); + Internal::AddArticulationJointInfo(idPair, jointName, manipulationJoints); + } + } + return manipulationJoints; + } + + void SetInitialPositions(ManipulationJoints& manipulationJoints, const AZStd::unordered_map& initialPositions) + { + // Set the initial / resting position to move to and keep. + for (const auto& [jointName, jointInfo] : manipulationJoints) + { + if (initialPositions.contains(jointName)) + { + manipulationJoints[jointName].m_restPosition = initialPositions.at(jointName); + } + else + { + AZ_Warning("JointsManipulationComponent", false, "No set initial position for joint %s", jointName.c_str()); + } + } + } + } // namespace Internal + + JointsManipulationComponent::JointsManipulationComponent() + { + } + + JointsManipulationComponent::JointsManipulationComponent( + const PublisherConfiguration& configuration, const AZStd::unordered_map& initialPositions) + : m_jointStatePublisherConfiguration(configuration) + , m_initialPositions(initialPositions) + { + } + + void JointsManipulationComponent::Activate() + { + auto* ros2Frame = Utils::GetGameOrEditorComponent(GetEntity()); + JointStatePublisherContext publisherContext; + publisherContext.m_publisherNamespace = ros2Frame->GetNamespace(); + publisherContext.m_frameId = ros2Frame->GetFrameID(); + publisherContext.m_entityId = GetEntityId(); + + m_jointStatePublisher = AZStd::make_unique(m_jointStatePublisherConfiguration, publisherContext); + + AZ::TickBus::Handler::BusConnect(); + JointsManipulationRequestBus::Handler::BusConnect(GetEntityId()); + } + + void JointsManipulationComponent::Deactivate() + { + JointsManipulationRequestBus::Handler::BusDisconnect(); + AZ::TickBus::Handler::BusDisconnect(); + } + + ManipulationJoints JointsManipulationComponent::GetJoints() + { + return m_manipulationJoints; + } + + AZ::Outcome 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 JointsManipulationComponent::GetJointPosition(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 GetJointPosition(jointInfo); + } + + AZ::Outcome JointsManipulationComponent::GetJointVelocity(const JointInfo& jointInfo) + { + float velocity{ 0.f }; + if (jointInfo.m_isArticulation) + { + PhysX::ArticulationJointRequestBus::EventResult( + velocity, + jointInfo.m_entityComponentIdPair.GetEntityId(), + &PhysX::ArticulationJointRequests::GetJointVelocity, + jointInfo.m_axis); + } + else + { + PhysX::JointRequestBus::EventResult(velocity, jointInfo.m_entityComponentIdPair, &PhysX::JointRequests::GetVelocity); + } + return AZ::Success(velocity); + } + + AZ::Outcome 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() + { + JointsManipulationRequests::JointsPositionsMap positions; + for (const auto& [jointName, jointInfo] : m_manipulationJoints) + { + 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 JointsManipulationComponent::GetJointEffort(const JointInfo& jointInfo) + { + auto jointStateData = Utils::GetJointState(jointInfo); + + return AZ::Success(jointStateData.effort); + } + + AZ::Outcome 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 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 JointsManipulationComponent::MoveJointToPosition( + const AZStd::string& jointName, JointPosition position) + { + if (!m_manipulationJoints.contains(jointName)) + { + return AZ::Failure(AZStd::string::format("Joint %s does not exist", jointName.c_str())); + } + m_manipulationJoints[jointName].m_restPosition = position; + return AZ::Success(); + } + + AZ::Outcome JointsManipulationComponent::MoveJointsToPositions( + const JointsManipulationRequests::JointsPositionsMap& positions) + { + for (const auto& [jointName, position] : positions) + { + auto outcome = MoveJointToPosition(jointName, position); + if (!outcome) + { + return outcome; + } + } + return AZ::Success(); + } + + void JointsManipulationComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required) + { + required.push_back(AZ_CRC_CE("ROS2Frame")); + required.push_back(AZ_CRC_CE("JointsControllerService")); + } + + void JointsManipulationComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) + { + provided.push_back(AZ_CRC_CE("JointsManipulationService")); + } + + void JointsManipulationComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) + { + incompatible.push_back(AZ_CRC_CE("JointsManipulationService")); + } + + void JointsManipulationComponent::Reflect(AZ::ReflectContext* context) + { + if (AZ::SerializeContext* serialize = azrtti_cast(context)) + { + serialize->Class() + ->Version(1) + ->Field("JointStatesPublisherConfiguration", &JointsManipulationComponent::m_jointStatePublisherConfiguration) + ->Field("InitialJointPosition", &JointsManipulationComponent::m_initialPositions); + } + } + + void JointsManipulationComponent::MoveToSetPositions(float deltaTime) + { + for (const auto& [jointName, jointInfo] : m_manipulationJoints) + { + float currentPosition = GetJointPosition(jointName).GetValue(); + float desiredPosition = jointInfo.m_restPosition; + + AZ::Outcome positionControlOutcome; + JointsPositionControllerRequestBus::EventResult( + positionControlOutcome, + GetEntityId(), + &JointsPositionControllerRequests::PositionControl, + jointName, + jointInfo, + currentPosition, + desiredPosition, + deltaTime); + + AZ_Warning( + "JointsManipulationComponent", + positionControlOutcome, + "Position control failed for joint %s (%s): %s", + jointName.c_str(), + jointInfo.m_entityComponentIdPair.GetEntityId().ToString().c_str(), + positionControlOutcome.GetError().c_str()); + } + } + + void JointsManipulationComponent::Stop() + { + for (auto& [jointName, jointInfo] : m_manipulationJoints) + { // 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(); + } + } + + AZStd::string JointsManipulationComponent::GetManipulatorNamespace() const + { + auto* frameComponent = Utils::GetGameOrEditorComponent(m_entity); + AZ_Assert(frameComponent, "ROS2FrameComponent is required for joints."); + return frameComponent->GetNamespace(); + } + + void JointsManipulationComponent::OnTick(float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time) + { + if (m_manipulationJoints.empty()) + { + const AZStd::string manipulatorNamespace = GetManipulatorNamespace(); + AZStd::unordered_map intialPositonNamespaced; + AZStd::transform( + m_initialPositions.begin(), + m_initialPositions.end(), + AZStd::inserter(intialPositonNamespaced, intialPositonNamespaced.end()), + [&manipulatorNamespace](const auto& pair) + { + return AZStd::make_pair(ROS2::ROS2Names::GetNamespacedName(manipulatorNamespace, pair.first), pair.second); + }); + + m_manipulationJoints = Internal::GetAllEntityHierarchyJoints(GetEntityId()); + + Internal::SetInitialPositions(m_manipulationJoints, intialPositonNamespaced); + if (m_manipulationJoints.empty()) + { + AZ_Warning("JointsManipulationComponent", false, "No manipulation joints to handle!"); + AZ::TickBus::Handler::BusDisconnect(); + return; + } + m_jointStatePublisher->InitializePublisher(); + } + MoveToSetPositions(deltaTime); + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.h b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.h new file mode 100644 index 000000000..b10dc8eb4 --- /dev/null +++ b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.h @@ -0,0 +1,86 @@ +/* + * 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 +#include +#include +#include + +#include "JointStatePublisher.h" +#include + +namespace ROS2 +{ + //! Component responsible for controlling a hierarchical system of joints such as robotic arm with Articulations or Hinge Joints. + //! This manipulator component uses simple joint position interface. For trajectory control, see JointsTrajectoryComponent. + class JointsManipulationComponent + : public AZ::Component + , public AZ::TickBus::Handler + , public JointsManipulationRequestBus::Handler + { + public: + JointsManipulationComponent(); + JointsManipulationComponent( + const PublisherConfiguration& configuration, const AZStd::unordered_map& initialPositions); + ~JointsManipulationComponent() = default; + AZ_COMPONENT(JointsManipulationComponent, "{3da9abfc-0028-4e3e-8d04-4e4440d2e319}", 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); + + // JointsManipulationRequestBus::Handler overrides ... + //! @see ROS2::JointsManipulationRequestBus::GetJoints + ManipulationJoints GetJoints() override; + //! @see ROS2::JointsManipulationRequestBus::GetJointPosition + AZ::Outcome GetJointPosition(const AZStd::string& jointName) override; + //! @see ROS2::JointsManipulationRequestBus::GetJointVelocity + AZ::Outcome 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 GetJointEffort(const AZStd::string& jointName) override; + //! @see ROS2::JointsManipulationRequestBus::GetAllJointsEfforts + JointsEffortsMap GetAllJointsEfforts() override; + //! @see ROS2::JointsManipulationRequestBus::SetMaxJointEffort + AZ::Outcome SetMaxJointEffort(const AZStd::string& jointName, JointEffort maxEffort); + //! @see ROS2::JointsManipulationRequestBus::MoveJointsToPositions + AZ::Outcome MoveJointsToPositions(const JointsPositionsMap& positions) override; + //! @see ROS2::JointsManipulationRequestBus::MoveJointToPosition + AZ::Outcome MoveJointToPosition(const AZStd::string& jointName, JointPosition position) override; + //! @see ROS2::JointsManipulationRequestBus::Stop + void Stop() override; + + private: + // Component overrides ... + void Activate() override; + void Deactivate() override; + + // AZ::TickBus::Handler overrides + void OnTick(float deltaTime, AZ::ScriptTimePoint time) override; + + void MoveToSetPositions(float deltaTime); + + AZStd::string GetManipulatorNamespace() const; + + AZ::Outcome GetJointPosition(const JointInfo& jointInfo); + AZ::Outcome GetJointVelocity(const JointInfo& jointInfo); + AZ::Outcome GetJointEffort(const JointInfo& jointInfo); + + AZStd::unique_ptr m_jointStatePublisher; + PublisherConfiguration m_jointStatePublisherConfiguration; + ManipulationJoints m_manipulationJoints; //!< Map of JointInfo where the key is a joint name (with namespace included) + AZStd::unordered_map + m_initialPositions; //!< Initial positions where the key is joint name (without namespace included) + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.cpp new file mode 100644 index 000000000..19eda6983 --- /dev/null +++ b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.cpp @@ -0,0 +1,82 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#include "JointsManipulationEditorComponent.h" +#include "JointsManipulationComponent.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ROS2 +{ + JointsManipulationEditorComponent::JointsManipulationEditorComponent() + { + m_jointStatePublisherConfiguration.m_topicConfiguration.m_type = "sensor_msgs::msg::JointState"; + m_jointStatePublisherConfiguration.m_topicConfiguration.m_topic = "joint_states"; + m_jointStatePublisherConfiguration.m_frequency = 25.0f; + } + + void JointsManipulationEditorComponent::BuildGameEntity(AZ::Entity* gameEntity) + { + gameEntity->CreateComponent(m_jointStatePublisherConfiguration, m_initialPositions); + } + + void JointsManipulationEditorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required) + { + required.push_back(AZ_CRC_CE("ROS2Frame")); + required.push_back(AZ_CRC_CE("JointsControllerService")); + } + + void JointsManipulationEditorComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) + { + provided.push_back(AZ_CRC_CE("JointsManipulationService")); + } + + void JointsManipulationEditorComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) + { + incompatible.push_back(AZ_CRC_CE("JointsManipulationService")); + } + + void JointsManipulationEditorComponent::Reflect(AZ::ReflectContext* context) + { + if (AZ::SerializeContext* serialize = azrtti_cast(context)) + { + serialize->Class() + ->Version(0) + ->Field("JointStatePublisherConfiguration", &JointsManipulationEditorComponent::m_jointStatePublisherConfiguration) + ->Field("Initial positions", &JointsManipulationEditorComponent::m_initialPositions); + + if (AZ::EditContext* ec = serialize->GetEditContext()) + { + ec->Class("JointsManipulationEditorComponent", "Component for manipulation of joints") + ->ClassElement(AZ::Edit::ClassElements::EditorData, "") + ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game")) + ->Attribute(AZ::Edit::Attributes::Category, "ROS2") + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/JointsManipulationEditorComponent.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/JointsManipulationEditorComponent.svg") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &JointsManipulationEditorComponent::m_jointStatePublisherConfiguration, + "Joint State Publisher", + "Configuration of Joint State Publisher") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &JointsManipulationEditorComponent::m_initialPositions, + "Initial positions", + "Initial positions of all the joints"); + } + } + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.h b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.h new file mode 100644 index 000000000..66f8df2f3 --- /dev/null +++ b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.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 +#include +#include +#include + +namespace ROS2 +{ + //! Editor Component responsible for a hierarchical system of joints such as robotic arm with Articulations or Hinge Joints. + class JointsManipulationEditorComponent : public AzToolsFramework::Components::EditorComponentBase + { + public: + JointsManipulationEditorComponent(); + ~JointsManipulationEditorComponent() = default; + AZ_EDITOR_COMPONENT(JointsManipulationEditorComponent, "{BF2F77FD-92FB-4730-92C7-DDEE54F508BF}"); + + 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); + + // AzToolsFramework::Components::EditorComponentBase overrides + void BuildGameEntity(AZ::Entity* gameEntity) override; + + private: + PublisherConfiguration m_jointStatePublisherConfiguration; + AZStd::unordered_map m_initialPositions; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp new file mode 100644 index 000000000..035e6b5b0 --- /dev/null +++ b/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp @@ -0,0 +1,260 @@ +/* + * 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 "JointsTrajectoryComponent.h" +#include +#include +#include +#include +#include +#include + +namespace ROS2 +{ + void JointsTrajectoryComponent::Activate() + { + auto* ros2Frame = Utils::GetGameOrEditorComponent(GetEntity()); + AZ_Assert(ros2Frame, "Missing Frame Component!"); + AZStd::string namespacedAction = ROS2Names::GetNamespacedName(ros2Frame->GetNamespace(), m_followTrajectoryActionName); + m_followTrajectoryServer = AZStd::make_unique(namespacedAction, GetEntityId()); + AZ::TickBus::Handler::BusConnect(); + JointsTrajectoryRequestBus::Handler::BusConnect(GetEntityId()); + } + + ManipulationJoints& JointsTrajectoryComponent::GetManipulationJoints() + { + if (m_manipulationJoints.empty()) + { + JointsManipulationRequestBus::EventResult(m_manipulationJoints, GetEntityId(), &JointsManipulationRequests::GetJoints); + } + return m_manipulationJoints; + } + + void JointsTrajectoryComponent::Deactivate() + { + JointsTrajectoryRequestBus::Handler::BusDisconnect(); + AZ::TickBus::Handler::BusDisconnect(); + m_followTrajectoryServer.reset(); + } + + void JointsTrajectoryComponent::Reflect(AZ::ReflectContext* context) + { + if (AZ::SerializeContext* serialize = azrtti_cast(context)) + { + serialize->Class()->Version(0)->Field( + "Action name", &JointsTrajectoryComponent::m_followTrajectoryActionName); + + if (AZ::EditContext* ec = serialize->GetEditContext()) + { + ec->Class("JointsTrajectoryComponent", "Component to control a robotic arm using trajectories") + ->ClassElement(AZ::Edit::ClassElements::EditorData, "") + ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game")) + ->Attribute(AZ::Edit::Attributes::Category, "ROS2") + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/JointsTrajectoryComponent.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/JointsTrajectoryComponent.svg") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &JointsTrajectoryComponent::m_followTrajectoryActionName, + "Action Name", + "Name the follow trajectory action server to accept movement commands"); + } + } + } + + void JointsTrajectoryComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required) + { + required.push_back(AZ_CRC_CE("ROS2Frame")); + required.push_back(AZ_CRC_CE("JointsManipulationService")); + } + + void JointsTrajectoryComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) + { + provided.push_back(AZ_CRC_CE("ManipulatorJointTrajectoryService")); + } + + void JointsTrajectoryComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) + { + incompatible.push_back(AZ_CRC_CE("ManipulatorJointTrajectoryService")); + } + + AZ::Outcome JointsTrajectoryComponent::StartTrajectoryGoal( + TrajectoryGoalPtr trajectoryGoal) + { + if (m_trajectoryInProgress) + { + 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); + if (!validationResult) + { + return validationResult; + } + m_trajectoryGoal = *trajectoryGoal; + m_trajectoryExecutionStartTime = rclcpp::Time(ROS2::ROS2Interface::Get()->GetROSTimestamp()); + m_trajectoryInProgress = true; + return AZ::Success(); + } + + AZ::Outcome JointsTrajectoryComponent::ValidateGoal(TrajectoryGoalPtr trajectoryGoal) + { + // Check joint names validity + for (const auto& jointName : trajectoryGoal->trajectory.joint_names) + { + 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()); + + 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(); + } + + void JointsTrajectoryComponent::UpdateFeedback() + { + auto goalStatus = GetGoalStatus(); + if (goalStatus != JointsTrajectoryRequests::TrajectoryActionStatus::Executing) + { + return; + } + + auto feedback = std::make_shared(); + + 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); + + 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(currentJointPosition)); + actualPoint.velocities.push_back(static_cast(currentJointVelocity)); + // Acceleration should also be filled in somehow, or removed from the trajectory altogether. + } + + trajectory_msgs::msg::JointTrajectoryPoint currentError; + 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 JointsTrajectoryComponent::CancelTrajectoryGoal() + { + m_trajectoryGoal.trajectory.points.clear(); + m_trajectoryInProgress = false; + return AZ::Success(); + } + + JointsTrajectoryRequests::TrajectoryActionStatus JointsTrajectoryComponent::GetGoalStatus() + { + return m_followTrajectoryServer->GetGoalStatus(); + } + + void JointsTrajectoryComponent::FollowTrajectory(const uint64_t deltaTimeNs) + { + auto goalStatus = GetGoalStatus(); + if (goalStatus == JointsTrajectoryRequests::TrajectoryActionStatus::Cancelled) + { + JointsManipulationRequestBus::Event(GetEntityId(), &JointsManipulationRequests::Stop); + auto result = std::make_shared(); + result->error_string = "User Cancelled"; + result->error_code = FollowJointTrajectoryActionServer::FollowJointTrajectory::Result::SUCCESSFUL; + m_followTrajectoryServer->CancelGoal(result); + m_followTrajectoryServer->SetGoalSuccess(); + return; + } + + if (goalStatus != JointsTrajectoryRequests::TrajectoryActionStatus::Executing) + { + return; + } + + if (m_trajectoryGoal.trajectory.points.size() == 0) + { // The manipulator has reached the goal. + AZ_TracePrintf("JointsManipulationComponent", "Goal Concluded: all points reached\n"); + auto successResult = std::make_shared(); //!< Empty defaults to success. + m_followTrajectoryServer->GoalSuccess(successResult); + m_trajectoryInProgress = false; + return; + } + + auto desiredGoal = m_trajectoryGoal.trajectory.points.front(); + rclcpp::Duration targetGoalTime = rclcpp::Duration(desiredGoal.time_from_start); //!< Requested arrival time for trajectory point. + rclcpp::Time timeNow = rclcpp::Time(ROS2::ROS2Interface::Get()->GetROSTimestamp()); //!< Current simulation time. + rclcpp::Duration threshold = rclcpp::Duration::from_nanoseconds(1e7); + + if (m_trajectoryExecutionStartTime + targetGoalTime <= timeNow + threshold) + { // Jump to the next point if current simulation time is ahead of timeFromStart + m_trajectoryGoal.trajectory.points.erase(m_trajectoryGoal.trajectory.points.begin()); + FollowTrajectory(deltaTimeNs); + return; + } + + MoveToNextPoint(desiredGoal); + } + + void JointsTrajectoryComponent::MoveToNextPoint(const trajectory_msgs::msg::JointTrajectoryPoint currentTrajectoryPoint) + { + for (int jointIndex = 0; jointIndex < m_trajectoryGoal.trajectory.joint_names.size(); jointIndex++) + { // Order each joint to be moved + AZStd::string jointName(m_trajectoryGoal.trajectory.joint_names[jointIndex].c_str()); + AZ_Assert(m_manipulationJoints.find(jointName) != m_manipulationJoints.end(), "Invalid trajectory executing"); + + float targetPos = currentTrajectoryPoint.positions[jointIndex]; + AZ::Outcome result; + JointsManipulationRequestBus::EventResult( + result, GetEntityId(), &JointsManipulationRequests::MoveJointToPosition, jointName, targetPos); + AZ_Warning("JointTrajectoryComponent", result, "Joint move cannot be realized: %s", result.GetError().c_str()); + } + } + + void JointsTrajectoryComponent::OnTick(float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time) + { + if (m_manipulationJoints.empty()) + { + GetManipulationJoints(); + return; + } + uint64_t deltaTimeNs = deltaTime * 1'000'000'000; + FollowTrajectory(deltaTimeNs); + UpdateFeedback(); + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.h b/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.h new file mode 100644 index 000000000..e743b05a7 --- /dev/null +++ b/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.h @@ -0,0 +1,70 @@ +/* + * 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 "FollowJointTrajectoryActionServer.h" +#include +#include +#include +#include +#include +#include + +namespace ROS2 +{ + //! Component responsible for execution of commands to move robotic arm (manipulator) based on set trajectory goal. + class JointsTrajectoryComponent + : public AZ::Component + , public AZ::TickBus::Handler + , public JointsTrajectoryRequestBus::Handler + { + public: + JointsTrajectoryComponent() = default; + ~JointsTrajectoryComponent() = default; + AZ_COMPONENT(JointsTrajectoryComponent, "{429DE04C-6B6D-4B2D-9D6C-3681F23CBF90}", 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); + + // JointsTrajectoryRequestBus::Handler overrides ... + //! @see ROS2::JointsTrajectoryRequestBus::StartTrajectoryGoal + AZ::Outcome StartTrajectoryGoal(TrajectoryGoalPtr trajectoryGoal) override; + //! @see ROS2::JointsTrajectoryRequestBus::CancelTrajectoryGoal + AZ::Outcome CancelTrajectoryGoal() override; + //! @see ROS2::JointsTrajectoryRequestBus::GetGoalStatus + TrajectoryActionStatus GetGoalStatus() override; + + private: + // Component overrides ... + void Activate() override; + void Deactivate() override; + + // AZ::TickBus::Handler overrides + void OnTick(float deltaTime, AZ::ScriptTimePoint time) override; + + //! Follow set trajectory. + //! @param deltaTimeNs frame time step, to advance trajectory by. + void FollowTrajectory(const uint64_t deltaTimeNs); + AZ::Outcome ValidateGoal(TrajectoryGoalPtr trajectoryGoal); + void MoveToNextPoint(const trajectory_msgs::msg::JointTrajectoryPoint currentTrajectoryPoint); + void UpdateFeedback(); + + //! Lazy initialize Manipulation joints on the start of simulation. + ManipulationJoints& GetManipulationJoints(); + + AZStd::string m_followTrajectoryActionName{ "arm_controller/follow_joint_trajectory" }; + AZStd::unique_ptr m_followTrajectoryServer; + TrajectoryGoal m_trajectoryGoal; + rclcpp::Time m_trajectoryExecutionStartTime; + ManipulationJoints m_manipulationJoints; + bool m_trajectoryInProgress{ false }; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/ManipulationUtils.cpp b/Gems/ROS2/Code/Source/Manipulation/ManipulationUtils.cpp new file mode 100644 index 000000000..a906256bf --- /dev/null +++ b/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 +#include + +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 diff --git a/Gems/ROS2/Code/Source/Manipulation/ManipulationUtils.h b/Gems/ROS2/Code/Source/Manipulation/ManipulationUtils.h new file mode 100644 index 000000000..e19821630 --- /dev/null +++ b/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 + +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 diff --git a/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/JointMotorControllerComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/JointMotorControllerComponent.cpp new file mode 100644 index 000000000..090803c62 --- /dev/null +++ b/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/JointMotorControllerComponent.cpp @@ -0,0 +1,113 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include + +namespace ROS2 +{ + void JointMotorControllerComponent::Activate() + { + AZ::TickBus::Handler::BusConnect(); + ImGui::ImGuiUpdateListenerBus::Handler::BusConnect(); + AZ::EntityBus::Handler::BusConnect(GetEntityId()); + } + + void JointMotorControllerComponent::Deactivate() + { + ImGui::ImGuiUpdateListenerBus::Handler::BusDisconnect(); + AZ::TickBus::Handler::BusDisconnect(); + } + + void JointMotorControllerComponent::Reflect(AZ::ReflectContext* context) + { + JointMotorControllerConfiguration::Reflect(context); + + if (auto* serializeContext = azrtti_cast(context)) + { + serializeContext->Class()->Version(2)->Field( + "JointMotorControllerConfiguration", &JointMotorControllerComponent::m_jointMotorControllerConfiguration); + if (AZ::EditContext* ec = serializeContext->GetEditContext()) + { + ec->Class( + "Joint Motor Controller Component", "Base component for motor controller components.") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &JointMotorControllerComponent::m_jointMotorControllerConfiguration, + "Motor Controller Configuration", + "Motor Controller Configuration"); + } + } + } + + void JointMotorControllerComponent::OnImGuiUpdate() + { + if (!m_jointComponentIdPair.GetEntityId().IsValid() || + !(m_jointMotorControllerConfiguration.m_isDebugController || m_jointMotorControllerConfiguration.m_debugMode)) + { + return; + } + + AZStd::pair limits{ 0.0f, 0.0f }; + PhysX::JointRequestBus::EventResult(limits, m_jointComponentIdPair, &PhysX::JointRequests::GetLimits); + PhysX::JointRequestBus::EventResult(m_currentSpeed, m_jointComponentIdPair, &PhysX::JointRequests::GetVelocity); + + AZStd::string s = + AZStd::string::format("Joint Motor Controller %s:%s", GetEntity()->GetName().c_str(), GetEntity()->GetId().ToString().c_str()); + ImGui::Begin(s.c_str()); + + ImGui::PushItemWidth(200.0f); + ImGui::SliderFloat("Position", &m_currentPosition, limits.first, limits.second); + ImGui::SliderFloat("Speed", &m_currentSpeed, -5.0f, 5.0f); + ImGui::PopItemWidth(); + + DisplayControllerParameters(); + + ImGui::End(); + } + + void JointMotorControllerComponent::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time) + { + if (!m_jointComponentIdPair.GetEntityId().IsValid()) + { + return; + } + + PhysX::JointRequestBus::EventResult(m_currentPosition, m_jointComponentIdPair, &PhysX::JointRequests::GetPosition); + float setSpeed = CalculateMotorSpeed(deltaTime); + PhysX::JointRequestBus::Event(m_jointComponentIdPair, &PhysX::JointRequests::SetVelocity, setSpeed); + } + + void JointMotorControllerComponent::OnEntityActivated(const AZ::EntityId& entityId) + { + AZ::ComponentId componentId; + if (auto* prismaticJointComponent = GetEntity()->FindComponent(); prismaticJointComponent) + { + componentId = prismaticJointComponent->GetId(); + } + else if (auto* hingeJointComponent = GetEntity()->FindComponent(); hingeJointComponent) + { + componentId = hingeJointComponent->GetId(); + } + else + { + AZ_Warning( + "MotorizedJointComponent", + false, + "Entity with ID %s either has no PhysX::Joint component or the joint is neither a Prismatic nor a Hinge Joint", + GetEntityId().ToString().c_str()); + } + + m_jointComponentIdPair = { GetEntityId(), componentId }; + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/JointMotorControllerConfiguration.cpp b/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/JointMotorControllerConfiguration.cpp new file mode 100644 index 000000000..293b92526 --- /dev/null +++ b/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/JointMotorControllerConfiguration.cpp @@ -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 + * + */ +#include +#include + +namespace ROS2 +{ + bool JointMotorControllerConfiguration::IsDebugModeVisible() const + { + return !m_isDebugController; + } + + void JointMotorControllerConfiguration::Reflect(AZ::ReflectContext* context) + { + if (auto serializeContext = azrtti_cast(context)) + { + serializeContext->Class()->Version(0)->Field( + "DebugMode", &JointMotorControllerConfiguration::m_debugMode); + + if (AZ::EditContext* ec = serializeContext->GetEditContext()) + { + ec->Class("Joint Motor Controller Configuration", "Motor Controller Configuration") + ->DataElement( + AZ::Edit::UIHandlers::Default, &JointMotorControllerConfiguration::m_debugMode, "Debug Mode", "Enable Debug Mode") + ->Attribute(AZ::Edit::Attributes::Visibility, &JointMotorControllerConfiguration::IsDebugModeVisible); + } + } + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/ManualMotorControllerComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/ManualMotorControllerComponent.cpp new file mode 100644 index 000000000..52fb2f78b --- /dev/null +++ b/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/ManualMotorControllerComponent.cpp @@ -0,0 +1,54 @@ +/* + * 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 +#include +#include + +namespace ROS2 +{ + ManualMotorControllerComponent::ManualMotorControllerComponent() + { + m_jointMotorControllerConfiguration.m_isDebugController = true; + } + + void ManualMotorControllerComponent::Reflect(AZ::ReflectContext* context) + { + if (auto* serializeContext = azrtti_cast(context)) + { + serializeContext->Class()->Version(2); + + if (AZ::EditContext* ec = serializeContext->GetEditContext()) + { + ec->Class("Manual Motor Controller", "Debug motor controller used via ImGui.") + ->ClassElement(AZ::Edit::ClassElements::EditorData, "") + ->Attribute(AZ::Edit::Attributes::Category, "ROS2") + ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/ManualMotorController.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/ManualMotorController.svg"); + } + } + } + + float ManualMotorControllerComponent::CalculateMotorSpeed([[maybe_unused]] float deltaTime) + { + return m_setSpeed; + } + + void ManualMotorControllerComponent::DisplayControllerParameters() + { + ImGui::PushItemWidth(200.0f); + ImGui::SliderFloat("SetSpeed", &m_setSpeed, -5.0f, 5.0f); + + ImGui::PopItemWidth(); + + if (ImGui::Button("Zero")) + { + m_setSpeed = 0.0f; + } + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/PidMotorControllerComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/PidMotorControllerComponent.cpp new file mode 100644 index 000000000..b0878e41c --- /dev/null +++ b/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/PidMotorControllerComponent.cpp @@ -0,0 +1,106 @@ +/* + * 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 +#include +#include +#include +#include +#include + +namespace ROS2 +{ + void PidMotorControllerComponent::Reflect(AZ::ReflectContext* context) + { + if (auto* serializeContext = azrtti_cast(context)) + { + serializeContext->Class() + ->Field("ZeroOffset", &PidMotorControllerComponent::m_zeroOffset) + ->Field("PidPosition", &PidMotorControllerComponent::m_pidPos) + ->Version(2); + + if (AZ::EditContext* ec = serializeContext->GetEditContext()) + { + ec->Class("PID Motor Controller", "") + ->ClassElement(AZ::Edit::ClassElements::EditorData, "") + ->Attribute(AZ::Edit::Attributes::Category, "ROS2") + ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/PidMotorController.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/PidMotorController.svg") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &PidMotorControllerComponent::m_zeroOffset, + "Zero Offset", + "Allows to change offset of zero to set point") + ->DataElement(AZ::Edit::UIHandlers::Default, &PidMotorControllerComponent::m_pidPos, "Pid Position", "Pid Position"); + } + } + + if (AZ::BehaviorContext* behaviorContext = azrtti_cast(context)) + { + behaviorContext->EBus("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() + { + m_pidPos.InitializePid(); + PidMotorControllerRequestBus::Handler::BusConnect(GetEntityId()); + JointMotorControllerComponent::Activate(); + } + + void PidMotorControllerComponent::Deactivate() + { + JointMotorControllerComponent::Deactivate(); + PidMotorControllerRequestBus::Handler::BusDisconnect(); + } + + void PidMotorControllerComponent::SetSetpoint(float setpoint) + { + m_setPoint = setpoint; + } + + float PidMotorControllerComponent::GetSetpoint() + { + return m_setPoint; + } + + float PidMotorControllerComponent::GetCurrentMeasurement() + { + return m_currentPosition - m_zeroOffset; + } + + float PidMotorControllerComponent::GetError() + { + return m_error; + } + + float PidMotorControllerComponent::CalculateMotorSpeed([[maybe_unused]] float deltaTime) + { + const float controlPositionError = (m_setPoint + m_zeroOffset) - m_currentPosition; + m_error = controlPositionError; + + const auto deltaTimeNs = aznumeric_cast(deltaTime * 1.0e9f); + return aznumeric_cast(m_pidPos.ComputeCommand(controlPositionError, deltaTimeNs)); + } + + void PidMotorControllerComponent::DisplayControllerParameters() + { + AZStd::pair limits{ 0.0f, 0.0f }; + PhysX::JointRequestBus::EventResult(limits, m_jointComponentIdPair, &PhysX::JointRequests::GetLimits); + + ImGui::PushItemWidth(200.0f); + ImGui::SliderFloat("SetPoint", &m_setPoint, limits.first + m_zeroOffset, limits.second + m_zeroOffset); + ImGui::PopItemWidth(); + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulator/MotorizedJointComponent.cpp b/Gems/ROS2/Code/Source/Manipulator/MotorizedJointComponent.cpp deleted file mode 100644 index 499428a29..000000000 --- a/Gems/ROS2/Code/Source/Manipulator/MotorizedJointComponent.cpp +++ /dev/null @@ -1,273 +0,0 @@ -/* - * 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 -#include -#include -#include -#include -#include - -namespace ROS2 -{ - void MotorizedJointComponent::Activate() - { - AZ::TickBus::Handler::BusConnect(); - m_pidPos.InitializePid(); - if (m_debugDrawEntity.IsValid()) - { - AZ::TransformBus::EventResult(m_debugDrawEntityInitialTransform, this->GetEntityId(), &AZ::TransformBus::Events::GetLocalTM); - } - MotorizedJointRequestBus::Handler::BusConnect(m_entity->GetId()); - } - - void MotorizedJointComponent::Deactivate() - { - AZ::TickBus::Handler::BusDisconnect(); - MotorizedJointRequestBus::Handler::BusDisconnect(); - } - - void MotorizedJointComponent::Reflect(AZ::ReflectContext* context) - { - if (AZ::SerializeContext* serialize = azrtti_cast(context)) - { - serialize->Class() - ->Version(2) - ->Field("JointAxis", &MotorizedJointComponent::m_jointDir) - ->Field("EffortAxis", &MotorizedJointComponent::m_effortAxis) - ->Field("Limit", &MotorizedJointComponent::m_limits) - ->Field("Linear", &MotorizedJointComponent::m_linear) - ->Field("AnimationMode", &MotorizedJointComponent::m_animationMode) - ->Field("ZeroOffset", &MotorizedJointComponent::m_zeroOffset) - ->Field("PidPosition", &MotorizedJointComponent::m_pidPos) - ->Field("DebugDrawEntity", &MotorizedJointComponent::m_debugDrawEntity) - ->Field("TestSinActive", &MotorizedJointComponent::m_testSinusoidal) - ->Field("TestSinAmplitude", &MotorizedJointComponent::m_sinAmplitude) - ->Field("TestSinFreq", &MotorizedJointComponent::m_sinFreq) - ->Field("TestSinDC", &MotorizedJointComponent::m_sinDC) - ->Field("DebugPrint", &MotorizedJointComponent::m_debugPrint) - ->Field("OverrideParent", &MotorizedJointComponent::m_measurementReferenceEntity); - - if (AZ::EditContext* ec = serialize->GetEditContext()) - { - ec->Class("MotorizedJointComponent", "MotorizedJointComponent") - ->ClassElement(AZ::Edit::ClassElements::EditorData, "MotorizedJointComponent") - ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) - ->Attribute(AZ::Edit::Attributes::Category, "MotorizedJointComponent") - ->DataElement( - AZ::Edit::UIHandlers::Default, - &MotorizedJointComponent::m_jointDir, - "Joint Dir.", - "Direction of joint in parent's reference frame.") - ->DataElement( - AZ::Edit::UIHandlers::Default, - &MotorizedJointComponent::m_effortAxis, - "Effort Dir.", - "Desired direction of force/torque vector that is applied to rigid body.") - - ->DataElement( - AZ::Edit::UIHandlers::Default, - &MotorizedJointComponent::m_limits, - "ControllerLimits", - "When measurement is outside the limits, ") - ->DataElement( - AZ::Edit::UIHandlers::Default, - &MotorizedJointComponent::m_debugDrawEntity, - "Setpoint", - "Allows to apply debug setpoint visualizer") - ->DataElement( - AZ::Edit::UIHandlers::Default, - &MotorizedJointComponent::m_zeroOffset, - "Zero Off.", - "Allows to change offset of zero to set point") - ->DataElement( - AZ::Edit::UIHandlers::Default, - &MotorizedJointComponent::m_linear, - "Linear joint", - "Applies linear force instead of torque") - ->DataElement( - AZ::Edit::UIHandlers::Default, - &MotorizedJointComponent::m_animationMode, - "Animation mode", - "In animation mode, the transform API is used instead of Rigid Body. " - "If this property is set to true the Rigid Body Component should be disabled.") - ->DataElement(AZ::Edit::UIHandlers::Default, &MotorizedJointComponent::m_pidPos, "PidPosition", "PidPosition") - ->DataElement( - AZ::Edit::UIHandlers::Default, - &MotorizedJointComponent::m_testSinusoidal, - "SinusoidalTest", - "Allows to apply sinusoidal test signal") - ->DataElement( - AZ::Edit::UIHandlers::Default, - &MotorizedJointComponent::m_sinAmplitude, - "Amplitude", - "Amplitude of sinusoidal test signal.") - ->DataElement( - AZ::Edit::UIHandlers::Default, - &MotorizedJointComponent::m_sinFreq, - "Frequency", - "Frequency of sinusoidal test signal.") - ->DataElement(AZ::Edit::UIHandlers::Default, &MotorizedJointComponent::m_sinDC, "DC", "DC of sinusoidal test signal.") - ->DataElement(AZ::Edit::UIHandlers::Default, &MotorizedJointComponent::m_debugPrint, "Debug", "Print debug to console") - ->DataElement( - AZ::Edit::UIHandlers::Default, - &MotorizedJointComponent::m_measurementReferenceEntity, - "Step Parent", - "Allows to override a parent to get correct measurement"); - } - } - } - void MotorizedJointComponent::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time) - { - const float measurement = ComputeMeasurement(time); - if (m_testSinusoidal) - { - m_setpoint = m_sinDC + m_sinAmplitude * AZ::Sin(m_sinFreq * time.GetSeconds()); - } - const float control_position_error = (m_setpoint + m_zeroOffset) - measurement; - m_error = control_position_error; - - if (m_debugDrawEntity.IsValid()) - { - if (m_linear) - { - AZ::Transform transform = AZ::Transform::Identity(); - transform.SetTranslation(m_jointDir * (m_setpoint + m_zeroOffset)); - AZ::TransformBus::Event( - m_debugDrawEntity, &AZ::TransformBus::Events::SetLocalTM, transform * m_debugDrawEntityInitialTransform); - } - else - { - AZ_Assert(false, "Not implemented"); - } - } - - const uint64_t deltaTimeNs = deltaTime * 1'000'000'000; - float speed_control = m_pidPos.ComputeCommand(control_position_error, deltaTimeNs); - - if (measurement <= m_limits.first) - { - // allow only positive control - speed_control = AZStd::max(0.f, speed_control); - } - else if (measurement >= m_limits.second) - { - // allow only negative control - speed_control = AZStd::min(0.f, speed_control); - } - - if (m_debugPrint) - { - AZ_Printf( - "MotorizedJointComponent", - " %s | pos: %f | err: %f | cntrl : %f | set : %f |\n", - GetEntity()->GetName().c_str(), - measurement, - control_position_error, - speed_control, - m_setpoint); - } - SetVelocity(speed_control, deltaTime); - } - - float MotorizedJointComponent::ComputeMeasurement(AZ::ScriptTimePoint time) - { - AZ::Transform transform; - if (!m_measurementReferenceEntity.IsValid()) - { - AZ::TransformBus::EventResult(transform, this->GetEntityId(), &AZ::TransformBus::Events::GetLocalTM); - } - else - { - AZ::Transform transformStepParent; - AZ::TransformBus::EventResult(transformStepParent, m_measurementReferenceEntity, &AZ::TransformBus::Events::GetWorldTM); - AZ::Transform transformStepChild; - AZ::TransformBus::EventResult(transformStepChild, GetEntityId(), &AZ::TransformBus::Events::GetWorldTM); - transform = transformStepParent.GetInverse() * transformStepChild; - } - if (m_linear) - { - const float last_position = m_currentPosition; - m_currentPosition = transform.GetTranslation().Dot(this->m_jointDir); - if (m_lastMeasurementTime > 0) - { - double delta_time = time.GetSeconds() - m_lastMeasurementTime; - m_currentVelocity = (m_currentPosition - last_position) / delta_time; - } - m_lastMeasurementTime = time.GetSeconds(); - return m_currentPosition; - } - AZ_Assert(false, "Measurement computation for rotation is not implemented"); - return 0; - } - - void MotorizedJointComponent::SetVelocity(float velocity, float deltaTime) - { - if (m_animationMode) - { - ApplyLinVelAnimation(velocity, deltaTime); - } - else - { - deltaTime = AZStd::min(deltaTime, 0.1f); // this affects applied force. Need to prevent value that is too large. - if (m_linear) - { - ApplyLinVelRigidBodyImpulse(velocity, deltaTime); - } - } - } - - void MotorizedJointComponent::ApplyLinVelAnimation(float velocity, float deltaTime) - { - AZ::Transform transform; - AZ::TransformBus::EventResult(transform, this->GetEntityId(), &AZ::TransformBus::Events::GetLocalTM); - transform.SetTranslation(transform.GetTranslation() + velocity * m_jointDir * deltaTime); - AZ::TransformBus::Event(this->GetEntityId(), &AZ::TransformBus::Events::SetLocalTM, transform); - } - - void MotorizedJointComponent::ApplyLinVelRigidBodyImpulse(float velocity, float deltaTime) - { - AZ::Quaternion transform; - AZ::TransformBus::EventResult(transform, this->GetEntityId(), &AZ::TransformBus::Events::GetWorldRotationQuaternion); - auto force_impulse = transform.TransformVector(m_effortAxis * velocity); - Physics::RigidBodyRequestBus::Event( - this->GetEntityId(), &Physics::RigidBodyRequests::ApplyLinearImpulse, force_impulse * deltaTime); - } - - void MotorizedJointComponent::ApplyLinVelRigidBody(float velocity, float deltaTime) - { - AZ::Quaternion transform; - AZ::TransformBus::EventResult(transform, this->GetEntityId(), &AZ::TransformBus::Events::GetWorldRotationQuaternion); - AZ::Vector3 currentVelocity; - auto transformed_velocity_increment = transform.TransformVector(m_effortAxis * velocity); - Physics::RigidBodyRequestBus::EventResult(currentVelocity, this->GetEntityId(), &Physics::RigidBodyRequests::GetLinearVelocity); - AZ::Vector3 new_velocity = currentVelocity + transformed_velocity_increment; - Physics::RigidBodyRequestBus::Event(this->GetEntityId(), &Physics::RigidBodyRequests::SetLinearVelocity, new_velocity); - } - - void MotorizedJointComponent::SetSetpoint(float setpoint) - { - m_setpoint = setpoint; - } - - float MotorizedJointComponent::GetSetpoint() - { - return m_setpoint; - } - - float MotorizedJointComponent::GetError() - { - return m_error; - } - - float MotorizedJointComponent::GetCurrentMeasurement() - { - return m_currentPosition - m_zeroOffset; - } - -} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Odometry/ROS2OdometryCovariance.cpp b/Gems/ROS2/Code/Source/Odometry/ROS2OdometryCovariance.cpp new file mode 100644 index 000000000..0a0c4df54 --- /dev/null +++ b/Gems/ROS2/Code/Source/Odometry/ROS2OdometryCovariance.cpp @@ -0,0 +1,61 @@ +/* + * 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 "ROS2OdometryCovariance.h" +#include +#include +#include +#include +#include + +namespace ROS2 +{ + void ROS2OdometryCovariance::Reflect(AZ::ReflectContext* context) + { + if (AZ::SerializeContext* serialize = azrtti_cast(context)) + { + serialize->Class() + ->Version(1) + ->Field("Linear covariance", &ROS2OdometryCovariance::m_linearCovariance) + ->Field("Angular covariance", &ROS2OdometryCovariance::m_angularCovariance); + + if (AZ::EditContext* ec = serialize->GetEditContext()) + { + ec->Class("ROS2 Odometry Covariance", "Define Odometry covariance") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &ROS2OdometryCovariance::m_linearCovariance, + "Linear covariance", + "Set the ROS linear covariance") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &ROS2OdometryCovariance::m_angularCovariance, + "Angular covariance", + "Set the ROS angular covariance"); + } + } + } + + std::array ROS2OdometryCovariance::GetRosCovariance() const + { + const AZ::Vector3& lin = m_linearCovariance; + const AZ::Vector3& ang = m_angularCovariance; + + // clang-format off + return { + lin.GetX(), 0.0L, 0.0L, 0.0L, 0.0L, 0.0L, + 0.0L, lin.GetY(), 0.0L, 0.0L, 0.0L, 0.0L, + 0.0L, 0.0L, lin.GetZ(), 0.0L, 0.0L, 0.0L, + 0.0L, 0.0L, 0.0L, ang.GetX(), 0.0L, 0.0L, + 0.0L, 0.0L, 0.0L, 0.0L, ang.GetY(), 0.0L, + 0.0L, 0.0L, 0.0L, 0.0L, 0.0L, ang.GetZ() + }; + // clang-format on + } + +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Odometry/ROS2OdometryCovariance.h b/Gems/ROS2/Code/Source/Odometry/ROS2OdometryCovariance.h new file mode 100644 index 000000000..946fad440 --- /dev/null +++ b/Gems/ROS2/Code/Source/Odometry/ROS2OdometryCovariance.h @@ -0,0 +1,29 @@ +/* + * 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 +#include +#include +#include +#include + +namespace ROS2 +{ + struct ROS2OdometryCovariance + { + AZ_TYPE_INFO(ROS2OdometryCovariance, "{db015832-f621-11ed-b67e-0242ac120002}"); + + static void Reflect(AZ::ReflectContext* context); + + std::array GetRosCovariance() const; + + AZ::Vector3 m_linearCovariance = AZ::Vector3::CreateZero(); + AZ::Vector3 m_angularCovariance = AZ::Vector3::CreateZero(); + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Odometry/ROS2OdometrySensorComponent.cpp b/Gems/ROS2/Code/Source/Odometry/ROS2OdometrySensorComponent.cpp index 5e0e701e7..86897025a 100644 --- a/Gems/ROS2/Code/Source/Odometry/ROS2OdometrySensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Odometry/ROS2OdometrySensorComponent.cpp @@ -6,43 +6,43 @@ * */ -#include "ROS2OdometrySensorComponent.h" -#include -#include +#include #include #include -#include -#include +#include "ROS2OdometrySensorComponent.h" #include #include namespace ROS2 { - namespace Internal + namespace { - const char* kOdometryMsgType = "nav_msgs::msg::Odometry"; + const char* OdometryMsgType = "nav_msgs::msg::Odometry"; } void ROS2OdometrySensorComponent::Reflect(AZ::ReflectContext* context) { - if (AZ::SerializeContext* serialize = azrtti_cast(context)) + if (auto* serialize = azrtti_cast(context)) { - serialize->Class()->Version(1); + serialize->Class()->Version(2); - if (AZ::EditContext* ec = serialize->GetEditContext()) + if (auto* editContext = serialize->GetEditContext()) { - ec->Class("ROS2 Odometry Sensor", "Odometry sensor component") + editContext->Class("ROS2 Odometry Sensor", "Odometry sensor component") ->ClassElement(AZ::Edit::ClassElements::EditorData, "") ->Attribute(AZ::Edit::Attributes::Category, "ROS2") - ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")); + ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/ROS2OdometrySensor.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/ROS2OdometrySensor.svg"); } } } ROS2OdometrySensorComponent::ROS2OdometrySensorComponent() + : m_initialTransform(AZ::Transform::CreateIdentity()) { TopicConfiguration tc; - const AZStd::string type = Internal::kOdometryMsgType; + const AZStd::string type = OdometryMsgType; tc.m_type = type; tc.m_topic = "odom"; m_sensorConfiguration.m_frequency = 10; @@ -51,30 +51,35 @@ 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() + void ROS2OdometrySensorComponent::OnOdometryEvent(AzPhysics::SceneHandle sceneHandle) { - InstallPhysicalCallback(m_entity->GetId()); - } + if (m_bodyHandle == AzPhysics::InvalidSimulatedBodyHandle) + { + 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()); - void ROS2OdometrySensorComponent::OnPhysicsInitialization(AzPhysics::SceneHandle sceneHandle) - { - auto* sceneInterface = AZ::Interface::Get(); - auto* simulatedBodyPtr = sceneInterface->GetSimulatedBodyFromHandle(sceneHandle, m_bodyHandle); - auto rigidbodyPtr = azrtti_cast(simulatedBodyPtr); - AZ_Assert(rigidbodyPtr, "Requested simulated body is not a rigid body"); - m_initialTransform = rigidbodyPtr->GetTransform(); - } + m_bodyHandle = rigidBody->m_bodyHandle; + + auto* sceneInterface = AZ::Interface::Get(); + AZ_Assert(sceneInterface, "Requested scene interface is missing"); + + auto* simulatedBodyPtr = sceneInterface->GetSimulatedBodyFromHandle(sceneHandle, m_bodyHandle); + auto* rigidbodyPtr = azrtti_cast(simulatedBodyPtr); + AZ_Assert(rigidbodyPtr, "Requested simulated body is not a rigid body"); + m_initialTransform = rigidbodyPtr->GetTransform(); + } - void ROS2OdometrySensorComponent::OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime) - { auto* sceneInterface = AZ::Interface::Get(); + AZ_Assert(sceneInterface, "Requested scene interface is missing"); + auto* simulatedBodyPtr = sceneInterface->GetSimulatedBodyFromHandle(sceneHandle, m_bodyHandle); - auto rigidbodyPtr = azrtti_cast(simulatedBodyPtr); + auto* rigidbodyPtr = azrtti_cast(simulatedBodyPtr); AZ_Assert(rigidbodyPtr, "Requested simulated body is not a rigid body"); const auto transform = rigidbodyPtr->GetTransform().GetInverse(); @@ -87,11 +92,8 @@ namespace ROS2 const auto odometry = m_initialTransform.GetInverse() * rigidbodyPtr->GetTransform(); - if (IsPublicationDeadline(deltaTime)) - { - m_odometryMsg.pose.pose = ROS2Conversions::ToROS2Pose(odometry); - m_odometryPublisher->publish(m_odometryMsg); - } + m_odometryMsg.pose.pose = ROS2Conversions::ToROS2Pose(odometry); + m_odometryPublisher->publish(m_odometryMsg); } void ROS2OdometrySensorComponent::Activate() { @@ -101,16 +103,25 @@ namespace ROS2 auto ros2Node = ROS2Interface::Get()->GetNode(); AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for Odometry sensor"); - const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kOdometryMsgType]; + const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[OdometryMsgType]; const auto fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic); m_odometryPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig.GetQoS()); - ROS2SensorComponent::Activate(); + StartSensor( + m_sensorConfiguration.m_frequency, + [this]([[maybe_unused]] float odomDeltaTime, AzPhysics::SceneHandle sceneHandle, [[maybe_unused]] float physicsDeltaTime) + { + if (!m_sensorConfiguration.m_publishingEnabled) + { + return; + } + OnOdometryEvent(sceneHandle); + }); } void ROS2OdometrySensorComponent::Deactivate() { - ROS2SensorComponent::Deactivate(); + StopSensor(); m_odometryPublisher.reset(); } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Odometry/ROS2OdometrySensorComponent.h b/Gems/ROS2/Code/Source/Odometry/ROS2OdometrySensorComponent.h index 4f933a033..27ca208c4 100644 --- a/Gems/ROS2/Code/Source/Odometry/ROS2OdometrySensorComponent.h +++ b/Gems/ROS2/Code/Source/Odometry/ROS2OdometrySensorComponent.h @@ -10,14 +10,10 @@ #include #include #include -#include -#include -#include -#include -#include -#include #include #include +#include +#include namespace ROS2 { @@ -26,11 +22,10 @@ namespace ROS2 //! This is a ground truth "sensor", which can be helpful for development and machine learning. //! @see nav_msgs package. class ROS2OdometrySensorComponent - : public ROS2SensorComponent - , public ROS2::Utils::PhysicsCallbackHandler + : public ROS2SensorComponentBase { public: - AZ_COMPONENT(ROS2OdometrySensorComponent, "{61387448-63AA-4563-AF87-60C72B05B863}", ROS2SensorComponent); + AZ_COMPONENT(ROS2OdometrySensorComponent, "{61387448-63AA-4563-AF87-60C72B05B863}", SensorBaseType); ROS2OdometrySensorComponent(); ~ROS2OdometrySensorComponent() = default; static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); @@ -42,16 +37,11 @@ namespace ROS2 ////////////////////////////////////////////////////////////////////////// private: + AzPhysics::SimulatedBodyHandle m_bodyHandle = AzPhysics::InvalidSimulatedBodyHandle; std::shared_ptr> m_odometryPublisher; nav_msgs::msg::Odometry m_odometryMsg; AZ::Transform m_initialTransform; - private: - // ROS2SensorComponent overrides ... - void SetupRefreshLoop() override; - - // ROS2::Utils::PhysicsCallbackHandler overrides ... - void OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime) override; - void OnPhysicsInitialization(AzPhysics::SceneHandle sceneHandle) override; + void OnOdometryEvent(AzPhysics::SceneHandle sceneHandle); }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Odometry/ROS2WheelOdometry.cpp b/Gems/ROS2/Code/Source/Odometry/ROS2WheelOdometry.cpp index abb4e0f67..0d81627d4 100644 --- a/Gems/ROS2/Code/Source/Odometry/ROS2WheelOdometry.cpp +++ b/Gems/ROS2/Code/Source/Odometry/ROS2WheelOdometry.cpp @@ -7,34 +7,47 @@ */ #include "ROS2WheelOdometry.h" +#include "Odometry/ROS2OdometryCovariance.h" #include "VehicleModelComponent.h" -#include -#include -#include -#include -#include #include #include namespace ROS2 { - namespace Internal + namespace { - const char* kWheelOdometryMsgType = "nav_msgs::msg::Odometry"; + const char* WheelOdometryMsgType = "nav_msgs::msg::Odometry"; } void ROS2WheelOdometryComponent::Reflect(AZ::ReflectContext* context) { - if (AZ::SerializeContext* serialize = azrtti_cast(context)) + ROS2OdometryCovariance::Reflect(context); + + if (auto* serialize = azrtti_cast(context)) { - serialize->Class()->Version(1); + serialize->Class() + ->Version(2) + ->Field("Twist covariance", &ROS2WheelOdometryComponent::m_twistCovariance) + ->Field("Pose covariance", &ROS2WheelOdometryComponent::m_poseCovariance); - if (AZ::EditContext* ec = serialize->GetEditContext()) + if (auto* editContext = serialize->GetEditContext()) { - ec->Class("ROS2 Wheel Odometry Sensor", "Odometry sensor component") + editContext->Class("ROS2 Wheel Odometry Sensor", "Odometry sensor component") ->ClassElement(AZ::Edit::ClassElements::EditorData, "") ->Attribute(AZ::Edit::Attributes::Category, "ROS2") - ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")); + ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/ROS2WheelOdometrySensor.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/ROS2WheelOdometrySensor.svg") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &ROS2WheelOdometryComponent::m_twistCovariance, + "Twist covariance", + "Set ROS twist covariance") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &ROS2WheelOdometryComponent::m_poseCovariance, + "Pose covariance", + "Set ROS pose covariance"); } } } @@ -42,7 +55,7 @@ namespace ROS2 ROS2WheelOdometryComponent::ROS2WheelOdometryComponent() { TopicConfiguration tc; - const AZStd::string type = Internal::kWheelOdometryMsgType; + const AZStd::string type = WheelOdometryMsgType; tc.m_type = type; tc.m_topic = "odom"; m_sensorConfiguration.m_frequency = 10; @@ -55,12 +68,16 @@ namespace ROS2 required.push_back(AZ_CRC_CE("SkidSteeringModelService")); } - void ROS2WheelOdometryComponent::SetupRefreshLoop() + void ROS2WheelOdometryComponent::OnOdometryEvent() { - InstallPhysicalCallback(m_entity->GetId()); + m_odometryMsg.pose.pose.position = ROS2Conversions::ToROS2Point(m_robotPose); + m_odometryMsg.pose.pose.orientation = ROS2Conversions::ToROS2Quaternion(m_robotRotation); + m_odometryMsg.pose.covariance = m_poseCovariance.GetRosCovariance(); + + m_odometryPublisher->publish(m_odometryMsg); } - void ROS2WheelOdometryComponent::OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime) + void ROS2WheelOdometryComponent::OnPhysicsEvent(float physicsDeltaTime) { AZStd::pair vt; @@ -70,20 +87,15 @@ namespace ROS2 m_odometryMsg.header.stamp = ROS2Interface::Get()->GetROSTimestamp(); m_odometryMsg.twist.twist.linear = ROS2Conversions::ToROS2Vector3(vt.first); m_odometryMsg.twist.twist.angular = ROS2Conversions::ToROS2Vector3(vt.second); + m_odometryMsg.twist.covariance = m_twistCovariance.GetRosCovariance(); + if (m_sensorConfiguration.m_frequency > 0) { - auto updatePos = deltaTime * vt.first; // in meters - auto updateRot = deltaTime * vt.second; // in radians + const auto updatePos = physicsDeltaTime * vt.first; // in meters + const auto updateRot = physicsDeltaTime * vt.second; // in radians m_robotPose += m_robotRotation.TransformVector(updatePos); m_robotRotation *= AZ::Quaternion::CreateFromScaledAxisAngle(updateRot); } - if (IsPublicationDeadline(deltaTime)) - { - m_odometryMsg.pose.pose.position = ROS2Conversions::ToROS2Point(m_robotPose); - m_odometryMsg.pose.pose.orientation = ROS2Conversions::ToROS2Quaternion(m_robotRotation); - - m_odometryPublisher->publish(m_odometryMsg); - } } void ROS2WheelOdometryComponent::Activate() @@ -98,16 +110,29 @@ namespace ROS2 auto ros2Node = ROS2Interface::Get()->GetNode(); AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for Odometry sensor"); - const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kWheelOdometryMsgType]; + const auto& publisherConfig = m_sensorConfiguration.m_publishersConfigurations[WheelOdometryMsgType]; const auto fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic); m_odometryPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig.GetQoS()); - ROS2SensorComponent::Activate(); + + StartSensor( + m_sensorConfiguration.m_frequency, + [this]([[maybe_unused]] auto&&... args) + { + if (!m_sensorConfiguration.m_publishingEnabled) + { + return; + } + OnOdometryEvent(); + }, + [this]([[maybe_unused]] AzPhysics::SceneHandle sceneHandle, float physicsDeltaTime) + { + OnPhysicsEvent(physicsDeltaTime); + }); } void ROS2WheelOdometryComponent::Deactivate() { - RemovePhysicalCallback(); - ROS2SensorComponent::Deactivate(); + StopSensor(); m_odometryPublisher.reset(); } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Odometry/ROS2WheelOdometry.h b/Gems/ROS2/Code/Source/Odometry/ROS2WheelOdometry.h index 512412cfd..55c688b88 100644 --- a/Gems/ROS2/Code/Source/Odometry/ROS2WheelOdometry.h +++ b/Gems/ROS2/Code/Source/Odometry/ROS2WheelOdometry.h @@ -8,14 +8,12 @@ #pragma once #include -#include -#include -#include -#include -#include #include #include -#include +#include +#include + +#include "ROS2OdometryCovariance.h" namespace ROS2 { @@ -24,11 +22,10 @@ namespace ROS2 //! This is a physical sensor that takes a vehicle's configuration and computes updates from the wheels' rotations. //! @see nav_msgs package. class ROS2WheelOdometryComponent - : public ROS2SensorComponent - , public ROS2::Utils::PhysicsCallbackHandler + : public ROS2SensorComponentBase { public: - AZ_COMPONENT(ROS2WheelOdometryComponent, "{9bdb8c23-ac76-4c25-8d35-37aaa9f43fac}", ROS2SensorComponent); + AZ_COMPONENT(ROS2WheelOdometryComponent, "{9bdb8c23-ac76-4c25-8d35-37aaa9f43fac}", SensorBaseType); ROS2WheelOdometryComponent(); ~ROS2WheelOdometryComponent() = default; static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); @@ -44,10 +41,10 @@ namespace ROS2 nav_msgs::msg::Odometry m_odometryMsg; AZ::Vector3 m_robotPose{ 0 }; AZ::Quaternion m_robotRotation{ 0, 0, 0, 1 }; + ROS2OdometryCovariance m_poseCovariance; + ROS2OdometryCovariance m_twistCovariance; - // ROS2SensorComponent overrides ... - void SetupRefreshLoop() override; - // ROS2::Utils::PhysicsCallbackHandler overrides ... - void OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime) override; + void OnOdometryEvent(); + void OnPhysicsEvent(float physicsDeltaTime); }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/ROS2EditorModule.cpp b/Gems/ROS2/Code/Source/ROS2EditorModule.cpp index 3435a4f15..f39f34146 100644 --- a/Gems/ROS2/Code/Source/ROS2EditorModule.cpp +++ b/Gems/ROS2/Code/Source/ROS2EditorModule.cpp @@ -5,11 +5,15 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ +#include "Spawner/ROS2SpawnPointEditorComponent.h" +#include "Spawner/ROS2SpawnerEditorComponent.h" +#include #include +#include #include #include -#include #include +#include #include @@ -30,31 +34,26 @@ namespace ROS2 ROS2EditorModule() { InitROS2Resources(); - - // Push results of [MyComponent]::CreateDescriptor() into m_descriptors here. - // Add ALL components descriptors associated with this gem to m_descriptors. - // This will associate the AzTypeInfo information for the components with the SerializeContext, BehaviorContext and - // EditContext. This happens through the [MyComponent]::Reflect() function. + m_descriptors.insert( m_descriptors.end(), - { - ROS2EditorSystemComponent::CreateDescriptor(), - LidarRegistrarEditorSystemComponent::CreateDescriptor(), - ROS2RobotImporterEditorSystemComponent::CreateDescriptor(), - ROS2CameraSensorEditorComponent::CreateDescriptor(), - }); + { ROS2EditorSystemComponent::CreateDescriptor(), + LidarRegistrarEditorSystemComponent::CreateDescriptor(), + ROS2RobotImporterEditorSystemComponent::CreateDescriptor(), + ROS2CameraSensorEditorComponent::CreateDescriptor(), + ROS2SpawnerEditorComponent::CreateDescriptor(), + ROS2SpawnPointEditorComponent::CreateDescriptor(), + SdfAssetBuilderSystemComponent::CreateDescriptor(), + JointsManipulationEditorComponent::CreateDescriptor() }); } - /** - * Add required SystemComponents to the SystemEntity. - * Non-SystemComponents should not be added here - */ AZ::ComponentTypeList GetRequiredSystemComponents() const override { return AZ::ComponentTypeList{ azrtti_typeid(), azrtti_typeid(), azrtti_typeid(), + azrtti_typeid(), }; } }; diff --git a/Gems/ROS2/Code/Source/ROS2EditorSystemComponent.cpp b/Gems/ROS2/Code/Source/ROS2EditorSystemComponent.cpp index 91efaf3b5..e5f26ac1d 100644 --- a/Gems/ROS2/Code/Source/ROS2EditorSystemComponent.cpp +++ b/Gems/ROS2/Code/Source/ROS2EditorSystemComponent.cpp @@ -5,6 +5,8 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ +#include "ROS2SystemComponent.h" +#include #include #include @@ -46,10 +48,20 @@ namespace ROS2 void ROS2EditorSystemComponent::Activate() { - ROS2SystemComponent::Activate(); + AzToolsFramework::EditorEntityContextNotificationBus::Handler::BusConnect(); + ROS2SystemComponent::InitPassTemplateMappingsHandler(); } void ROS2EditorSystemComponent::Deactivate() + { + AzToolsFramework::EditorEntityContextNotificationBus::Handler::BusDisconnect(); + } + + void ROS2EditorSystemComponent::OnStartPlayInEditorBegin() + { + ROS2SystemComponent::Activate(); + } + void ROS2EditorSystemComponent::OnStopPlayInEditor() { ROS2SystemComponent::Deactivate(); } diff --git a/Gems/ROS2/Code/Source/ROS2EditorSystemComponent.h b/Gems/ROS2/Code/Source/ROS2EditorSystemComponent.h index 1a8226d47..5129b55a1 100644 --- a/Gems/ROS2/Code/Source/ROS2EditorSystemComponent.h +++ b/Gems/ROS2/Code/Source/ROS2EditorSystemComponent.h @@ -14,7 +14,9 @@ namespace ROS2 { /// System component for ROS2 editor - class ROS2EditorSystemComponent : public ROS2SystemComponent + class ROS2EditorSystemComponent + : public ROS2SystemComponent + , private AzToolsFramework::EditorEntityContextNotificationBus::Handler { using BaseSystemComponent = ROS2SystemComponent; @@ -36,5 +38,11 @@ namespace ROS2 void Activate() override; void Deactivate() override; ////////////////////////////////////////////////////////////////////////// + + ////////////////////////////////////////////////////////////////////////// + // EditorEntityContextNotificationBus overrides + void OnStartPlayInEditorBegin() override; + void OnStopPlayInEditor() override; + ////////////////////////////////////////////////////////////////////////// }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/ROS2ModuleInterface.h b/Gems/ROS2/Code/Source/ROS2ModuleInterface.h index 4181f4101..19a3e12cb 100644 --- a/Gems/ROS2/Code/Source/ROS2ModuleInterface.h +++ b/Gems/ROS2/Code/Source/ROS2ModuleInterface.h @@ -11,27 +11,37 @@ #include #include #include +#include #include +#include +#include +#include #include #include -#include #include +#include +#include +#include +#include +#include #include #include #include -#include +#include +#include +#include #include #include #include #include #include +#include #include #include #include #include #include #include - namespace ROS2 { class ROS2ModuleInterface : public AZ::Module @@ -42,37 +52,47 @@ namespace ROS2 ROS2ModuleInterface() { - // Push results of [MyComponent]::CreateDescriptor() into m_descriptors here. - // Add ALL components descriptors associated with this gem to m_descriptors. - // This will associate the AzTypeInfo information for the components with the SerializeContext, BehaviorContext and EditContext. - // This happens through the [MyComponent]::Reflect() function. m_descriptors.insert( m_descriptors.end(), - { ROS2SystemComponent::CreateDescriptor(), - LidarRegistrarSystemComponent::CreateDescriptor(), - ROS2RobotImporterSystemComponent::CreateDescriptor(), - ROS2SensorComponent::CreateDescriptor(), - ROS2ImuSensorComponent::CreateDescriptor(), - ROS2GNSSSensorComponent::CreateDescriptor(), - ROS2LidarSensorComponent::CreateDescriptor(), - ROS2Lidar2DSensorComponent::CreateDescriptor(), - ROS2OdometrySensorComponent::CreateDescriptor(), - ROS2WheelOdometryComponent::CreateDescriptor(), - ROS2FrameComponent::CreateDescriptor(), - ROS2RobotControlComponent::CreateDescriptor(), - ROS2CameraSensorComponent::CreateDescriptor(), - AckermannControlComponent::CreateDescriptor(), - RigidBodyTwistControlComponent::CreateDescriptor(), - SkidSteeringControlComponent::CreateDescriptor(), - ROS2SpawnerComponent::CreateDescriptor(), - ROS2SpawnPointComponent::CreateDescriptor(), - VehicleDynamics::AckermannVehicleModelComponent::CreateDescriptor(), - VehicleDynamics::WheelControllerComponent::CreateDescriptor(), - VehicleDynamics::SkidSteeringModelComponent::CreateDescriptor(), - MotorizedJointComponent::CreateDescriptor() }); + { + ROS2SystemComponent::CreateDescriptor(), + ROS2SensorComponentBase::CreateDescriptor(), + ROS2SensorComponentBase::CreateDescriptor(), + LidarRegistrarSystemComponent::CreateDescriptor(), + ROS2RobotImporterSystemComponent::CreateDescriptor(), + ROS2ImuSensorComponent::CreateDescriptor(), + ROS2GNSSSensorComponent::CreateDescriptor(), + ROS2LidarSensorComponent::CreateDescriptor(), + ROS2Lidar2DSensorComponent::CreateDescriptor(), + ROS2OdometrySensorComponent::CreateDescriptor(), + ROS2WheelOdometryComponent::CreateDescriptor(), + ROS2FrameComponent::CreateDescriptor(), + ROS2RobotControlComponent::CreateDescriptor(), + ROS2CameraSensorComponent::CreateDescriptor(), + AckermannControlComponent::CreateDescriptor(), + RigidBodyTwistControlComponent::CreateDescriptor(), + SkidSteeringControlComponent::CreateDescriptor(), + ROS2CameraSensorComponent::CreateDescriptor(), + ROS2SpawnerComponent::CreateDescriptor(), + ROS2SpawnPointComponent::CreateDescriptor(), + VehicleDynamics::AckermannVehicleModelComponent::CreateDescriptor(), + VehicleDynamics::WheelControllerComponent::CreateDescriptor(), + VehicleDynamics::SkidSteeringModelComponent::CreateDescriptor(), + JointMotorControllerComponent::CreateDescriptor(), + ManualMotorControllerComponent::CreateDescriptor(), + JointsManipulationComponent::CreateDescriptor(), + JointsArticulationControllerComponent::CreateDescriptor(), + JointsPIDControllerComponent::CreateDescriptor(), + JointsTrajectoryComponent::CreateDescriptor(), + PidMotorControllerComponent::CreateDescriptor(), + GripperActionServerComponent::CreateDescriptor(), + VacuumGripperComponent::CreateDescriptor(), + FingerGripperComponent::CreateDescriptor(), + ROS2ContactSensorComponent::CreateDescriptor(), + FollowingCameraComponent::CreateDescriptor(), + }); } - //! Add required SystemComponents to the SystemEntity. AZ::ComponentTypeList GetRequiredSystemComponents() const override { return AZ::ComponentTypeList{ diff --git a/Gems/ROS2/Code/Source/ROS2SystemComponent.cpp b/Gems/ROS2/Code/Source/ROS2SystemComponent.cpp index c276a92c0..69842ef86 100644 --- a/Gems/ROS2/Code/Source/ROS2SystemComponent.cpp +++ b/Gems/ROS2/Code/Source/ROS2SystemComponent.cpp @@ -5,9 +5,12 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ +#include #include +#include #include #include +#include #include #include #include @@ -21,6 +24,7 @@ #include #include #include +#include namespace ROS2 { @@ -31,8 +35,12 @@ namespace ROS2 // Reflect structs not strictly owned by any single component QoS::Reflect(context); TopicConfiguration::Reflect(context); + PublisherConfiguration::Reflect(context); + LidarCore::Reflect(context); + SensorConfiguration::Reflect(context); VehicleDynamics::VehicleModelComponent::Reflect(context); ROS2::Controllers::PidConfiguration::Reflect(context); + if (AZ::SerializeContext* serialize = azrtti_cast(context)) { serialize->Class()->Version(0); @@ -74,7 +82,25 @@ namespace ROS2 { ROS2Interface::Register(this); } + return; + } + + ROS2SystemComponent::~ROS2SystemComponent() + { + if (ROS2Interface::Get() == this) + { + ROS2Interface::Unregister(this); + } + rclcpp::shutdown(); + } + + void ROS2SystemComponent::Init() + { + rclcpp::init(0, 0); + } + void ROS2SystemComponent::InitClock() + { bool useSteadyTime = false; auto* registry = AZ::SettingsRegistry::Get(); AZ_Assert(registry, "No Registry available"); @@ -90,41 +116,38 @@ namespace ROS2 } AZ_Printf("ROS2SystemComponent", "Enabling realtime clock"); m_simulationClock = AZStd::make_unique(); - return; } - ROS2SystemComponent::~ROS2SystemComponent() + void ROS2SystemComponent::InitPassTemplateMappingsHandler() { - if (ROS2Interface::Get() == this) - { - ROS2Interface::Unregister(this); - } - rclcpp::shutdown(); + auto* passSystem = AZ::RPI::PassSystemInterface::Get(); + AZ_Assert(passSystem, "Cannot get the pass system."); + + m_loadTemplatesHandler = AZ::RPI::PassSystemInterface::OnReadyLoadTemplatesEvent::Handler( + [this]() + { + this->LoadPassTemplateMappings(); + }); + passSystem->ConnectEvent(m_loadTemplatesHandler); } - void ROS2SystemComponent::Init() + void ROS2SystemComponent::Activate() { - rclcpp::init(0, 0); + InitClock(); m_simulationClock->Activate(); m_ros2Node = std::make_shared("o3de_ros2_node"); m_executor = AZStd::make_shared(); m_executor->add_node(m_ros2Node); - } - void ROS2SystemComponent::Activate() - { m_staticTFBroadcaster = AZStd::make_unique(m_ros2Node); m_dynamicTFBroadcaster = AZStd::make_unique(m_ros2Node); - auto* passSystem = AZ::RPI::PassSystemInterface::Get(); - AZ_Assert(passSystem, "Cannot get the pass system."); - - m_loadTemplatesHandler = AZ::RPI::PassSystemInterface::OnReadyLoadTemplatesEvent::Handler( - [this]() - { - this->LoadPassTemplateMappings(); - }); - passSystem->ConnectEvent(m_loadTemplatesHandler); + AZ::ApplicationTypeQuery appType; + AZ::ComponentApplicationBus::Broadcast(&AZ::ComponentApplicationBus::Events::QueryApplicationType, appType); + if (appType.IsGame()) + { + InitPassTemplateMappingsHandler(); + } ROS2RequestBus::Handler::BusConnect(); AZ::TickBus::Handler::BusConnect(); @@ -138,6 +161,10 @@ namespace ROS2 m_loadTemplatesHandler.Disconnect(); m_dynamicTFBroadcaster.reset(); m_staticTFBroadcaster.reset(); + m_executor->remove_node(m_ros2Node); + m_executor.reset(); + m_simulationClock.reset(); + m_ros2Node.reset(); } builtin_interfaces::msg::Time ROS2SystemComponent::GetROSTimestamp() const diff --git a/Gems/ROS2/Code/Source/ROS2SystemComponent.h b/Gems/ROS2/Code/Source/ROS2SystemComponent.h index 62e164020..0e9a8009b 100644 --- a/Gems/ROS2/Code/Source/ROS2SystemComponent.h +++ b/Gems/ROS2/Code/Source/ROS2SystemComponent.h @@ -66,9 +66,11 @@ namespace ROS2 const SimulationClock& GetSimulationClock() const override; ////////////////////////////////////////////////////////////////////////// + void InitPassTemplateMappingsHandler(); + protected: //////////////////////////////////////////////////////////////////////// - // AZ::Component interface implementation + // AZ::Component override void Init() override; void Activate() override; void Deactivate() override; @@ -79,6 +81,8 @@ namespace ROS2 void OnTick(float deltaTime, AZ::ScriptTimePoint time) override; //////////////////////////////////////////////////////////////////////// private: + void InitClock(); + std::shared_ptr m_ros2Node; AZStd::shared_ptr m_executor; AZStd::unique_ptr m_dynamicTFBroadcaster; diff --git a/Gems/ROS2/Code/Source/RobotControl/Controllers/AckermannController/AckermannControlComponent.cpp b/Gems/ROS2/Code/Source/RobotControl/Controllers/AckermannController/AckermannControlComponent.cpp index 1c5d79d34..337cf1e33 100644 --- a/Gems/ROS2/Code/Source/RobotControl/Controllers/AckermannController/AckermannControlComponent.cpp +++ b/Gems/ROS2/Code/Source/RobotControl/Controllers/AckermannController/AckermannControlComponent.cpp @@ -24,7 +24,9 @@ namespace ROS2 ec->Class("Ackermann Control", "Relays Ackermann commands to vehicle inputs") ->ClassElement(AZ::Edit::ClassElements::EditorData, "") ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) - ->Attribute(AZ::Edit::Attributes::Category, "ROS2"); + ->Attribute(AZ::Edit::Attributes::Category, "ROS2") + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/AckermannControl.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/AckermannControl.svg"); } } } diff --git a/Gems/ROS2/Code/Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.cpp b/Gems/ROS2/Code/Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.cpp index bfdaebe0f..0ca735199 100644 --- a/Gems/ROS2/Code/Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.cpp +++ b/Gems/ROS2/Code/Source/RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.cpp @@ -25,7 +25,9 @@ namespace ROS2 ec->Class("Rigid Body Twist Control", "Simple control through RigidBody") ->ClassElement(AZ::Edit::ClassElements::EditorData, "") ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) - ->Attribute(AZ::Edit::Attributes::Category, "ROS2"); + ->Attribute(AZ::Edit::Attributes::Category, "ROS2") + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/RigidBodyTwistControl.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/RigidBodyTwistControl.svg"); } } } diff --git a/Gems/ROS2/Code/Source/RobotControl/Controllers/SkidSteeringController/SkidSteeringControlComponent.cpp b/Gems/ROS2/Code/Source/RobotControl/Controllers/SkidSteeringController/SkidSteeringControlComponent.cpp index 8e6262819..7628c3e75 100644 --- a/Gems/ROS2/Code/Source/RobotControl/Controllers/SkidSteeringController/SkidSteeringControlComponent.cpp +++ b/Gems/ROS2/Code/Source/RobotControl/Controllers/SkidSteeringController/SkidSteeringControlComponent.cpp @@ -25,10 +25,12 @@ namespace ROS2 serialize->Class()->Version(1); if (AZ::EditContext* editorContext = serialize->GetEditContext()) { - editorContext->Class("Skid steering Twist Control", "") + editorContext->Class("Skid Steering Twist Control", "") ->ClassElement(AZ::Edit::ClassElements::EditorData, "") ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) - ->Attribute(AZ::Edit::Attributes::Category, "ROS2"); + ->Attribute(AZ::Edit::Attributes::Category, "ROS2") + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/SkidSteeringTwistControl.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/SkidSteeringTwistControl.svg"); } } } diff --git a/Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.cpp b/Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.cpp index 719a9dbee..90b77974e 100644 --- a/Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.cpp +++ b/Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.cpp @@ -61,10 +61,12 @@ namespace ROS2 if (AZ::EditContext* ec = serialize->GetEditContext()) { - ec->Class("ROS2 Robot control", "Customizable robot control component") + ec->Class("ROS2 Robot Control", "Customizable robot control component") ->ClassElement(AZ::Edit::ClassElements::EditorData, "") ->Attribute(AZ::Edit::Attributes::Category, "ROS2") ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game")) + ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/ROS2RobotControl.svg") + ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/ROS2RobotControl.svg") ->DataElement( AZ::Edit::UIHandlers::Default, &ROS2RobotControlComponent::m_controlConfiguration, @@ -89,7 +91,7 @@ namespace ROS2 return m_controlConfiguration; } - const TopicConfiguration& ROS2RobotControlComponent::GetSubscriberConfigration() const + const TopicConfiguration& ROS2RobotControlComponent::GetSubscriberConfiguration() const { return m_subscriberConfiguration; } diff --git a/Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.h b/Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.h index 149daa312..48975441a 100644 --- a/Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.h +++ b/Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.h @@ -32,7 +32,7 @@ namespace ROS2 void SetControlConfiguration(const ControlConfiguration& controlConfiguration); - const TopicConfiguration& GetSubscriberConfigration() const; + const TopicConfiguration& GetSubscriberConfiguration() const; void SetSubscriberConfiguration(const TopicConfiguration& subscriberConfiguration); diff --git a/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.cpp b/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.cpp new file mode 100644 index 000000000..749dae5db --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.cpp @@ -0,0 +1,181 @@ +/* + * 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 "FixURDF.h" +#include "URDFModifications.h" +#include +#include +#include +#include + +namespace ROS2::Utils +{ + //! Modifies a parsed URDF in memory to add missing inertia to links, which prevents SDF error 19. + //! @param urdf URDF to modify. + //! @returns a list of names of links that were modified. + AZStd::pair, AZStd::vector> AddMissingInertiaToLinks(AZ::rapidxml::xml_node<>* urdf) + { + AZStd::vector missingInertias; + AZStd::vector incompleteInertias; + using namespace AZ::rapidxml; + + for (xml_node<>* link = urdf->first_node("link"); link; link = link->next_sibling("link")) + { + bool modified = false; + bool inertialPresent = true; + + auto name_xml = link->first_attribute("name"); + AZStd::string name = "unknown_link"; + if (name_xml) + { + name = name_xml->value(); + } + + auto inertial = link->first_node("inertial"); + if (!inertial) + { + AZ_Warning("URDF", false, "Missing inertial tag in link %s, applying default tag.", name.c_str()); + inertial = urdf->document()->allocate_node(node_element, "inertial"); + link->append_node(inertial); + modified = true; + inertialPresent = false; + + MissingInertia missingInertia; + missingInertia.linkName = name; + missingInertias.push_back(missingInertia); + } + + IncompleteInertia incompleteInertia; + incompleteInertia.linkName = name; + + auto mass = inertial->first_node("mass"); + if (!mass) + { + AZ_Warning( + "URDF", !inertialPresent, "Missing mass tag inside the inertial tag link %s, applying default mass tag.", name.c_str()); + mass = urdf->document()->allocate_node(node_element, "mass"); + mass->append_attribute(urdf->document()->allocate_attribute("value", "1.")); + inertial->append_node(mass); + modified = true; + incompleteInertia.missingTags.push_back("mass"); + } + + auto inertia = inertial->first_node("inertia"); + if (!inertia) + { + AZ_Warning( + "URDF", + !inertialPresent, + "Missing inertia tag inside the inertial tag link %s, applying default inertia tag.", + name.c_str()); + inertia = urdf->document()->allocate_node(node_element, "inertia"); + + inertia->append_attribute(urdf->document()->allocate_attribute("ixx", "1.")); + inertia->append_attribute(urdf->document()->allocate_attribute("ixy", "0.")); + inertia->append_attribute(urdf->document()->allocate_attribute("ixz", "0.")); + + inertia->append_attribute(urdf->document()->allocate_attribute("iyx", "0.")); + inertia->append_attribute(urdf->document()->allocate_attribute("iyy", "1.")); + inertia->append_attribute(urdf->document()->allocate_attribute("iyz", "0.")); + + inertia->append_attribute(urdf->document()->allocate_attribute("izx", "0.")); + inertia->append_attribute(urdf->document()->allocate_attribute("izy", "0.")); + inertia->append_attribute(urdf->document()->allocate_attribute("izz", "1.")); + + inertial->append_node(inertia); + modified = true; + incompleteInertia.missingTags.push_back("inertia"); + } + + if (modified && inertialPresent) + { + incompleteInertias.push_back(incompleteInertia); + } + } + + return { missingInertias, incompleteInertias }; + } + + //! Handles a case of multiple joints and the link sharing a common names which causes SDF error2 (but is fine in URDF) + //! Function will add a suffix "_dup" to the name of the joint if it is also the name of a link. + //! If there are name collisions in links, this will not be able to fix it, the SDF parser will throw an error. + //! @param urdf URDF to modify. + //! @returns a list of links that were modified + AZStd::vector RenameDuplicatedJoints(AZ::rapidxml::xml_node<>* urdf) + { + using namespace AZ::rapidxml; + AZStd::vector duplicatedJoints; + AZStd::unordered_map linkAndJointsName; + for (xml_node<>* link = urdf->first_node("link"); link; link = link->next_sibling("link")) + { + auto* name = link->first_attribute("name"); + if (name) + { + linkAndJointsName.insert(AZStd::make_pair(name->value(), 0)); + } + } + for (xml_node<>* joint = urdf->first_node("joint"); joint; joint = joint->next_sibling("joint")) + { + auto* name = joint->first_attribute("name"); + if (name) + { + AZStd::string name_value = name->value(); + + if (linkAndJointsName.contains(name_value)) + { + AZStd::string name_value = name->value(); + + unsigned int& count = linkAndJointsName[name->value()]; + auto newName = AZStd::string::format("%s_dup%u", name_value.c_str(), count); + name->value(urdf->document()->allocate_string(newName.c_str())); + count++; + + DuplicatedJoint duplication; + duplication.newName = newName; + duplication.oldName = name_value; + duplicatedJoints.push_back(duplication); + } + else + { + linkAndJointsName.insert(AZStd::make_pair(name_value, 0)); + } + } + } + return duplicatedJoints; + } + + AZStd::pair ModifyURDFInMemory(const AZStd::string& data) + { + UrdfModifications modifiedElements; + using namespace AZ::rapidxml; + xml_document<> doc; + doc.parse<0>(const_cast(data.c_str())); + xml_node<>* urdf = doc.first_node("robot"); + if (!urdf) + { + AZ_Warning("URDF", false, "No robot tag in URDF"); + return { data.c_str(), modifiedElements }; + } + auto [missing, incomplete] = AddMissingInertiaToLinks(urdf); + + modifiedElements.missingInertias = missing; + modifiedElements.incompleteInertias = incomplete; + + modifiedElements.duplicatedJoints = RenameDuplicatedJoints(urdf); + + std::string xmlDocString; + AZ::rapidxml::print(std::back_inserter(xmlDocString), *urdf, 0); + return { xmlDocString, modifiedElements }; + } + + AZStd::pair ModifyURDFInMemory(const std::string& data) + { + return ModifyURDFInMemory(AZStd::string(data.c_str())); + } + +} // namespace ROS2::Utils diff --git a/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.h b/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.h new file mode 100644 index 000000000..d33f062cc --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.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 "URDFModifications.h" +#include +#include + +namespace ROS2::Utils +{ + //! Modifies in memory URDF to increase chance of successful conversion to SDF. + //! It does the following: + //! - Adds missing inertia to links of mass 1 kg and identity inertia matrix. + //! - Renames joints that have the same name as a link. + //! @param urdf URDF to modify. + //! @returns a modified URDF and a list of XML element that were modified + AZStd::pair ModifyURDFInMemory(const std::string& data); + AZStd::pair ModifyURDFInMemory(const AZStd::string& data); + +} // namespace ROS2::Utils diff --git a/Gems/ROS2/Code/Source/RobotImporter/FixURDF/URDFModifications.h b/Gems/ROS2/Code/Source/RobotImporter/FixURDF/URDFModifications.h new file mode 100644 index 000000000..579cd70f8 --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/FixURDF/URDFModifications.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 +#include + +namespace ROS2::Utils +{ + struct MissingInertia + { + AZStd::string linkName; + + ~MissingInertia() = default; + }; + + struct IncompleteInertia + { + AZStd::string linkName; + AZStd::vector missingTags; + }; + + struct DuplicatedJoint + { + AZStd::string oldName; + AZStd::string newName; + }; + + struct UrdfModifications + { + AZStd::vector missingInertias; + AZStd::vector incompleteInertias; + AZStd::vector duplicatedJoints; + }; + +} // namespace ROS2::Utils diff --git a/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckAssetPage.cpp b/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckAssetPage.cpp index 3361ad7ce..38c6b4a79 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckAssetPage.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckAssetPage.cpp @@ -17,7 +17,7 @@ namespace ROS2 { namespace Columns { - constexpr int UrdfMeshPath{ 0 }; + constexpr int SdfMeshPath{ 0 }; constexpr int ResolvedMeshPath{ 1 }; constexpr int SourceAsset{ 3 }; constexpr int ProductAsset{ 2 }; @@ -48,10 +48,10 @@ namespace ROS2 m_table->setSelectionBehavior(QAbstractItemView::SelectRows); m_table->setSelectionMode(QAbstractItemView::SingleSelection); // Set the header items. - QTableWidgetItem* headerItem = new QTableWidgetItem(tr("URDF mesh path")); + QTableWidgetItem* headerItem = new QTableWidgetItem(tr("URDF/SDF asset path")); headerItem->setTextAlignment(Qt::AlignVCenter | Qt::AlignLeft); - m_table->setHorizontalHeaderItem(Columns::UrdfMeshPath, headerItem); - headerItem = new QTableWidgetItem(tr("Resolved mesh from URDF")); + m_table->setHorizontalHeaderItem(Columns::SdfMeshPath, headerItem); + headerItem = new QTableWidgetItem(tr("Resolved asset from URDF/SDF")); headerItem->setTextAlignment(Qt::AlignVCenter | Qt::AlignLeft); m_table->setHorizontalHeaderItem(Columns::ResolvedMeshPath, headerItem); headerItem = new QTableWidgetItem(tr("Type")); @@ -63,7 +63,7 @@ namespace ROS2 headerItem = new QTableWidgetItem(tr("Product asset")); headerItem->setTextAlignment(Qt::AlignVCenter | Qt::AlignLeft); m_table->setHorizontalHeaderItem(Columns::ProductAsset, headerItem); - m_table->horizontalHeader()->resizeSection(Columns::UrdfMeshPath, 200); + m_table->horizontalHeader()->resizeSection(Columns::SdfMeshPath, 200); m_table->horizontalHeader()->resizeSection(Columns::ResolvedMeshPath, 350); m_table->horizontalHeader()->resizeSection(Columns::Type, 50); m_table->horizontalHeader()->resizeSection(Columns::SourceAsset, 400); @@ -81,11 +81,11 @@ namespace ROS2 { if (m_missingCount == 0) { - setTitle(tr("Resolved meshes")); + setTitle(tr("Resolved assets")); } else { - setTitle(tr("There are ") + QString::number(m_missingCount) + tr(" unresolved meshes")); + setTitle(tr("There are ") + QString::number(m_missingCount) + tr(" unresolved assets")); } } @@ -96,43 +96,71 @@ namespace ROS2 void CheckAssetPage::ReportAsset( const AZ::Uuid assetUuid, - const AZStd::string urdfPath, + const AZStd::string sdfPath, const QString& type, - const AZStd::string assetSourcePath, - const AZ::Crc32& crc32, - const AZStd::string resolvedUrdfPath) + const AZStd::optional& assetSourcePath, + const AZStd::optional& crc32, + const AZStd::optional& resolvedSdfPath) { - int i = m_table->rowCount(); - m_table->setRowCount(i + 1); + int rowId = m_table->rowCount(); + m_table->setRowCount(rowId + 1); - bool isOk = !assetSourcePath.empty(); + // The Asset ID GUID must not be null(all zeros) and the asset source path must not be empty + bool isOk = (assetSourcePath.has_value() && !assetSourcePath->empty() && assetSourcePath != "not found") + && (resolvedSdfPath.has_value() && !resolvedSdfPath->empty()) && !assetUuid.IsNull(); if (!isOk) { m_missingCount++; } SetTitle(); - AZStd::string crcStr = AZStd::to_string(crc32); - QTableWidgetItem* p = createCell(isOk, QString::fromUtf8(urdfPath.data(), urdfPath.size())); + AZStd::string crcStr; + if (crc32) + { + crcStr = AZStd::to_string(*crc32); + } + QTableWidgetItem* p = createCell(isOk, QString::fromUtf8(sdfPath.data(), sdfPath.size())); if (crc32 != AZ::Crc32()) { p->setToolTip(tr("CRC for file : ") + QString::fromUtf8(crcStr.data(), crcStr.size())); } - m_table->setItem(i, Columns::UrdfMeshPath, p); - m_table->setItem( - i, Columns::ResolvedMeshPath, createCell(isOk, QString::fromUtf8(resolvedUrdfPath.data(), resolvedUrdfPath.size()))); - m_table->setItem(i, Columns::Type, createCell(isOk, type)); - m_table->setItem(i, Columns::SourceAsset, createCell(isOk, QString::fromUtf8(assetSourcePath.data(), assetSourcePath.size()))); + m_table->setItem(rowId, Columns::SdfMeshPath, p); + + if (resolvedSdfPath) + { + m_table->setItem( + rowId, Columns::ResolvedMeshPath, createCell(true, QString::fromUtf8(resolvedSdfPath->data(), resolvedSdfPath->size()))); + } + else + { + m_table->setItem(rowId, Columns::ResolvedMeshPath, createCell(false, tr("Not found"))); + } + + m_table->setItem(rowId, Columns::Type, createCell(isOk, type)); + + if (assetSourcePath && !assetSourcePath->empty()) + { + m_table->setItem( + rowId, Columns::SourceAsset, createCell(true, QString::fromUtf8(assetSourcePath->data(), assetSourcePath->size()))); + m_assetsPaths[assetUuid] = *assetSourcePath; + } + else + { + m_table->setItem(rowId, Columns::SourceAsset, createCell(false, tr("Not found"))); + } + if (isOk) { - m_table->item(i, Columns::ResolvedMeshPath)->setIcon(m_okIcon); + m_table->item(rowId, Columns::ResolvedMeshPath)->setIcon(m_okIcon); } else { - m_table->item(i, Columns::ResolvedMeshPath)->setIcon(m_failureIcon); - m_table->setItem(i, Columns::ProductAsset, createCell(false, QString())); + m_table->item(rowId, Columns::ResolvedMeshPath)->setIcon(m_failureIcon); + m_table->setItem(rowId, Columns::ProductAsset, createCell(false, QString())); + } + if (isOk) + { + m_assetsUuidsToColumnIndex[assetUuid] = rowId; } - m_assetsPaths.push_back(assetSourcePath); - m_assetsUuids.push_back(assetUuid); } void CheckAssetPage::StartWatchAsset() @@ -154,7 +182,7 @@ namespace ROS2 void CheckAssetPage::ClearAssetsList() { - m_assetsUuids.clear(); + m_assetsUuidsToColumnIndex.clear(); m_assetsUuidsFinished.clear(); m_assetsPaths.clear(); m_table->setRowCount(0); @@ -165,27 +193,30 @@ namespace ROS2 bool CheckAssetPage::IsEmpty() const { - return m_assetsUuids.empty(); + return m_assetsUuidsToColumnIndex.empty(); } - void CheckAssetPage::DoubleClickRow(int row, int col) + void CheckAssetPage::DoubleClickRow(int row, [[maybe_unused]] int col) { - AZ_Printf("CheckAssetPage", "Clicked on row", row); - if (row < m_assetsPaths.size()) + for (const auto& [assetUuid, columnId] : m_assetsUuidsToColumnIndex) { - AzFramework::AssetSystemRequestBus::Broadcast( - &AzFramework::AssetSystem::AssetSystemRequests::ShowInAssetProcessor, m_assetsPaths[row]); + if (columnId == row && m_assetsPaths.contains(assetUuid)) + { + AzFramework::AssetSystemRequestBus::Broadcast( + &AzFramework::AssetSystem::AssetSystemRequests::ShowInAssetProcessor, m_assetsPaths[assetUuid]); + } + } } void CheckAssetPage::RefreshTimerElapsed() { - for (int i = 0; i < m_assetsUuids.size(); i++) + for (const auto& [assetUuid, rowId] : m_assetsUuidsToColumnIndex) { - const AZ::Uuid& assetUuid = m_assetsUuids[i]; - const AZStd::string& sourceAssetFullPath = m_assetsPaths[i]; - if (!m_assetsUuidsFinished.contains(assetUuid)) + if (m_assetsPaths.contains(assetUuid) && !m_assetsUuidsFinished.contains(assetUuid)) { + // Execute for all found source assets that are not finished yet. + const AZStd::string& sourceAssetFullPath = m_assetsPaths[assetUuid]; using namespace AzToolsFramework; using namespace AzToolsFramework::AssetSystem; @@ -195,7 +226,7 @@ namespace ROS2 if (result) { bool allFinished = true; - bool failed = false; + bool productAssetFailed = false; JobInfoContainer& allJobs = result.GetValue(); for (const JobInfo& job : allJobs) { @@ -205,42 +236,45 @@ namespace ROS2 } if (job.m_status == JobStatus::Failed) { - failed = true; - m_failedCount++; + productAssetFailed = true; } } if (allFinished) { - if (!failed) + if (!productAssetFailed) { - const AZStd::string productRelPathVisual = Utils::GetModelProductAsset(assetUuid); - const AZStd::string productRelPathCollider = Utils::GetPhysXMeshProductAsset(assetUuid); - QString text = QString::fromUtf8(productRelPathVisual.data(), productRelPathVisual.size()) + " " + - QString::fromUtf8(productRelPathCollider.data(), productRelPathCollider.size()); - m_table->setItem(i, Columns::ProductAsset, createCell(true, text)); - m_table->item(i, Columns::ProductAsset)->setIcon(m_okIcon); + const AZStd::vector productPaths = Utils::GetProductAssets(assetUuid); + QString text; + for (const auto& productPath : productPaths) + { + text += QString::fromUtf8(productPath.data(), productPath.size()) + " "; + } + m_table->setItem(rowId, Columns::ProductAsset, createCell(true, text)); + m_table->item(rowId, Columns::ProductAsset)->setIcon(m_okIcon); } else { - m_table->setItem(i, Columns::ProductAsset, createCell(false, tr("Failed"))); - m_table->item(i, Columns::ProductAsset)->setIcon(m_failureIcon); + m_table->setItem(rowId, Columns::ProductAsset, createCell(false, tr("Failed"))); + m_table->item(rowId, Columns::ProductAsset)->setIcon(m_failureIcon); + m_failedCount++; } m_assetsUuidsFinished.insert(assetUuid); } } } } - if (m_assetsUuidsFinished.size() == m_assetsUuids.size()) + + if (m_assetsUuidsFinished.size() == m_assetsUuidsToColumnIndex.size()) { m_refreshTimer->stop(); if (m_failedCount == 0 && m_missingCount == 0) { - setTitle(tr("All meshes were processed")); + setTitle(tr("All assets were processed")); } else { setTitle( - tr("There are ") + QString::number(m_missingCount) + tr(" unresolved meshes.") + tr("There are ") + + tr("There are ") + QString::number(m_missingCount) + tr(" unresolved assets.") + tr("There are ") + QString::number(m_failedCount) + tr(" failed asset processor jobs.")); } } diff --git a/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckAssetPage.h b/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckAssetPage.h index d63566911..43f76e85b 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckAssetPage.h +++ b/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckAssetPage.h @@ -34,11 +34,11 @@ namespace ROS2 //! Function reports assets that are will be processed by asset processor. void ReportAsset( const AZ::Uuid assetUuid, - const AZStd::string urdfPath, + const AZStd::string sdfPath, const QString& type, - const AZStd::string assetSourcePath, - const AZ::Crc32& crc32, - const AZStd::string resolvedUrdfPath); + const AZStd::optional& assetSourcePath, + const AZStd::optional& crc32, + const AZStd::optional& resolvedSdfPath); void ClearAssetsList(); bool IsEmpty() const; bool isComplete() const override; @@ -53,9 +53,9 @@ namespace ROS2 unsigned int m_missingCount{ 0 }; unsigned int m_failedCount{ 0 }; void SetTitle(); - AZStd::vector m_assetsUuids; - AZStd::vector m_assetsPaths; - AZStd::unordered_set m_assetsUuidsFinished; + AZStd::unordered_map m_assetsUuidsToColumnIndex; //!< Map of asset UUIDs to column index in the table. + AZStd::unordered_map m_assetsPaths; //! Map of asset UUIDs to asset source paths. + AZStd::unordered_set m_assetsUuidsFinished; //!< Set of asset UUIDs that have been processed by asset processor. void DoubleClickRow(int row, int col); void RefreshTimerElapsed(); QIcon m_failureIcon; diff --git a/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckUrdfPage.cpp b/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckUrdfPage.cpp index 2c68a830e..8fdc7a645 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckUrdfPage.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckUrdfPage.cpp @@ -14,9 +14,10 @@ namespace ROS2 CheckUrdfPage::CheckUrdfPage(QWizard* parent) : QWizardPage(parent) , m_success(false) + , m_warning(false) { m_log = new QTextEdit(this); - setTitle(tr("URDF opening results:")); + setTitle(tr("URDF/SDF opening results:")); QVBoxLayout* layout = new QVBoxLayout; layout->addWidget(m_log); m_log->acceptRichText(); @@ -24,10 +25,11 @@ namespace ROS2 setLayout(layout); } - void CheckUrdfPage::ReportURDFResult(const QString& status, bool isSuccess) + void CheckUrdfPage::ReportURDFResult(const QString& status, bool isSuccess, bool isWarning) { m_log->setMarkdown(status); m_success = isSuccess; + m_warning = isWarning; emit completeChanged(); } @@ -35,4 +37,9 @@ namespace ROS2 { return m_success; } + + bool CheckUrdfPage::isWarning() const + { + return m_warning; + } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckUrdfPage.h b/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckUrdfPage.h index 7877f4af3..293ffc09e 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckUrdfPage.h +++ b/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckUrdfPage.h @@ -22,12 +22,14 @@ namespace ROS2 Q_OBJECT public: explicit CheckUrdfPage(QWizard* parent); - void ReportURDFResult(const QString& result, bool isSuccess); + void ReportURDFResult(const QString& result, bool isSuccess, bool isWarning = false); bool isComplete() const override; + bool isWarning() const; private: QTextEdit* m_log; QString m_fileName; bool m_success; + bool m_warning; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/Pages/FileSelectionPage.cpp b/Gems/ROS2/Code/Source/RobotImporter/Pages/FileSelectionPage.cpp index d52a9fd14..85ea32b2d 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Pages/FileSelectionPage.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/Pages/FileSelectionPage.cpp @@ -8,32 +8,60 @@ #include "FileSelectionPage.h" #include +#include +#include + +#include #include #include -#include +#include namespace ROS2 { + static constexpr const char FileSelectionPageDefaultFile[] = "RobotImporter/SelectFileDefaultFile"; + FileSelectionPage::FileSelectionPage(QWizard* parent) : QWizardPage(parent) + , m_sdfAssetBuilderSettings(AZStd::make_unique()) { m_fileDialog = new QFileDialog(this); - m_fileDialog->setDirectory(QString::fromUtf8(AZ::Utils::GetProjectPath().data())); - m_fileDialog->setNameFilter("URDF, XACRO (*.urdf *.xacro)"); + m_fileDialog->setNameFilter("URDF, XACRO, SDF, WORLD (*.urdf *.xacro *.sdf *.world)"); + // Whenever the selected file is successfully changed via the File Dialog or the Text Edit widget, + // save the full file name with path into the QSettings so that it defaults correctly the next time it is opened. + connect(this, &QWizardPage::completeChanged, [this]() + { + if (m_fileExists) + { + QSettings settings; + const QString absolutePath = m_textEdit->text(); + settings.setValue(FileSelectionPageDefaultFile, absolutePath); + } + }); + m_button = new QPushButton("...", this); m_textEdit = new QLineEdit("", this); - m_copyFiles = new QCheckBox(tr("Import meshes during URDF load"), this); - m_copyFiles->setCheckState(Qt::CheckState::Checked); - setTitle(tr("Load URDF file")); + setTitle(tr("Load URDF/SDF file")); QVBoxLayout* layout = new QVBoxLayout; layout->addStretch(); - layout->addWidget(new QLabel(tr("URDF file path to load : "), this)); - QHBoxLayout* layout_in = new QHBoxLayout; - layout_in->addWidget(m_button); - layout_in->addWidget(m_textEdit); - layout->addLayout(layout_in); - layout->addWidget(m_copyFiles); - layout->addStretch(); + layout->addWidget(new QLabel(tr("URDF/SDF file path to load : "), this)); + QHBoxLayout* layoutHorizontal = new QHBoxLayout; + layoutHorizontal->addWidget(m_button); + layoutHorizontal->addWidget(m_textEdit); + layout->addLayout(layoutHorizontal); + + // Seed the SDF Asset Builder Settings with the values from the Settings Registry + m_sdfAssetBuilderSettings->LoadSettings(); + // Create a reflected property editor that can modify the SDF AssetBuilder Settings + m_sdfAssetBuilderSettingsEditor = new AzToolsFramework::ReflectedPropertyEditor(this); + AZ::ComponentApplicationRequests* componentApplicationRequests = AZ::Interface::Get(); + AZ::SerializeContext* serializeContext{ componentApplicationRequests ? componentApplicationRequests->GetSerializeContext() : nullptr }; + AZ_Assert(serializeContext != nullptr, "Serialize Context is missing. It is required for the SDF Asset Builder Settings Editor"); + constexpr bool enableScrollBars = true; + m_sdfAssetBuilderSettingsEditor->Setup(serializeContext, nullptr, enableScrollBars); + m_sdfAssetBuilderSettingsEditor->AddInstance(m_sdfAssetBuilderSettings.get()); + m_sdfAssetBuilderSettingsEditor->InvalidateAll(); + layout->addWidget(m_sdfAssetBuilderSettingsEditor); + this->setLayout(layout); connect(m_button, &QPushButton::pressed, this, &FileSelectionPage::onLoadButtonPressed); connect(m_fileDialog, &QFileDialog::fileSelected, this, &FileSelectionPage::onFileSelected); @@ -41,8 +69,35 @@ namespace ROS2 FileSelectionPage::onEditingFinished(); } + FileSelectionPage::~FileSelectionPage() = default; + + void FileSelectionPage::RefreshDefaultPath() + { + // The first time this dialog ever gets opened, default to the project's root directory. + // Once a URDF/SDF file has been selected or typed in, change the default directory to the location of that file. + // This gets stored in QSettings, so it will persist between Editor runs. + QSettings settings; + QString defaultFile(settings.value(FileSelectionPageDefaultFile).toString()); + if (!defaultFile.isEmpty() && QFile(defaultFile).exists()) + { + // Set both the default directory and the default file in that directory. + m_fileDialog->setDirectory(QFileInfo(defaultFile).absolutePath()); + m_fileDialog->selectFile(QFileInfo(defaultFile).fileName()); + } + else + { + // No valid file was found, so default back to the current project path. + m_fileDialog->setDirectory(QString::fromUtf8(AZ::Utils::GetProjectPath().c_str())); + m_fileDialog->selectFile(""); + } + } + void FileSelectionPage::onLoadButtonPressed() { + // Refresh the default path in the file dialog every time it is opened so that + // any changes in the text edit box are reflected in its default path and any Cancel + // pressed to escape from the file dialog *don't* change its default path. + RefreshDefaultPath(); m_fileDialog->show(); } @@ -54,7 +109,7 @@ namespace ROS2 emit completeChanged(); } - void FileSelectionPage::onEditingFinished() + void FileSelectionPage::onEditingFinished() { QFileInfo urdfFile(m_textEdit->text()); m_fileExists = urdfFile.exists() && urdfFile.isFile(); @@ -66,8 +121,13 @@ namespace ROS2 return m_fileExists; } - bool FileSelectionPage::getIfCopyAssetsDuringUrdfImport() const + QString FileSelectionPage::getFileName() const + { + return isComplete() ? m_textEdit->text(): QString{}; + } + + const SdfAssetBuilderSettings& FileSelectionPage::GetSdfAssetBuilderSettings() const { - return m_copyFiles->isChecked(); + return *m_sdfAssetBuilderSettings; } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/Pages/FileSelectionPage.h b/Gems/ROS2/Code/Source/RobotImporter/Pages/FileSelectionPage.h index c507b209e..ac48a9739 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Pages/FileSelectionPage.h +++ b/Gems/ROS2/Code/Source/RobotImporter/Pages/FileSelectionPage.h @@ -18,38 +18,53 @@ #include #endif +#include + +namespace AzToolsFramework +{ + class ReflectedPropertyEditor; +} + namespace ROS2 { + struct SdfAssetBuilderSettings; +} +namespace ROS2 +{ class FileSelectionPage : public QWizardPage { Q_OBJECT public: explicit FileSelectionPage(QWizard* parent); + // The destructor is defaulted in the cpp file to allow the unique_ptr + // to not need a complete type in the header + ~FileSelectionPage(); bool isComplete() const override; - QString getFileName() const - { - if (m_fileExists) - { - return m_textEdit->text(); - } - return ""; - } - bool getIfCopyAssetsDuringUrdfImport() const; + QString getFileName() const; + + const SdfAssetBuilderSettings& GetSdfAssetBuilderSettings() const; private: QFileDialog* m_fileDialog; QPushButton* m_button; QLineEdit* m_textEdit; QCheckBox* m_copyFiles; + + AZStd::unique_ptr m_sdfAssetBuilderSettings; + AzToolsFramework::ReflectedPropertyEditor* m_sdfAssetBuilderSettingsEditor{}; void onLoadButtonPressed(); void onFileSelected(const QString& file); void onEditingFinished(); + //! Refresh the default path in the file dialog based either on what was previously selected + //! or what was entered in on the text edit line. + void RefreshDefaultPath(); + bool m_fileExists{ false }; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/Pages/IntroPage.cpp b/Gems/ROS2/Code/Source/RobotImporter/Pages/IntroPage.cpp index a19f53610..effccc051 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Pages/IntroPage.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/Pages/IntroPage.cpp @@ -17,7 +17,7 @@ namespace ROS2 setTitle(QObject::tr("Introduction")); m_label = new QLabel( - QObject::tr("This wizard allows you to build a robot prefab using a URDF description file." + QObject::tr("This wizard allows you to build a robot prefab using a URDF description file or object/environment prefab using an SDF file." " Before processing, please make sure that all of the robot's description packages have been built and sourced." " Details can be found format." "During the import process, the assets will be imported and processed." - "A level must be opened before using the URDF Importer.")); + "A level must be opened before using the URDF/SDF Importer.")); m_label->setTextFormat(Qt::RichText); m_label->setOpenExternalLinks(true); m_label->setWordWrap(true); diff --git a/Gems/ROS2/Code/Source/RobotImporter/Pages/PrefabMakerPage.cpp b/Gems/ROS2/Code/Source/RobotImporter/Pages/PrefabMakerPage.cpp index 1c099c75a..4a97d4513 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Pages/PrefabMakerPage.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/Pages/PrefabMakerPage.cpp @@ -7,27 +7,62 @@ */ #include "PrefabMakerPage.h" +#include +#include +#include +#include +#include +#include + +#include +#include #include +#include +#include +#include namespace ROS2 { - PrefabMakerPage::PrefabMakerPage(RobotImporterWidget* parent) : QWizardPage(parent) - , m_parentImporterWidget(parent) , m_success(false) + , m_parentImporterWidget(parent) { + AZ::EBusAggregateResults> allActiveSpawnPoints; + SpawnerRequestsBus::BroadcastResult(allActiveSpawnPoints, &SpawnerRequestsBus::Events::GetAllSpawnPointInfos); + + m_spawnPointsComboBox = new QComboBox(this); + m_spawnPointsInfos = allActiveSpawnPoints.values; + + for (int i = 0; i < allActiveSpawnPoints.values.size(); i++) + { + for (const auto& element : allActiveSpawnPoints.values[i]) + { + m_spawnPointsComboBox->addItem(element.first.c_str(), QVariant(i)); + } + } + m_prefabName = new QLineEdit(this); m_createButton = new QPushButton(tr("Create Prefab"), this); m_log = new QTextEdit(this); - m_useArticulation = new QCheckBox(tr("Use articulation for joints and rigid bodies"), this); + setTitle(tr("Prefab creation")); QVBoxLayout* layout = new QVBoxLayout; QHBoxLayout* layoutInner = new QHBoxLayout; layoutInner->addWidget(m_prefabName); layoutInner->addWidget(m_createButton); layout->addLayout(layoutInner); - layout->addWidget(m_useArticulation); + QLabel* spawnPointListLabel; + if (allActiveSpawnPoints.values.size() == 0) + { + spawnPointListLabel = new QLabel("Select spawn position (No spawn positions were detected)", this); + } + else + { + spawnPointListLabel = new QLabel("Select spawn position", this); + } + layout->addWidget(spawnPointListLabel); + layout->addWidget(m_spawnPointsComboBox); layout->addWidget(m_log); setLayout(layout); connect(m_createButton, &QPushButton::pressed, this, &PrefabMakerPage::onCreateButtonPressed); @@ -56,9 +91,19 @@ namespace ROS2 { return m_success; } - bool PrefabMakerPage::IsUseArticulations() const + AZStd::optional PrefabMakerPage::getSelectedSpawnPoint() const { - return m_useArticulation->isChecked(); + if (!m_spawnPointsInfos.empty()) + { + int vectorIndex = m_spawnPointsComboBox->currentData().toInt(); + AZStd::string mapKey(m_spawnPointsComboBox->currentText().toStdString().c_str()); + auto& map = m_spawnPointsInfos[vectorIndex]; + if (auto spawnInfo = map.find(mapKey); + spawnInfo != map.end()) + { + return spawnInfo->second.pose; + } + } + return AZStd::nullopt; } - } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/Pages/PrefabMakerPage.h b/Gems/ROS2/Code/Source/RobotImporter/Pages/PrefabMakerPage.h index 42bbd8032..758fedd90 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Pages/PrefabMakerPage.h +++ b/Gems/ROS2/Code/Source/RobotImporter/Pages/PrefabMakerPage.h @@ -8,6 +8,9 @@ #pragma once +#include "ROS2/Spawner/SpawnerInfo.h" +#include +#include #if !defined(Q_MOC_RUN) #include #include @@ -33,7 +36,7 @@ namespace ROS2 void reportProgress(const AZStd::string& progressForUser); void setSuccess(bool success); bool isComplete() const override; - bool IsUseArticulations() const; + AZStd::optional getSelectedSpawnPoint() const; Q_SIGNALS: void onCreateButtonPressed(); @@ -42,7 +45,8 @@ namespace ROS2 QLineEdit* m_prefabName; QPushButton* m_createButton; QTextEdit* m_log; - QCheckBox* m_useArticulation; + QComboBox* m_spawnPointsComboBox; + AZStd::vector m_spawnPointsInfos; RobotImporterWidget* m_parentImporterWidget; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp b/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp index 943df41aa..730380056 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp @@ -9,7 +9,21 @@ #include "ROS2RobotImporterEditorSystemComponent.h" #include "RobotImporterWidget.h" #include +#include +#include +#include +#include +#include #include +#include +#include +#include +#include +#include +#include + +#include + #if !defined(Q_MOC_RUN) #include #endif @@ -20,7 +34,25 @@ namespace ROS2 { if (auto serializeContext = azrtti_cast(context)) { - serializeContext->Class()->Version(0); + const auto& importerHookCamera = ROS2::SDFormat::ROS2SensorHooks::ROS2CameraSensor(); + const auto& importerHookGNSS = ROS2::SDFormat::ROS2SensorHooks::ROS2GNSSSensor(); + const auto& importerHookImu = ROS2::SDFormat::ROS2SensorHooks::ROS2ImuSensor(); + const auto& importerHookLidar = ROS2::SDFormat::ROS2SensorHooks::ROS2LidarSensor(); + serializeContext->Class()->Version(0)->Attribute( + "SensorImporterHooks", + SDFormat::SensorImporterHooksStorage{ AZStd::move(importerHookCamera), + AZStd::move(importerHookGNSS), + AZStd::move(importerHookImu), + AZStd::move(importerHookLidar) }); + } + + if (AZ::BehaviorContext* behaviorContext = azrtti_cast(context)) + { + behaviorContext->EBus("RobotImporterBus") + ->Attribute(AZ::Script::Attributes::Category, "Robotics") + ->Attribute(AZ::Script::Attributes::Scope, AZ::Script::Attributes::ScopeFlags::Automation) + ->Attribute(AZ::Script::Attributes::Module, "ROS2") + ->Event("ImportURDF", &RobotImporterRequestBus::Events::GeneratePrefabFromFile); } } @@ -40,10 +72,32 @@ namespace ROS2 { ROS2RobotImporterSystemComponent::Activate(); AzToolsFramework::EditorEvents::Bus::Handler::BusConnect(); + RobotImporterRequestBus::Handler::BusConnect(); + + auto serializeContext = AZ::Interface::Get()->GetSerializeContext(); + serializeContext->EnumerateAll( + [&](const AZ::SerializeContext::ClassData* classData, const AZ::Uuid& typeId) -> bool + { + auto* attribute = AZ::FindAttribute(AZ::Crc32("SensorImporterHooks"), classData->m_attributes); + if (attribute == nullptr) + { + return true; + } + + AZ::AttributeReader reader(nullptr, attribute); + SDFormat::SensorImporterHooksStorage sensorHooks; + if (reader.Read(sensorHooks)) + { + m_sensorHooks.insert(m_sensorHooks.end(), sensorHooks.begin(), sensorHooks.end()); + } + + return false; + }); } void ROS2RobotImporterEditorSystemComponent::Deactivate() { + RobotImporterRequestBus::Handler::BusDisconnect(); AzToolsFramework::EditorEvents::Bus::Handler::BusDisconnect(); ROS2RobotImporterSystemComponent::Deactivate(); } @@ -61,4 +115,148 @@ namespace ROS2 options.toolbarIcon = ":/ROS2/ROS_import_icon.svg"; AzToolsFramework::RegisterViewPane("Robot Importer", "ROS2", options); } + + bool ROS2RobotImporterEditorSystemComponent::GeneratePrefabFromFile( + const AZStd::string_view filePath, bool importAssetWithUrdf, bool useArticulation) + { + if (filePath.empty()) + { + AZ_Warning("ROS2RobotImporterEditorSystemComponent", false, "Path provided for prefab is empty"); + return false; + } + if (Utils::IsFileXacro(filePath)) + { + AZ_Warning("ROS2RobotImporterEditorSystemComponent", false, "XACRO formatted files are not supported"); + return false; + } + + // Read the SDF Settings from the Settings Registry into a local struct + SdfAssetBuilderSettings sdfBuilderSettings; + sdfBuilderSettings.LoadSettings(); + // Set the parser config settings for URDF content + sdf::ParserConfig parserConfig = Utils::SDFormat::CreateSdfParserConfigFromSettings(sdfBuilderSettings, filePath); + + auto parsedSdfOutcome = UrdfParser::ParseFromFile(filePath, parserConfig, sdfBuilderSettings); + if (!parsedSdfOutcome) + { + const AZStd::string log = Utils::JoinSdfErrorsToString(parsedSdfOutcome.GetSdfErrors()); + + AZ_Warning("ROS2RobotImporterEditorSystemComponent", false, "URDF/SDF parsing failed with errors:\nRefer to %s", log.c_str()); + return false; + } + + // Urdf Root has been parsed successfully retrieve it from the Outcome + const sdf::Root& parsedSdfRoot = parsedSdfOutcome.GetRoot(); + + auto assetNames = Utils::GetReferencedAssetFilenames(parsedSdfRoot); + AZStd::shared_ptr urdfAssetsMapping = AZStd::make_shared(); + if (importAssetWithUrdf) + { + urdfAssetsMapping = AZStd::make_shared( + Utils::CopyReferencedAssetsAndCreateAssetMap(assetNames, filePath, sdfBuilderSettings)); + } + bool allAssetProcessed = false; + bool assetProcessorFailed = false; + + auto loopStartTime = AZStd::chrono::system_clock::now(); + + /* This loop waits until all of the assets are processed. + The urdf prefab cannot be created before all assets are processed. + There are three stop conditions: allAssetProcessed, assetProcessorFailed and a timeout. + After all asset are processed the allAssetProcessed will be set to true. + assetProcessorFailed will be set to true if the asset processor does not respond. + The time out will break the loop if assetLoopTimeout is exceed. */ + while (!allAssetProcessed && !assetProcessorFailed) + { + auto loopTime = AZStd::chrono::system_clock::now(); + + if (loopTime - loopStartTime > assetLoopTimeout) + { + AZ_Warning("ROS2RobotImporterEditorSystemComponent", false, "Loop waiting for assets timed out"); + break; + } + + allAssetProcessed = true; + for (const auto& [name, asset] : *urdfAssetsMapping) + { + auto sourceAssetFullPath = asset.m_availableAssetInfo.m_sourceAssetGlobalPath; + if (sourceAssetFullPath.empty()) + { + AZ_Warning("ROS2RobotImporterEditorSystemComponent", false, "Asset %s missing `sourceAssetFullPath`", name.c_str()); + continue; + } + using namespace AzToolsFramework; + using namespace AzToolsFramework::AssetSystem; + AZ::Outcome result = AZ::Failure(); + AssetSystemJobRequestBus::BroadcastResult( + result, &AssetSystemJobRequestBus::Events::GetAssetJobsInfo, sourceAssetFullPath.Native(), true); + + if (!result.IsSuccess()) + { + assetProcessorFailed = true; + AZ_Error("ROS2RobotImporterEditorSystemComponent", false, "Asset System failed to reply with jobs infos"); + break; + } + + JobInfoContainer& allJobs = result.GetValue(); + for (const JobInfo& job : allJobs) + { + if (job.m_status == JobStatus::Queued || job.m_status == JobStatus::InProgress) + { + AZ_Printf("ROS2RobotImporterEditorSystemComponent", "asset %s is being processed\n", sourceAssetFullPath.c_str()); + allAssetProcessed = false; + } + else + { + AZ_Printf("ROS2RobotImporterEditorSystemComponent", "asset %s is done\n", sourceAssetFullPath.c_str()); + } + } + } + + if (allAssetProcessed && !assetProcessorFailed) + { + AZ_Printf("ROS2RobotImporterEditorSystemComponent", "All assets processed\n"); + } + }; + + // Use the URDF/SDF file name stem the prefab name + auto fileStem = AZ::IO::PathView(filePath).Stem(); + AZStd::string prefabName = AZStd::string::format("%.*s.prefab", AZ_PATH_ARG(fileStem)); + + if (prefabName.empty()) + { + AZ_Error( + "ROS2RobotImporterEditorSystemComponent", + false, + R"(URDF/SDF doesn't filename doesn't contain a stem "%.*s".)" + " O3DE Prefab cannot be created", + AZ_STRING_ARG(filePath)); + return false; + } + + const AZ::IO::Path prefabPathRelative(AZ::IO::Path("Assets") / "Importer" / prefabName); + const AZ::IO::Path prefabPath(AZ::IO::Path(AZ::Utils::GetProjectPath()) / prefabPathRelative); + AZStd::unique_ptr prefabMaker = + AZStd::make_unique(filePath, &parsedSdfRoot, prefabPath.String(), urdfAssetsMapping, useArticulation); + + auto prefabOutcome = prefabMaker->CreatePrefabFromUrdfOrSdf(); + + if (!prefabOutcome.IsSuccess()) + { + AZ_Error( + "ROS2RobotImporterEditorSystemComponent", + false, + "Unable to create Prefab from URDF/SDF file %.*s", + AZ_STRING_ARG(filePath)); + return false; + } + + return true; + } + + const SDFormat::SensorImporterHooksStorage& ROS2RobotImporterEditorSystemComponent::GetSensorHooks() const + { + return m_sensorHooks; + } + } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.h b/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.h index 9e429270a..2953b384d 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.h +++ b/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.h @@ -9,14 +9,19 @@ #pragma once #include "ROS2RobotImporterSystemComponent.h" +#include #include - +#include +#include +#include namespace ROS2 { + //! Editor component for RobotImporter widget class ROS2RobotImporterEditorSystemComponent : public ROS2RobotImporterSystemComponent , private AzToolsFramework::EditorEvents::Bus::Handler + , private RobotImporterRequestBus::Handler { public: AZ_COMPONENT(ROS2RobotImporterEditorSystemComponent, "{1cc069d0-72f9-411e-a94b-9159979e5a0c}", ROS2RobotImporterSystemComponent); @@ -29,15 +34,21 @@ namespace ROS2 ~ROS2RobotImporterEditorSystemComponent() = default; private: - ////////////////////////////////////////////////////////////////////////// - // Component overrides + // Component overrides ... void Activate() override; void Deactivate() override; - ////////////////////////////////////////////////////////////////////////// - ////////////////////////////////////////////////////////////////////////// - // AzToolsFramework::EditorEvents::Bus::Handler overrides + // AzToolsFramework::EditorEvents::Bus::Handler overrides ... void NotifyRegisterViews() override; - ////////////////////////////////////////////////////////////////////////// + + // RobotImporterRequestsBus::Handler overrides .. + bool GeneratePrefabFromFile(const AZStd::string_view filePath, bool importAssetWithUrdf, bool useArticulation) override; + const SDFormat::SensorImporterHooksStorage& GetSensorHooks() const override; + + // Timeout for loop waiting for assets to be built + static constexpr AZStd::chrono::seconds assetLoopTimeout = AZStd::chrono::seconds(30); + + // Cache for storing sensor importer hooks (read only once) + SDFormat::SensorImporterHooksStorage m_sensorHooks; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp index 740355931..b1554cb77 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp @@ -8,19 +8,23 @@ #include #include +#include #include +#include "FixURDF/URDFModifications.h" #include "RobotImporterWidget.h" -#include "URDF/URDFPrefabMaker.h" -#include "URDF/UrdfParser.h" -#include "Utils/RobotImporterUtils.h" #include #include #include +#include +#include +#include +#include +#include +#include namespace ROS2 { - RobotImporterWidget::RobotImporterWidget(QWidget* parent) : QWizard(parent) { @@ -77,77 +81,150 @@ namespace ROS2 } } + void RobotImporterWidget::AddModificationWarningsToReportString(QString& report, const UrdfParser::RootObjectOutcome& parsedSdfOutcome) + { + // This is a URDF only path, and therefore the report text does not mention SDF + report += "# " + tr("The URDF was parsed, though results were modified to be compatible with SDFormat") + "\n"; + + if (!parsedSdfOutcome.m_urdfModifications.missingInertias.empty()) + { + report += "## " + tr("Inertial information in the following links is missing, reset to default: ") + "\n"; + for (const auto& modifiedTag : parsedSdfOutcome.m_urdfModifications.missingInertias) + { + report += " - " + QString::fromUtf8(modifiedTag.linkName.data(), static_cast(modifiedTag.linkName.size())) + "\n"; + } + report += "\n"; + } + + if (!parsedSdfOutcome.m_urdfModifications.incompleteInertias.empty()) + { + report += + "## " + tr("Inertial information in the following links is incomplete, set default values for listed subtags: ") + "\n"; + for (const auto& modifiedTag : parsedSdfOutcome.m_urdfModifications.incompleteInertias) + { + report += " - " + QString::fromUtf8(modifiedTag.linkName.data(), static_cast(modifiedTag.linkName.size())) + ": "; + + for (const auto& tag : modifiedTag.missingTags) + { + report += QString::fromUtf8(tag.data(), static_cast(tag.size())) + ", "; + } + + report += "\n"; + } + report += "\n"; + } + + if (!parsedSdfOutcome.m_urdfModifications.duplicatedJoints.empty()) + { + report += "## " + tr("The following joints were renamed to avoid duplication") + "\n"; + for (const auto& modifiedTag : parsedSdfOutcome.m_urdfModifications.duplicatedJoints) + { + report += " - " + QString::fromUtf8(modifiedTag.oldName.data(), static_cast(modifiedTag.oldName.size())) + " -> " + + QString::fromUtf8(modifiedTag.newName.data(), static_cast(modifiedTag.newName.size())) + "\n"; + } + } + + report += "\n# " + tr("The modified URDF code:") + "\n"; + report += "```\n" + QString::fromStdString(parsedSdfOutcome.m_modifiedURDFContent) + "```\n"; + } + void RobotImporterWidget::OpenUrdf() { + UrdfParser::RootObjectOutcome parsedSdfOutcome; QString report; if (!m_urdfPath.empty()) { - if (IsFileXacro(m_urdfPath)) + // Read the SDF Settings from PrefabMakerPage + const SdfAssetBuilderSettings& sdfBuilderSettings = m_fileSelectPage->GetSdfAssetBuilderSettings(); + + // Set the parser config settings for URDF content + sdf::ParserConfig parserConfig = Utils::SDFormat::CreateSdfParserConfigFromSettings(sdfBuilderSettings, m_urdfPath); + + if (Utils::IsFileXacro(m_urdfPath)) { - Utils::xacro::ExecutionOutcome outcome = Utils::xacro::ParseXacro(m_urdfPath.String(), m_params); + Utils::xacro::ExecutionOutcome outcome = + Utils::xacro::ParseXacro(m_urdfPath.String(), m_params, parserConfig, sdfBuilderSettings); + // Store off the URDF parsing outcome which will be output later in this function + parsedSdfOutcome = AZStd::move(outcome.m_urdfHandle); if (outcome) { - m_parsedUrdf = outcome.m_urdfHandle; report += "# " + tr("XACRO execution succeeded") + "\n"; m_assetPage->ClearAssetsList(); } else { - report += "# " + tr("XACRO parsing failed") + "\n"; - report += "\n\n## " + tr("Command called") + "\n\n`" + QString::fromUtf8(outcome.m_called.data()) + "`"; - report += "\n\n" + tr("Process failed"); - report += "\n\n## " + tr("Error output") + "\n\n"; - report += "```\n"; - if (outcome.m_logErrorOutput.size()) - { - report += - QString::fromLocal8Bit(outcome.m_logErrorOutput.data(), static_cast(outcome.m_logErrorOutput.size())); - } - else + if (outcome.m_succeed) { - report += tr("(EMPTY)"); - } - report += "\n```"; - report += "\n\n## " + tr("Standard output") + "\n\n"; - report += "```\n"; - if (outcome.m_logStandardOutput.size()) - { - report += QString::fromLocal8Bit( - outcome.m_logStandardOutput.data(), static_cast(outcome.m_logStandardOutput.size())); + report += "# " + tr("XACRO execution succeeded, but URDF parsing failed") + "\n"; } else { - report += tr("(EMPTY)"); + report += "# " + tr("XACRO parsing failed") + "\n"; + report += "\n\n## " + tr("Command called") + "\n\n`" + QString::fromUtf8(outcome.m_called.data()) + "`"; + report += "\n\n" + tr("Process failed"); + report += "\n\n## " + tr("Error output") + "\n\n"; + report += "```\n"; + if (outcome.m_logErrorOutput.size()) + { + report += QString::fromUtf8(outcome.m_logErrorOutput.data(), static_cast(outcome.m_logErrorOutput.size())); + } + else + { + report += tr("(EMPTY)"); + } + report += "\n```"; + report += "\n\n## " + tr("Standard output") + "\n\n"; + report += "```\n"; + if (outcome.m_logStandardOutput.size()) + { + report += + QString::fromUtf8(outcome.m_logStandardOutput.data(), static_cast(outcome.m_logStandardOutput.size())); + } + else + { + report += tr("(EMPTY)"); + } + report += "\n```"; + m_checkUrdfPage->ReportURDFResult(report, false); + return; } - report += "\n```"; - m_checkUrdfPage->ReportURDFResult(report, false); - m_parsedUrdf = nullptr; - return; } } - else if (IsFileUrdf(m_urdfPath)) + else if (Utils::IsFileUrdfOrSdf(m_urdfPath)) { // standard URDF - m_parsedUrdf = UrdfParser::ParseFromFile(m_urdfPath.Native()); + parsedSdfOutcome = UrdfParser::ParseFromFile(m_urdfPath, parserConfig, sdfBuilderSettings); } else { AZ_Assert(false, "Unknown file extension : %s \n", m_urdfPath.c_str()); } - const auto log = UrdfParser::GetUrdfParsingLog(); - if (m_parsedUrdf) + + AZStd::string log; + const bool urdfParsedSuccess{ parsedSdfOutcome }; + const bool urdfParsedWithWarnings{ parsedSdfOutcome.UrdfParsedWithModifiedContent() }; + if (urdfParsedSuccess) { - report += "# " + tr("The URDF was parsed and opened successfully") + "\n"; + if (urdfParsedWithWarnings) + { + AddModificationWarningsToReportString(report, parsedSdfOutcome); + } + else + { + report += "# " + tr("The URDF/SDF was parsed and opened successfully") + "\n"; + AZ_Printf("Wizard", "Wizard skips m_checkUrdfPage since there is no errors in URDF\n"); + } + m_parsedSdf = AZStd::move(parsedSdfOutcome.GetRoot()); m_prefabMaker.reset(); // Report the status of skipping this page - AZ_Printf("Wizard", "Wizard skips m_checkUrdfPage since there is no errors in URDF\n"); - m_meshNames = Utils::GetMeshesFilenames(m_parsedUrdf->getRoot(), true, true); + m_assetNames = Utils::GetReferencedAssetFilenames(m_parsedSdf); m_assetPage->ClearAssetsList(); } else { - report += "# " + tr("The URDF was not opened") + "\n"; - report += tr("URDF parser returned following errors:") + "\n\n"; + log = Utils::JoinSdfErrorsToString(parsedSdfOutcome.GetSdfErrors()); + report += "# " + tr("The URDF/SDF was not opened") + "\n"; + report += tr("URDF/SDF parser returned following errors:") + "\n\n"; } if (!log.empty()) { @@ -155,7 +232,18 @@ namespace ROS2 report += QString::fromUtf8(log.data(), int(log.size())); report += "`"; } - m_checkUrdfPage->ReportURDFResult(report, m_parsedUrdf != nullptr); + m_checkUrdfPage->ReportURDFResult(report, urdfParsedSuccess, urdfParsedWithWarnings); + const auto& messages = parsedSdfOutcome.GetParseMessages(); + if (!messages.empty()) + { + report += "\n\n"; + report += tr("URDF/SDF parser returned following messages:") + "\n\n"; + report += "```bash\n"; + report += QString::fromUtf8(messages.c_str(), int(messages.size())); + report += "\n```\n"; + AZ_Printf("RobotImporterWidget", "SDF Stream: %s\n", messages.c_str()); + } + m_checkUrdfPage->ReportURDFResult(report, urdfParsedSuccess); } } @@ -175,74 +263,85 @@ namespace ROS2 void RobotImporterWidget::FillAssetPage() { - if (m_parsedUrdf && m_assetPage->IsEmpty()) + if (m_assetPage->IsEmpty()) { - auto collidersNames = Utils::GetMeshesFilenames(m_parsedUrdf->getRoot(), false, true); - auto visualNames = Utils::GetMeshesFilenames(m_parsedUrdf->getRoot(), true, false); + AZ::Uuid::FixedString dirSuffix; + if (!m_params.empty()) + { + auto paramsUuid = AZ::Uuid::CreateNull(); + for (auto& [key, value] : m_params) + { + paramsUuid += AZ::Uuid::CreateName(key); + paramsUuid += AZ::Uuid::CreateName(value); + } + + dirSuffix = paramsUuid.ToFixedString(); + } + + // Read the SDF Settings from PrefabMakerPage + const SdfAssetBuilderSettings& sdfBuilderSettings = m_fileSelectPage->GetSdfAssetBuilderSettings(); if (m_importAssetWithUrdf) { - m_urdfAssetsMapping = AZStd::make_shared( - Utils::CopyAssetForURDFAndCreateAssetMap(m_meshNames, m_urdfPath.String(), collidersNames, visualNames)); + m_urdfAssetsMapping = AZStd::make_shared(Utils::CopyReferencedAssetsAndCreateAssetMap( + m_assetNames, m_urdfPath.String(), sdfBuilderSettings, dirSuffix)); } else { - m_urdfAssetsMapping = AZStd::make_shared(Utils::FindAssetsForUrdf(m_meshNames, m_urdfPath.String())); - for (const AZStd::string& meshPath : m_meshNames) + m_urdfAssetsMapping = + AZStd::make_shared(Utils::FindReferencedAssets(m_assetNames, m_urdfPath.String(), sdfBuilderSettings)); + for (const auto& [assetPath, assetReferenceType] : m_assetNames) { - if (m_urdfAssetsMapping->contains(meshPath)) + if (m_urdfAssetsMapping->contains(assetPath)) { - const auto& asset = m_urdfAssetsMapping->at(meshPath); - bool visual = visualNames.contains(meshPath); - bool collider = collidersNames.contains(meshPath); - Utils::CreateSceneManifest(asset.m_availableAssetInfo.m_sourceAssetGlobalPath, collider, visual); + const auto& asset = m_urdfAssetsMapping->at(assetPath); + bool visual = (assetReferenceType & Utils::ReferencedAssetType::VisualMesh) == Utils::ReferencedAssetType::VisualMesh; + bool collider = (assetReferenceType & Utils::ReferencedAssetType::ColliderMesh) == Utils::ReferencedAssetType::ColliderMesh; + if (visual || collider) + { + Utils::CreateSceneManifest(asset.m_availableAssetInfo.m_sourceAssetGlobalPath, collider, visual); + } } } }; - for (const AZStd::string& meshPath : m_meshNames) + for (const auto& [assetPath, assetReferenceType] : m_assetNames) { - const QString kNotFound = tr("not found"); - const AZStd::string kNotFoundAz(kNotFound.toUtf8()); - AZ::Uuid sourceAssetUuid; - if (m_urdfAssetsMapping->contains(meshPath)) + AZ::Uuid sourceAssetUuid = AZ::Uuid::CreateNull(); + QString type = tr("Unknown"); + AZStd::optional crc; + AZStd::optional sourcePath; + AZStd::optional resolvedPath; + + if (m_urdfAssetsMapping->contains(assetPath)) { - QString type = kNotFound; - AZStd::string sourcePath(kNotFoundAz); - AZStd::string resolvedPath(kNotFoundAz); - QString productAssetText; - auto crc = AZ::Crc32(); - QString tooltip = kNotFound; - bool visual = visualNames.contains(meshPath); - bool collider = collidersNames.contains(meshPath); + bool visual = (assetReferenceType & Utils::ReferencedAssetType::VisualMesh) == Utils::ReferencedAssetType::VisualMesh; + bool collider = (assetReferenceType & Utils::ReferencedAssetType::ColliderMesh) == Utils::ReferencedAssetType::ColliderMesh; + bool texture = (assetReferenceType & Utils::ReferencedAssetType::Texture) == Utils::ReferencedAssetType::Texture; if (visual && collider) { - type = tr("Visual and Collider"); + type = tr("Visual and Collider Mesh"); } else if (visual) { - type = tr("Visual"); + type = tr("Visual Mesh"); } else if (collider) { - type = tr("Collider"); + type = tr("Collider Mesh"); } - - if (m_urdfAssetsMapping->contains(meshPath)) + else if (texture) { - const auto& asset = m_urdfAssetsMapping->at(meshPath); - sourceAssetUuid = asset.m_availableAssetInfo.m_sourceGuid; - sourcePath = asset.m_availableAssetInfo.m_sourceAssetRelativePath; - resolvedPath = asset.m_resolvedUrdfPath.data(); - crc = asset.m_urdfFileCRC; - tooltip = QString::fromUtf8(resolvedPath.data(), resolvedPath.size()); + type = tr("Texture"); } - m_assetPage->ReportAsset(sourceAssetUuid, meshPath, type, sourcePath, crc, resolvedPath); + + const auto& asset = m_urdfAssetsMapping->at(assetPath); + sourceAssetUuid = asset.m_availableAssetInfo.m_sourceGuid; + sourcePath = asset.m_availableAssetInfo.m_sourceAssetRelativePath.String(); + resolvedPath = asset.m_resolvedUrdfPath.String(); + crc = asset.m_urdfFileCRC; } - else - { - m_assetPage->ReportAsset(sourceAssetUuid, meshPath, kNotFound, kNotFoundAz, AZ::Crc32(), kNotFoundAz); - }; + m_assetPage->ReportAsset(sourceAssetUuid, assetPath, type, sourcePath, crc, resolvedPath); } m_assetPage->StartWatchAsset(); } @@ -250,38 +349,41 @@ namespace ROS2 void RobotImporterWidget::FillPrefabMakerPage() { - if (m_parsedUrdf) - { - AZStd::string robotName = AZStd::string(m_parsedUrdf->getName().c_str(), m_parsedUrdf->getName().size()) + ".prefab"; - m_prefabMakerPage->setProposedPrefabName(robotName); - QWizard::button(PrefabCreationButtonId)->setText(tr("Create Prefab")); - QWizard::setOption(HavePrefabCreationButton, true); - } + // Use the URDF/SDF file name stem the prefab name + AZStd::string robotName = AZStd::string::format("%.*s.prefab", AZ_PATH_ARG(m_urdfPath.Stem())); + m_prefabMakerPage->setProposedPrefabName(robotName); + QWizard::button(PrefabCreationButtonId)->setText(tr("Create Prefab")); + QWizard::setOption(HavePrefabCreationButton, true); } bool RobotImporterWidget::validateCurrentPage() { + // If SDF file are desired to be brought in via the RobotImporter workflow + // an OpenSdf function would need to be added if (currentPage() == m_fileSelectPage) { m_params.clear(); m_urdfPath = AZStd::string(m_fileSelectPage->getFileName().toUtf8().constData()); - if (IsFileXacro(m_urdfPath)) + if (Utils::IsFileXacro(m_urdfPath)) { m_params = Utils::xacro::GetParameterFromXacroFile(m_urdfPath.String()); AZ_Printf("RobotImporterWidget", "Xacro has %d arguments\n", m_params.size()); m_xacroParamsPage->SetXacroParameters(m_params); } // no need to wait for param page - parse urdf now, nextId will skip unnecessary pages - if (m_params.empty()) + if (const bool isFileXacroUrdfOrSdf = Utils::IsFileXacroOrUrdfOrSdf(m_urdfPath); m_params.empty() && isFileXacroUrdfOrSdf) { OpenUrdf(); } - m_importAssetWithUrdf = m_fileSelectPage->getIfCopyAssetsDuringUrdfImport(); + m_importAssetWithUrdf = m_fileSelectPage->GetSdfAssetBuilderSettings().m_importReferencedMeshFiles; } if (currentPage() == m_xacroParamsPage) { m_params = m_xacroParamsPage->GetXacroParameters(); - OpenUrdf(); + if (const bool isFileXacroUrdfOrSdf = Utils::IsFileXacroOrUrdfOrSdf(m_urdfPath); isFileXacroUrdfOrSdf) + { + OpenUrdf(); + } } if (currentPage() == m_introPage) { @@ -295,7 +397,7 @@ namespace ROS2 if (!levelEntityId.IsValid() || levelEntity == nullptr) { QMessageBox noLevelLoadedMessage; - noLevelLoadedMessage.critical(0, "No level opened", "A level must be opened before using URDF Importer"); + noLevelLoadedMessage.critical(0, "No level opened", "A level must be opened before using the Robot Importer"); noLevelLoadedMessage.setFixedSize(500, 200); return false; @@ -308,30 +410,38 @@ namespace ROS2 { if ((currentPage() == m_fileSelectPage && m_params.empty()) || currentPage() == m_xacroParamsPage) { - if (m_parsedUrdf) + if (!m_checkUrdfPage->isWarning()) { - if (m_meshNames.size() == 0) + return m_xacroParamsPage->nextId(); + } + if (m_checkUrdfPage->isComplete()) + { + if (m_assetNames.empty()) { - // skip two pages when urdf is parsed without problems, and it has no meshes + // skip two pages when urdf/sdf is parsed without problems, and it has no assets return m_assetPage->nextId(); } else { - // skip one page when urdf is parsed without problems + // skip one page when urdf/sdf is parsed without problems return m_checkUrdfPage->nextId(); } } + if (m_params.empty()) + { + return m_xacroParamsPage->nextId(); + } } return currentPage()->nextId(); } void RobotImporterWidget::CreatePrefab(AZStd::string prefabName) { - const AZ::IO::Path prefabPathRealative(AZ::IO::Path("Assets") / "Importer" / prefabName); - const AZ::IO::Path prefabPath(AZ::IO::Path(AZ::Utils::GetProjectPath()) / prefabPathRealative); + const AZ::IO::Path prefabPathRelative(AZ::IO::Path("Assets") / "Importer" / prefabName); + const AZ::IO::Path prefabPath(AZ::IO::Path(AZ::Utils::GetProjectPath()) / prefabPathRelative); bool fileExists = AZ::IO::FileIOBase::GetInstance()->Exists(prefabPath.c_str()); - if (CheckCyclicalDependency(prefabPathRealative)) + if (CheckCyclicalDependency(prefabPathRelative)) { m_prefabMakerPage->setSuccess(false); return; @@ -350,11 +460,18 @@ namespace ROS2 return; } } - const bool useArticulation = m_prefabMakerPage->IsUseArticulations(); - m_prefabMaker = AZStd::make_unique( - m_urdfPath.String(), m_parsedUrdf, prefabPath.String(), m_urdfAssetsMapping, useArticulation); - auto prefabOutcome = m_prefabMaker->CreatePrefabFromURDF(); + const auto& sdfAssetBuilderSettings = m_fileSelectPage->GetSdfAssetBuilderSettings(); + const bool useArticulation = sdfAssetBuilderSettings.m_useArticulations; + m_prefabMaker = AZStd::make_unique( + m_urdfPath.String(), + &m_parsedSdf, + prefabPath.String(), + m_urdfAssetsMapping, + useArticulation, + m_prefabMakerPage->getSelectedSpawnPoint()); + + auto prefabOutcome = m_prefabMaker->CreatePrefabFromUrdfOrSdf(); if (prefabOutcome.IsSuccess()) { AZStd::string status = m_prefabMaker->GetStatus(); @@ -402,8 +519,8 @@ namespace ROS2 if (focusPrefabFilename == importedPrefabPath) { - ReportError( - tr("Cyclical dependency detected.\nSelected URDF model is currently being edited. Exit prefab edit mode and try again.")); + ReportError(tr( + "Cyclical dependency detected.\nSelected URDF/SDF model is currently being edited. Exit prefab edit mode and try again.")); return true; } @@ -416,21 +533,4 @@ namespace ROS2 AZ_Error("RobotImporterWidget", false, "%s", errorMessage.toUtf8().constData()); } - AZStd::string RobotImporterWidget::GetCapitalizedExtension(const AZ::IO::Path& filename) const - { - AZStd::string extension{ filename.Extension().Native() }; - AZStd::to_upper(extension.begin(), extension.end()); - return extension; - } - - bool RobotImporterWidget::IsFileXacro(const AZ::IO::Path& filename) const - { - return filename.HasExtension() && GetCapitalizedExtension(filename) == ".XACRO"; - } - - bool RobotImporterWidget::IsFileUrdf(const AZ::IO::Path& filename) const - { - return filename.HasExtension() && GetCapitalizedExtension(filename) == ".URDF"; - } - } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.h b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.h index d2c6f604b..1d6cfc5b8 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.h +++ b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.h @@ -20,6 +20,7 @@ #include "URDF/UrdfParser.h" #include #include +#include #include #include @@ -69,7 +70,7 @@ namespace ROS2 PrefabMakerPage* m_prefabMakerPage; XacroParamsPage* m_xacroParamsPage; AZ::IO::Path m_urdfPath; - urdf::ModelInterfaceSharedPtr m_parsedUrdf; + sdf::Root m_parsedSdf{}; //! User's choice to copy meshes during urdf import bool m_importAssetWithUrdf{ false }; @@ -77,7 +78,7 @@ namespace ROS2 /// mapping from urdf path to asset source AZStd::shared_ptr m_urdfAssetsMapping; AZStd::unique_ptr m_prefabMaker; - AZStd::unordered_set m_meshNames; + Utils::AssetFilenameReferences m_assetNames; /// Xacro params Utils::xacro::Params m_params; @@ -88,7 +89,7 @@ namespace ROS2 //! Checks if the importedPrefabFilename is the same as focused prefab name. //! @param importedPrefabFilename name of imported prefab - //! @return True if names of prefabs are identical or an erorr occured during validation + //! @return True if names of prefabs are identical or an error occurred during validation bool CheckCyclicalDependency(AZ::IO::Path importedPrefabFilename); //! Report an error to the user. @@ -96,17 +97,7 @@ namespace ROS2 //! @param errorMessage error message to display to the user void ReportError(const QString& errorMessage); - //! Returns if file is xacro. - //! @param filename path to check - bool IsFileXacro(const AZ::IO::Path& filename) const; - - //! Returns if file is urdf. - //! @param filename path to check - bool IsFileUrdf(const AZ::IO::Path& filename) const; - - //! Returns capitalized extension. - //! @param filename path to check - AZStd::string GetCapitalizedExtension(const AZ::IO::Path& filename) const; + void AddModificationWarningsToReportString(QString& report, const UrdfParser::RootObjectOutcome& parsedSdfOutcome); static constexpr QWizard::WizardButton PrefabCreationButtonId{ QWizard::CustomButton1 }; static constexpr QWizard::WizardOption HavePrefabCreationButton{ QWizard::HaveCustomButton1 }; diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp new file mode 100644 index 000000000..76ca3f956 --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp @@ -0,0 +1,96 @@ +/* + * 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 +#include +#include +#include +#include + +#include +#include + +namespace ROS2::SDFormat +{ + SensorImporterHook ROS2SensorHooks::ROS2CameraSensor() + { + SensorImporterHook importerHook; + importerHook.m_sensorTypes = + AZStd::unordered_set{ sdf::SensorType::CAMERA, sdf::SensorType::DEPTH_CAMERA, sdf::SensorType::RGBD_CAMERA }; + importerHook.m_supportedSensorParams = + AZStd::unordered_set{ ">update_rate", ">camera>horizontal_fov", ">camera>image>width", + ">camera>image>height", ">camera>clip>near", ">camera>clip>far" }; + importerHook.m_pluginNames = AZStd::unordered_set{ "libgazebo_ros_camera.so", + "libgazebo_ros_depth_camera.so", + "libgazebo_ros_openni_kinect.so" }; + importerHook.m_supportedPluginParams = AZStd::unordered_set{}; + importerHook.m_sdfSensorToComponentCallback = [](AZ::Entity& entity, + const sdf::Sensor& sdfSensor) -> SensorImporterHook::ConvertSensorOutcome + { + auto* cameraSensor = sdfSensor.CameraSensor(); + if (!cameraSensor) + { + return AZ::Failure(AZStd::string("Failed to read parsed SDFormat data of %s camera sensor", sdfSensor.Name().c_str())); + } + + CameraSensorConfiguration cameraConfiguration; + cameraConfiguration.m_depthCamera = cameraSensor->HasDepthCamera(); + cameraConfiguration.m_colorCamera = (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA) ? true : false; + cameraConfiguration.m_width = cameraSensor->ImageWidth(); + cameraConfiguration.m_height = cameraSensor->ImageHeight(); + if (cameraConfiguration.m_width != 0) + { + double aspectRatio = static_cast(cameraConfiguration.m_height) / cameraConfiguration.m_width; + cameraConfiguration.m_verticalFieldOfViewDeg = + AZ::RadToDeg(2.0 * AZStd::atan(AZStd::tan(cameraSensor->HorizontalFov().Radian() / 2.0) * aspectRatio)); + } + if (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA) + { + cameraConfiguration.m_nearClipDistance = static_cast(cameraSensor->NearClip()); + cameraConfiguration.m_farClipDistance = static_cast(cameraSensor->FarClip()); + } + else + { + cameraConfiguration.m_nearClipDistance = static_cast(cameraSensor->DepthNearClip()); + cameraConfiguration.m_farClipDistance = static_cast(cameraSensor->DepthFarClip()); + } + + SensorConfiguration sensorConfiguration; + sensorConfiguration.m_frequency = sdfSensor.UpdateRate(); + if (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA) + { // COLOR_CAMERA and RGBD_CAMERA + Utils::AddTopicConfiguration( + sensorConfiguration, "camera_image_color", CameraConstants::ImageMessageType, CameraConstants::ColorImageConfig); + Utils::AddTopicConfiguration( + sensorConfiguration, "color_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::ColorInfoConfig); + } + if (sdfSensor.Type() != sdf::SensorType::CAMERA) + { // DEPTH_CAMERA and RGBD_CAMERA + Utils::AddTopicConfiguration( + sensorConfiguration, "camera_image_depth", CameraConstants::ImageMessageType, CameraConstants::DepthImageConfig); + Utils::AddTopicConfiguration( + sensorConfiguration, "depth_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig); + } + + // Create required components + Utils::CreateComponent(entity); + + // Create Camera component + if (Utils::CreateComponent(entity, sensorConfiguration, cameraConfiguration)) + { + return AZ::Success(); + } + else + { + return AZ::Failure(AZStd::string("Failed to create ROS2 Camera Sensor component")); + } + }; + + return importerHook; + } +} // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp new file mode 100644 index 000000000..0a8485f1a --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp @@ -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 + * + */ + +#include +#include +#include + +#include +#include + +namespace ROS2::SDFormat +{ + SensorImporterHook ROS2SensorHooks::ROS2GNSSSensor() + { + SensorImporterHook importerHook; + importerHook.m_sensorTypes = AZStd::unordered_set{ sdf::SensorType::NAVSAT }; + importerHook.m_supportedSensorParams = AZStd::unordered_set{ ">update_rate" }; + importerHook.m_pluginNames = AZStd::unordered_set{ "libgazebo_ros_gps_sensor.so" }; + importerHook.m_supportedPluginParams = AZStd::unordered_set{}; + importerHook.m_sdfSensorToComponentCallback = [](AZ::Entity& entity, + const sdf::Sensor& sdfSensor) -> SensorImporterHook::ConvertSensorOutcome + { + if (!sdfSensor.NavSatSensor()) + { + return AZ::Failure(AZStd::string("Failed to read parsed SDFormat data of %s NavSat sensor", sdfSensor.Name().c_str())); + } + + SensorConfiguration sensorConfiguration; + sensorConfiguration.m_frequency = sdfSensor.UpdateRate(); + const AZStd::string messageType = "sensor_msgs::msg::NavSatFix"; + Utils::AddTopicConfiguration(sensorConfiguration, "gnss", messageType, messageType); + + if (Utils::CreateComponent(entity, sensorConfiguration, GNSSSensorConfiguration())) + { + return AZ::Success(); + } + else + { + return AZ::Failure(AZStd::string("Failed to create ROS2 GNSS Sensor component")); + } + }; + + return importerHook; + } +} // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp new file mode 100644 index 000000000..aa135ab2d --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp @@ -0,0 +1,101 @@ +/* + * 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 +#include +#include +#include +#include + +#include +#include + +namespace ROS2::SDFormat +{ + SensorImporterHook ROS2SensorHooks::ROS2ImuSensor() + { + SensorImporterHook importerHook; + importerHook.m_sensorTypes = AZStd::unordered_set{ sdf::SensorType::IMU }; + importerHook.m_supportedSensorParams = AZStd::unordered_set{ ">update_rate", + ">imu>angular_velocity>x>noise>mean", + ">imu>angular_velocity>x>noise>stddev", + ">imu>angular_velocity>y>noise>mean", + ">imu>angular_velocity>y>noise>stddev", + ">imu>angular_velocity>z>noise>mean", + ">imu>angular_velocity>z>noise>stddev", + ">imu>linear_acceleration>x>noise>mean", + ">imu>linear_acceleration>x>noise>stddev", + ">imu>linear_acceleration>y>noise>mean", + ">imu>linear_acceleration>y>noise>stddev", + ">imu>linear_acceleration>z>noise>mean", + ">imu>linear_acceleration>z>noise>stddev" }; + importerHook.m_pluginNames = AZStd::unordered_set{ "libgazebo_ros_imu_sensor.so" }; + importerHook.m_supportedPluginParams = AZStd::unordered_set{}; + importerHook.m_sdfSensorToComponentCallback = [](AZ::Entity& entity, + const sdf::Sensor& sdfSensor) -> SensorImporterHook::ConvertSensorOutcome + { + auto* imuSensor = sdfSensor.ImuSensor(); + if (!imuSensor) + { + return AZ::Failure(AZStd::string("Failed to read parsed SDFormat data of %s imu sensor", sdfSensor.Name().c_str())); + } + + ImuSensorConfiguration imuConfiguration; + const auto& angVelXNoise = imuSensor->AngularVelocityXNoise(); + const auto& angVelYNoise = imuSensor->AngularVelocityYNoise(); + const auto& angVelZNoise = imuSensor->AngularVelocityZNoise(); + if (angVelXNoise.Type() == sdf::NoiseType::GAUSSIAN && angVelYNoise.Type() == sdf::NoiseType::GAUSSIAN && + angVelZNoise.Type() == sdf::NoiseType::GAUSSIAN) + { + if (angVelXNoise.Mean() == 0.0 && angVelYNoise.Mean() == 0.0 && angVelZNoise.Mean() == 0.0) + { + imuConfiguration.m_angularVelocityVariance = AZ::Vector3( + static_cast(angVelXNoise.StdDev() * angVelXNoise.StdDev()), + static_cast(angVelYNoise.StdDev() * angVelYNoise.StdDev()), + static_cast(angVelZNoise.StdDev() * angVelZNoise.StdDev())); + } + } + + const auto& linAccXNoise = imuSensor->LinearAccelerationXNoise(); + const auto& linAccYNoise = imuSensor->LinearAccelerationYNoise(); + const auto& linAccZNoise = imuSensor->LinearAccelerationZNoise(); + if (linAccXNoise.Type() == sdf::NoiseType::GAUSSIAN && linAccYNoise.Type() == sdf::NoiseType::GAUSSIAN && + linAccZNoise.Type() == sdf::NoiseType::GAUSSIAN) + { + if (linAccXNoise.Mean() == 0.0 && linAccYNoise.Mean() == 0.0 && linAccZNoise.Mean() == 0.0) + { + imuConfiguration.m_linearAccelerationVariance = AZ::Vector3( + static_cast(linAccXNoise.StdDev() * linAccXNoise.StdDev()), + static_cast(linAccYNoise.StdDev() * linAccYNoise.StdDev()), + static_cast(linAccZNoise.StdDev() * linAccZNoise.StdDev())); + } + } + + SensorConfiguration sensorConfiguration; + sensorConfiguration.m_frequency = sdfSensor.UpdateRate(); + const AZStd::string messageType = "sensor_msgs::msg::Imu"; + Utils::AddTopicConfiguration(sensorConfiguration, "imu", messageType, messageType); + + // Create required components + Utils::CreateComponent(entity); + Utils::CreateComponent(entity); + + // Create Imu component + if (Utils::CreateComponent(entity, sensorConfiguration, imuConfiguration)) + { + return AZ::Success(); + } + else + { + return AZ::Failure(AZStd::string("Failed to create ROS2 Imu Sensor component")); + } + }; + + return importerHook; + } +} // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp new file mode 100644 index 000000000..fdd09cc64 --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp @@ -0,0 +1,87 @@ +/* + * 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 +#include +#include +#include + +#include +#include + +namespace ROS2::SDFormat +{ + SensorImporterHook ROS2SensorHooks::ROS2LidarSensor() + { + SensorImporterHook importerHook; + importerHook.m_sensorTypes = AZStd::unordered_set{ sdf::SensorType::LIDAR }; + importerHook.m_supportedSensorParams = AZStd::unordered_set{ + ">update_rate", + ">lidar>scan>horizontal>samples", + ">lidar>scan>horizontal>min_angle", + ">lidar>scan>horizontal>max_angle", + ">lidar>scan>vertical>samples", + ">lidar>scan>vertical>min_angle", + ">lidar>scan>vertical>max_angle", + ">lidar>range>min", + ">lidar>range>max", + // Gazebo-classic + ">ray>scan>horizontal>samples", + ">ray>scan>horizontal>min_angle", + ">ray>scan>horizontal>max_angle", + ">ray>scan>vertical>samples", + ">ray>scan>vertical>min_angle", + ">ray>scan>vertical>max_angle", + ">ray>range>min", + ">ray>range>max", + }; + importerHook.m_pluginNames = AZStd::unordered_set{ "libgazebo_ros_ray_sensor.so", "libgazebo_ros_laser.so" }; + importerHook.m_supportedPluginParams = AZStd::unordered_set{}; + importerHook.m_sdfSensorToComponentCallback = [](AZ::Entity& entity, + const sdf::Sensor& sdfSensor) -> SensorImporterHook::ConvertSensorOutcome + { + auto* lidarSensor = sdfSensor.LidarSensor(); + if (!lidarSensor) + { + return AZ::Failure(AZStd::string("Failed to read parsed SDFormat data of %s Lidar sensor", sdfSensor.Name().c_str())); + } + + SensorConfiguration sensorConfiguration; + sensorConfiguration.m_frequency = sdfSensor.UpdateRate(); + const AZStd::string messageType = "sensor_msgs::msg::PointCloud2"; + Utils::AddTopicConfiguration(sensorConfiguration, "pc", messageType, messageType); + + LidarSensorConfiguration lidarConfiguration; + lidarConfiguration.m_lidarParameters.m_model = LidarTemplate::LidarModel::Custom3DLidar; + lidarConfiguration.m_lidarParameters.m_name = AZStd::string(sdfSensor.Name().c_str()); + lidarConfiguration.m_lidarParameters.m_minHAngle = lidarSensor->HorizontalScanMinAngle().Degree(); + lidarConfiguration.m_lidarParameters.m_maxHAngle = lidarSensor->HorizontalScanMaxAngle().Degree(); + lidarConfiguration.m_lidarParameters.m_minVAngle = lidarSensor->VerticalScanMinAngle().Degree(); + lidarConfiguration.m_lidarParameters.m_maxVAngle = lidarSensor->VerticalScanMaxAngle().Degree(); + lidarConfiguration.m_lidarParameters.m_layers = lidarSensor->HorizontalScanSamples(); + lidarConfiguration.m_lidarParameters.m_numberOfIncrements = lidarSensor->VerticalScanSamples(); + lidarConfiguration.m_lidarParameters.m_minRange = lidarSensor->RangeMin(); + lidarConfiguration.m_lidarParameters.m_maxRange = lidarSensor->RangeMax(); + + // Create required components + Utils::CreateComponent(entity); + + // Create Lidar component + if (Utils::CreateComponent(entity, sensorConfiguration, lidarConfiguration)) + { + return AZ::Success(); + } + else + { + return AZ::Failure(AZStd::string("Failed to create ROS2 Lidar Sensor component")); + } + }; + + return importerHook; + } +} // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h new file mode 100644 index 000000000..2ba4d2636 --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h @@ -0,0 +1,30 @@ +/* + * 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 + +namespace ROS2::SDFormat +{ + namespace ROS2SensorHooks + { + //! Retrieve a sensor importer hook which is used to map SDFormat sensor of type camera, depth or rgbd into O3DE + //! ROS2CameraSensorComponent + SensorImporterHook ROS2CameraSensor(); + + //! Retrieve a sensor importer hook which is used to map SDFormat sensor of type navsat into O3DE ROS2GNSSSensorComponent + SensorImporterHook ROS2GNSSSensor(); + + //! Retrieve a sensor importer hook which is used to map SDFormat sensor of type imu into O3DE ROS2ImuSensorComponent + SensorImporterHook ROS2ImuSensor(); + + //! Retrieve a sensor importer hook which is used to map SDFormat sensor of type lidar into O3DE ROS2LidarSensorComponent + SensorImporterHook ROS2LidarSensor(); + } // namespace ROS2SensorHooks +} // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.cpp new file mode 100644 index 000000000..004a5755a --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.cpp @@ -0,0 +1,28 @@ +/* + * 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 "ROS2SensorHooksUtils.h" +#include + +namespace ROS2::SDFormat +{ + namespace ROS2SensorHooks + { + void Utils::AddTopicConfiguration( + SensorConfiguration& sensorConfig, + const AZStd::string& topic, + const AZStd::string& messageType, + const AZStd::string& configName) + { + TopicConfiguration config; + config.m_topic = topic; + config.m_type = messageType; + sensorConfig.m_publishersConfigurations.insert(AZStd::make_pair(configName, config)); + } + } // namespace ROS2SensorHooks +} // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h new file mode 100644 index 000000000..a11aa414c --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h @@ -0,0 +1,78 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include +#include + +namespace ROS2::SDFormat +{ + namespace ROS2SensorHooks + { + namespace Utils + { + //! Add a ROS2 topic configuration to sensor parameters. + //! @param sensorConfig sensor's configuration which hosts multiple topic configurations + //! @param topic ROS2 topic name + //! @param messageType ROS2 message type + //! @param configName name under which topic configuration is stored in sensor's configuration + void AddTopicConfiguration( + SensorConfiguration& sensorConfig, + const AZStd::string& topic, + const AZStd::string& messageType, + const AZStd::string& configName); + + //! Create a component and attach the component to the entity. + //! This method ensures that game components are wrapped into GenericComponentWrapper. + //! @param entity entity to which the new component is added + //! @param args constructor arguments used to create the new component + //! @return A pointer to the component. Returns a null pointer if the component could not be created. + template + AZ::Component* CreateComponent(AZ::Entity& entity, Args&&... args) + { + // Do not create a component if the same type is already added. + if (entity.FindComponent()) + { + return nullptr; + } + + // Create component. + // If it's not an "editor component" then wrap it in a GenericComponentWrapper. + AZ::Component* component = nullptr; + if (AZ::GetRttiHelper() && + AZ::GetRttiHelper()->IsTypeOf(AzToolsFramework::Components::EditorComponentBase::RTTI_Type())) + { + component = aznew ComponentType(AZStd::forward(args)...); + } + else + { + AZ::Component* gameComponent = aznew ComponentType(AZStd::forward(args)...); + component = aznew AzToolsFramework::Components::GenericComponentWrapper(gameComponent); + } + AZ_Assert(component, "Failed to create component: %s", AZ::AzTypeInfo::Name()); + + if (component) + { + if (!entity.IsComponentReadyToAdd(component) || !entity.AddComponent(component)) + { + delete component; + component = nullptr; + } + } + return component; + } + } // namespace Utils + } // namespace ROS2SensorHooks +} // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.cpp index c52d282b5..7a9f687c8 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.cpp @@ -7,9 +7,10 @@ */ #include "ArticulationsMaker.h" -#include "RobotImporter/Utils/DefaultSolverConfiguration.h" #include #include +#include +#include #include #include @@ -19,51 +20,59 @@ namespace ROS2 namespace { using ArticulationCfg = PhysX::EditorArticulationLinkConfiguration; - static const AZStd::unordered_map SupportedJointTypes{ { - { urdf::Joint::REVOLUTE, PhysX::ArticulationJointType::Hinge }, - { urdf::Joint::CONTINUOUS, PhysX::ArticulationJointType::Hinge }, - { urdf::Joint::PRISMATIC, PhysX::ArticulationJointType::Prismatic }, - { urdf::Joint::FIXED, PhysX::ArticulationJointType::Fix }, + static const AZStd::unordered_map SupportedJointTypes{ { + { sdf::JointType::REVOLUTE, PhysX::ArticulationJointType::Hinge }, + { sdf::JointType::CONTINUOUS, PhysX::ArticulationJointType::Hinge }, + { sdf::JointType::PRISMATIC, PhysX::ArticulationJointType::Prismatic }, + { sdf::JointType::FIXED, PhysX::ArticulationJointType::Fix }, } }; } // namespace - ArticulationCfg& AddToArticulationConfig(ArticulationCfg& articulationLinkConfiguration, const urdf::JointSharedPtr joint) + ArticulationCfg& AddToArticulationConfig(ArticulationCfg& articulationLinkConfiguration, const sdf::Joint* joint) { if (!joint) { return articulationLinkConfiguration; } - auto supportedArticulationType = SupportedJointTypes.find(joint->type); + auto supportedArticulationType = SupportedJointTypes.find(joint->Type()); AZ_Warning( "ArticulationsMaker", supportedArticulationType != SupportedJointTypes.end(), - "Articulations do not support type %d for URDF joint %s.", - joint->type, - joint->name.c_str()); + "Articulations do not support type %d for URDF/SDF joint %s.", + static_cast(joint->Type()), + joint->Name().c_str()); if (supportedArticulationType != SupportedJointTypes.end()) { const auto type = supportedArticulationType->second; articulationLinkConfiguration.m_articulationJointType = type; const AZ::Vector3 o3deJointDir{ 1.0, 0.0, 0.0 }; - const AZ::Vector3 jointAxis = URDF::TypeConversions::ConvertVector3(joint->axis); - const auto quaternion = - jointAxis.IsZero() ? AZ::Quaternion::CreateIdentity() : AZ::Quaternion::CreateShortestArc(o3deJointDir, jointAxis); + AZ::Vector3 jointCoordinateAxis = AZ::Vector3::CreateZero(); + auto quaternion = AZ::Quaternion::CreateIdentity(); + + const sdf::JointAxis* jointAxis = joint->Axis(); + if (jointAxis != nullptr) + { + jointCoordinateAxis = URDF::TypeConversions::ConvertVector3(jointAxis->Xyz()); + quaternion = + jointCoordinateAxis.IsZero() ? AZ::Quaternion::CreateIdentity() : AZ::Quaternion::CreateShortestArc(o3deJointDir, jointCoordinateAxis); + } + const AZ::Vector3 rotation = quaternion.GetEulerDegrees(); articulationLinkConfiguration.m_localRotation = rotation; - if (joint->limits) + if (jointAxis) { if (type == PhysX::ArticulationJointType::Hinge) { - const double limitUpper = AZ::RadToDeg(joint->limits->upper); - const double limitLower = AZ::RadToDeg(joint->limits->lower); + const double limitUpper = AZ::RadToDeg(jointAxis->Upper()); + const double limitLower = AZ::RadToDeg(jointAxis->Lower()); articulationLinkConfiguration.m_angularLimitNegative = limitLower; articulationLinkConfiguration.m_angularLimitPositive = limitUpper; } else if (type == PhysX::ArticulationJointType::Prismatic) { - articulationLinkConfiguration.m_linearLimitLower = joint->limits->upper; - articulationLinkConfiguration.m_linearLimitUpper = joint->limits->lower; + articulationLinkConfiguration.m_linearLimitLower = jointAxis->Upper(); + articulationLinkConfiguration.m_linearLimitUpper = jointAxis->Lower(); } } else @@ -74,37 +83,39 @@ namespace ROS2 return articulationLinkConfiguration; } - ArticulationCfg& AddToArticulationConfig(ArticulationCfg& articulationLinkConfiguration, const urdf::InertialSharedPtr inertial) + ArticulationCfg& AddToArticulationConfig(ArticulationCfg& articulationLinkConfiguration, const gz::math::Inertiald& inertial) { - if (!inertial) - { - return articulationLinkConfiguration; - } articulationLinkConfiguration.m_solverPositionIterations = AZStd::max(articulationLinkConfiguration.m_solverPositionIterations, URDF::DefaultNumberPosSolver); articulationLinkConfiguration.m_solverVelocityIterations = AZStd::max(articulationLinkConfiguration.m_solverVelocityIterations, URDF::DefaultNumberVelSolver); - articulationLinkConfiguration.m_mass = inertial->mass; - articulationLinkConfiguration.m_centerOfMassOffset = URDF::TypeConversions::ConvertVector3(inertial->origin.position); + articulationLinkConfiguration.m_mass = inertial.MassMatrix().Mass(); + articulationLinkConfiguration.m_centerOfMassOffset = URDF::TypeConversions::ConvertVector3(inertial.Pose().Pos()); - if (!URDF::TypeConversions::ConvertQuaternion(inertial->origin.rotation).IsIdentity()) + if (!URDF::TypeConversions::ConvertQuaternion(inertial.Pose().Rot()).IsIdentity()) { // There is a rotation component in URDF that we are not able to apply - AZ_Warning("AddArticulationLink", false, "Ignoring URDF inertial origin rotation (no such field in rigid body configuration)"); + AZ_Warning("AddArticulationLink", false, "Ignoring URDF/SDF inertial origin rotation (no such field in rigid body configuration)"); } return articulationLinkConfiguration; } - void ArticulationsMaker::AddArticulationLink(const urdf::LinkSharedPtr link, AZ::EntityId entityId) const + void ArticulationsMaker::AddArticulationLink(const sdf::Model& model, const sdf::Link* link, AZ::EntityId entityId) const { AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId); AZ_Assert(entity, "No entity for id %s", entityId.ToString().c_str()); - AZ_TracePrintf("ArticulationsMaker", "Processing inertial for entity id: %s\n", entityId.ToString().c_str()); + AZ_Trace("ArticulationsMaker", "Processing inertial for entity id: %s\n", entityId.ToString().c_str()); PhysX::EditorArticulationLinkConfiguration articulationLinkConfiguration; - articulationLinkConfiguration = AddToArticulationConfig(articulationLinkConfiguration, link->inertial); - articulationLinkConfiguration = AddToArticulationConfig(articulationLinkConfiguration, link->parent_joint); + articulationLinkConfiguration = AddToArticulationConfig(articulationLinkConfiguration, link->Inertial()); + + constexpr bool getNestedModelJoints = true; + AZStd::string linkName(link->Name().c_str(), link->Name().size()); + for (const sdf::Joint* joint : Utils::GetJointsForChildLink(model, linkName, getNestedModelJoints)) + { + articulationLinkConfiguration = AddToArticulationConfig(articulationLinkConfiguration, joint); + } entity->CreateComponent(articulationLinkConfiguration); } diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.h index fd5a9e39a..42c8a9038 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.h @@ -20,8 +20,10 @@ namespace ROS2 { public: //! Add zero or one inertial and joints elements to a given entity (depending on link content). - //! @param link A pointer to a parsed URDF link. + //! @param model SDF model object which can be queried to locate the joints needed to determine if the supplied + //! link is a child link within a joint + //! @param link A pointer to a parsed SDF link. //! @param entityId A non-active entity which will be populated according to inertial content. - void AddArticulationLink(urdf::LinkSharedPtr link, AZ::EntityId entityId) const; + void AddArticulationLink(const sdf::Model& model, const sdf::Link* link, AZ::EntityId entityId) const; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.cpp index 59dbfc2f8..0172c44f0 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.cpp @@ -37,7 +37,6 @@ namespace ROS2 CollidersMaker::CollidersMaker(const AZStd::shared_ptr& urdfAssetsMapping) : m_urdfAssetsMapping(urdfAssetsMapping) { - FindWheelMaterial(); } void CollidersMaker::FindWheelMaterial() @@ -58,182 +57,38 @@ namespace ROS2 { m_wheelMaterial = AZ::Data::Asset(assetId, Physics::MaterialAsset::TYPEINFO_Uuid(), physicsMaterialAssetRelPath); - AZ_TracePrintf(Internal::CollidersMakerLoggingTag, "Waiting for asset load\n"); - m_wheelMaterial.BlockUntilLoadComplete(); } else { - AZ_Warning(Internal::CollidersMakerLoggingTag, false, "Cannot load wheel material"); + AZ_Warning(Internal::CollidersMakerLoggingTag, false, "Cannot locate wheel material asset"); } } - void CollidersMaker::BuildColliders(urdf::LinkSharedPtr link) - { - for (auto collider : link->collision_array) - { - BuildCollider(collider); - } - - if (link->collision_array.empty()) - { - BuildCollider(link->collision); - } - } - - void CollidersMaker::BuildCollider(urdf::CollisionSharedPtr collision) - { - if (!collision) - { // it is ok not to have collision in a link - return; - } - - auto geometry = collision->geometry; - bool isPrimitiveShape = geometry->type != urdf::Geometry::MESH; - if (!isPrimitiveShape) - { - auto meshGeometry = std::dynamic_pointer_cast(geometry); - if (!meshGeometry) - { - return; - } - const auto asset = PrefabMakerUtils::GetAssetFromPath(*m_urdfAssetsMapping, meshGeometry->filename); - if (!asset) - { - return; - } - const AZStd::string& azMeshPath = asset->m_sourceAssetGlobalPath; - - AZStd::shared_ptr scene; - AZ::SceneAPI::Events::SceneSerializationBus::BroadcastResult( - scene, &AZ::SceneAPI::Events::SceneSerialization::LoadScene, azMeshPath.c_str(), AZ::Uuid::CreateNull(), ""); - if (!scene) - { - AZ_Error( - Internal::CollidersMakerLoggingTag, - false, - "Error loading collider. Invalid scene: %s, URDF path: %s", - azMeshPath.c_str(), - meshGeometry->filename.c_str()); - return; - } - - AZ::SceneAPI::Containers::SceneManifest& manifest = scene->GetManifest(); - auto valueStorage = manifest.GetValueStorage(); - if (valueStorage.empty()) - { - AZ_Error( - Internal::CollidersMakerLoggingTag, false, "Error loading collider. Invalid value storage: %s", azMeshPath.c_str()); - return; - } - - auto view = AZ::SceneAPI::Containers::MakeDerivedFilterView(valueStorage); - if (view.empty()) - { - AZ_Error(Internal::CollidersMakerLoggingTag, false, "Error loading collider. Invalid node views: %s", azMeshPath.c_str()); - return; - } - - // Select all nodes for both visual and collision nodes - for (AZ::SceneAPI::DataTypes::ISceneNodeGroup& mg : view) - { - AZ::SceneAPI::Utilities::SceneGraphSelector::SelectAll(scene->GetGraph(), mg.GetSceneNodeSelectionList()); - } - - // Update scene with all nodes selected - AZ::SceneAPI::Events::ProcessingResultCombiner result; - AZ::SceneAPI::Events::AssetImportRequestBus::BroadcastResult( - result, - &AZ::SceneAPI::Events::AssetImportRequest::UpdateManifest, - *scene, - AZ::SceneAPI::Events::AssetImportRequest::ManifestAction::Update, - AZ::SceneAPI::Events::AssetImportRequest::RequestingApplication::Editor); - - if (result.GetResult() != AZ::SceneAPI::Events::ProcessingResult::Success) - { - AZ_TracePrintf(Internal::CollidersMakerLoggingTag, "Scene updated\n"); - return; - } - - auto assetInfoFilePath = AZ::IO::Path{ azMeshPath }; - assetInfoFilePath.Native() += ".assetinfo"; - AZ_Printf(Internal::CollidersMakerLoggingTag, "Saving collider manifest to %s\n", assetInfoFilePath.c_str()); - scene->GetManifest().SaveToFile(assetInfoFilePath.c_str()); - - // Set export method to convex mesh - auto readOutcome = AZ::JsonSerializationUtils::ReadJsonFile(assetInfoFilePath.c_str()); - if (!readOutcome.IsSuccess()) - { - AZ_Error( - Internal::CollidersMakerLoggingTag, - false, - "Could not read %s with %s", - assetInfoFilePath.c_str(), - readOutcome.GetError().c_str()); - return; - } - rapidjson::Document assetInfoJson = readOutcome.TakeValue(); - auto manifestObject = assetInfoJson.GetObject(); - auto valuesIterator = manifestObject.FindMember("values"); - if (valuesIterator == manifestObject.MemberEnd()) - { - AZ_Error( - Internal::CollidersMakerLoggingTag, false, "Invalid json file: %s (Missing 'values' node)", assetInfoFilePath.c_str()); - return; - } - - constexpr AZStd::string_view physXMeshGroupType = "{5B03C8E6-8CEE-4DA0-A7FA-CD88689DD45B} MeshGroup"; - auto valuesArray = valuesIterator->value.GetArray(); - for (auto& value : valuesArray) - { - auto object = value.GetObject(); - - auto physXMeshGroupIterator = object.FindMember("$type"); - if (AZ::StringFunc::Equal(physXMeshGroupIterator->value.GetString(), physXMeshGroupType)) - { - value.AddMember(rapidjson::StringRef("export method"), rapidjson::StringRef("1"), assetInfoJson.GetAllocator()); - } - } - - auto saveOutcome = AZ::JsonSerializationUtils::WriteJsonFile(assetInfoJson, assetInfoFilePath.c_str()); - if (!saveOutcome.IsSuccess()) - { - AZ_Error( - Internal::CollidersMakerLoggingTag, - false, - "Could not save %s with %s", - assetInfoFilePath.c_str(), - saveOutcome.GetError().c_str()); - return; - } - } - } - - void CollidersMaker::AddColliders(urdf::LinkSharedPtr link, AZ::EntityId entityId) + void CollidersMaker::AddColliders(const sdf::Model& model, const sdf::Link* link, AZ::EntityId entityId) { AZStd::string typeString = "collider"; - const bool isWheelEntity = Utils::IsWheelURDFHeuristics(link); + const bool isWheelEntity = Utils::IsWheelURDFHeuristics(model, link); if (isWheelEntity) { - AZ_Printf(Internal::CollidersMakerLoggingTag, "Due to its name, %s is considered a wheel entity\n", link->name.c_str()); + AZ_Printf(Internal::CollidersMakerLoggingTag, "Due to its name, %s is considered a wheel entity\n", link->Name().c_str()); + if (!m_wheelMaterial.GetId().IsValid()) + { + FindWheelMaterial(); + } } const AZ::Data::Asset materialAsset = isWheelEntity ? m_wheelMaterial : AZ::Data::Asset(); size_t nameSuffixIndex = 0; // For disambiguation when multiple unnamed colliders are present. The order does not matter here - for (auto collider : link->collision_array) + for (uint64_t index = 0; index < link->CollisionCount(); index++) { // Add colliders (if any) from the collision array AddCollider( - collider, entityId, PrefabMakerUtils::MakeEntityName(link->name.c_str(), typeString, nameSuffixIndex), materialAsset); + link->CollisionByIndex(index), entityId, PrefabMakerUtils::MakeEntityName(link->Name().c_str(), typeString, nameSuffixIndex), materialAsset); nameSuffixIndex++; } - - if (nameSuffixIndex == 0) - { // If there are no colliders in the array, the element member is used instead - AddCollider(link->collision, entityId, PrefabMakerUtils::MakeEntityName(link->name.c_str(), typeString), materialAsset); - } } void CollidersMaker::AddCollider( - urdf::CollisionSharedPtr collision, + const sdf::Collision* collision, AZ::EntityId entityId, const AZStd::string& generatedName, const AZ::Data::Asset& materialAsset) @@ -242,9 +97,9 @@ namespace ROS2 { // it is ok not to have collision in a link return; } - AZ_TracePrintf(Internal::CollidersMakerLoggingTag, "Processing collisions for entity id:%s\n", entityId.ToString().c_str()); + AZ_Trace(Internal::CollidersMakerLoggingTag, "Processing collisions for entity id:%s\n", entityId.ToString().c_str()); - auto geometry = collision->geometry; + auto geometry = collision->Geom(); if (!geometry) { // non-empty visual should have a geometry AZ_Warning(Internal::CollidersMakerLoggingTag, false, "No Geometry for a collider of entity %s", entityId.ToString().c_str()); @@ -255,44 +110,37 @@ namespace ROS2 } void CollidersMaker::AddColliderToEntity( - urdf::CollisionSharedPtr collision, AZ::EntityId entityId, const AZ::Data::Asset& materialAsset) const + const sdf::Collision* collision, AZ::EntityId entityId, const AZ::Data::Asset& materialAsset) const { AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId); AZ_Assert(entity, "AddColliderToEntity called with invalid entityId"); - auto geometry = collision->geometry; - bool isPrimitiveShape = geometry->type != urdf::Geometry::MESH; + auto geometry = collision->Geom(); + bool isPrimitiveShape = geometry->Type() != sdf::GeometryType::MESH; Physics::ColliderConfiguration colliderConfig; colliderConfig.m_materialSlots.SetMaterialAsset(0, materialAsset); - colliderConfig.m_position = URDF::TypeConversions::ConvertVector3(collision->origin.position); - colliderConfig.m_rotation = URDF::TypeConversions::ConvertQuaternion(collision->origin.rotation); + colliderConfig.m_position = URDF::TypeConversions::ConvertVector3(collision->RawPose().Pos()); + colliderConfig.m_rotation = URDF::TypeConversions::ConvertQuaternion(collision->RawPose().Rot()); if (!isPrimitiveShape) { AZ_Printf(Internal::CollidersMakerLoggingTag, "Adding mesh collider to %s\n", entityId.ToString().c_str()); - auto meshGeometry = std::dynamic_pointer_cast(geometry); + auto meshGeometry = geometry->MeshShape(); AZ_Assert(meshGeometry, "geometry is not meshGeometry"); - auto asset = PrefabMakerUtils::GetAssetFromPath(*m_urdfAssetsMapping, meshGeometry->filename); + auto asset = PrefabMakerUtils::GetAssetFromPath(*m_urdfAssetsMapping, meshGeometry->Uri()); if (!asset) { return; } - AZStd::string pxmodelPath = Utils::GetPhysXMeshProductAsset(asset->m_sourceGuid); - if (pxmodelPath.empty()) + AZ::Data::AssetId assetId = Utils::GetPhysXMeshProductAssetId(asset->m_sourceGuid); + if (!assetId.IsValid()) { AZ_Error( Internal::CollidersMakerLoggingTag, false, "Could not find pxmodel for %s", asset->m_sourceAssetGlobalPath.c_str()); - entity->Deactivate(); return; } - AZ_Printf(Internal::CollidersMakerLoggingTag, "pxmodelPath %s\n", pxmodelPath.c_str()); - // Get asset product id (pxmesh) - AZ::Data::AssetId assetId; - const AZ::Data::AssetType PhysxMeshAssetType = azrtti_typeid(); - AZ::Data::AssetCatalogRequestBus::BroadcastResult( - assetId, &AZ::Data::AssetCatalogRequests::GetAssetIdByPath, pxmodelPath.c_str(), PhysxMeshAssetType, true); AZ_Printf( Internal::CollidersMakerLoggingTag, "Collider %s has assetId %s\n", @@ -300,38 +148,40 @@ namespace ROS2 assetId.ToString().c_str()); Physics::PhysicsAssetShapeConfiguration shapeConfiguration; + auto scale = geometry->MeshShape()->Scale(); + shapeConfiguration.m_assetScale = AZ::Vector3(scale.X(), scale.Y(), scale.Z()); shapeConfiguration.m_useMaterialsFromAsset = false; if (assetId.IsValid()) { - auto mesh = AZ::Data::Asset(assetId, PhysxMeshAssetType); + auto mesh = AZ::Data::Asset(assetId, azrtti_typeid()); shapeConfiguration.m_asset = mesh; entity->CreateComponent(colliderConfig, shapeConfiguration); } return; } - AZ_Printf(Internal::CollidersMakerLoggingTag, "URDF geometry type: %d\n", geometry->type); - switch (geometry->type) + AZ_Printf(Internal::CollidersMakerLoggingTag, "URDF/SDF geometry type: %d\n", (int)geometry->Type()); + switch (geometry->Type()) { - case urdf::Geometry::SPHERE: + case sdf::GeometryType::SPHERE: { - auto sphereGeometry = std::dynamic_pointer_cast(geometry); + auto sphereGeometry = geometry->SphereShape(); AZ_Assert(sphereGeometry, "geometry is not sphereGeometry"); - const Physics::SphereShapeConfiguration cfg{ static_cast(sphereGeometry->radius) }; + const Physics::SphereShapeConfiguration cfg{ static_cast(sphereGeometry->Radius()) }; entity->CreateComponent(colliderConfig, cfg); } break; - case urdf::Geometry::BOX: + case sdf::GeometryType::BOX: { - const auto boxGeometry = std::dynamic_pointer_cast(geometry); + const auto boxGeometry = geometry->BoxShape(); AZ_Assert(boxGeometry, "geometry is not boxGeometry"); - const Physics::BoxShapeConfiguration cfg{ URDF::TypeConversions::ConvertVector3(boxGeometry->dim) }; + const Physics::BoxShapeConfiguration cfg{ URDF::TypeConversions::ConvertVector3(boxGeometry->Size()) }; entity->CreateComponent(colliderConfig, cfg); } break; - case urdf::Geometry::CYLINDER: + case sdf::GeometryType::CYLINDER: { - auto cylinderGeometry = std::dynamic_pointer_cast(geometry); + auto cylinderGeometry = geometry->CylinderShape(); AZ_Assert(cylinderGeometry, "geometry is not cylinderGeometry"); Physics::BoxShapeConfiguration cfg; auto* component = entity->CreateComponent(colliderConfig, cfg); @@ -345,11 +195,11 @@ namespace ROS2 PhysX::EditorPrimitiveColliderComponentRequestBus::Event( AZ::EntityComponentIdPair(entityId, component->GetId()), &PhysX::EditorPrimitiveColliderComponentRequests::SetCylinderHeight, - cylinderGeometry->length); + cylinderGeometry->Length()); PhysX::EditorPrimitiveColliderComponentRequestBus::Event( AZ::EntityComponentIdPair(entityId, component->GetId()), &PhysX::EditorPrimitiveColliderComponentRequests::SetCylinderRadius, - cylinderGeometry->radius); + cylinderGeometry->Radius()); PhysX::EditorPrimitiveColliderComponentRequestBus::Event( AZ::EntityComponentIdPair(entityId, component->GetId()), &PhysX::EditorPrimitiveColliderComponentRequests::SetCylinderSubdivisionCount, @@ -358,12 +208,12 @@ namespace ROS2 } else { - AZ_Warning(Internal::CollidersMakerLoggingTag, false, "The entity was no activated %s", entity->GetName().c_str()); + AZ_Warning(Internal::CollidersMakerLoggingTag, false, "The entity was not activated %s", entity->GetName().c_str()); } } break; default: - AZ_Warning(Internal::CollidersMakerLoggingTag, false, "Unsupported collider geometry type: %d", geometry->type); + AZ_Warning(Internal::CollidersMakerLoggingTag, false, "Unsupported collider geometry type: %d", (int)geometry->Type()); break; } } diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.h index db9fb916b..5ea9284a1 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.h @@ -30,34 +30,31 @@ namespace ROS2 class CollidersMaker { public: - //! Construct the class based on URDF asset mapping. - //! @param urdfAssetsMapping a prepared mapping of Assets used by the source URDF. - CollidersMaker(const AZStd::shared_ptr& urdfAssetsMapping); + //! Construct the class based on SDF asset mapping. + //! @param sdfAssetsMapping a prepared mapping of Assets used by the source URDF/SDF. + CollidersMaker(const AZStd::shared_ptr& sdfAssetsMapping); //! Prevent copying of existing CollidersMaker CollidersMaker(const CollidersMaker& other) = delete; - //! Builds .pxmeshes for every collider in link collider mesh. - //! @param link A parsed URDF tree link node which could hold information about colliders. - void BuildColliders(urdf::LinkSharedPtr link); //! Add zero, one or many collider elements (depending on link content). - //! @param link A parsed URDF tree link node which could hold information about colliders. + //! @param model An SDF model object provided by libsdformat from a parsed URDF/SDF + //! @param link A parsed SDF tree link node which could hold information about colliders. //! @param entityId A non-active entity which will be affected. - void AddColliders(urdf::LinkSharedPtr link, AZ::EntityId entityId); + void AddColliders(const sdf::Model& model, const sdf::Link* link, AZ::EntityId entityId); //! Sends meshes required for colliders to asset processor. //! @param buildReadyCb Function to call when the processing finishes. void ProcessMeshes(BuildReadyCallback notifyBuildReadyCb); private: void FindWheelMaterial(); - void BuildCollider(urdf::CollisionSharedPtr collision); void AddCollider( - urdf::CollisionSharedPtr collision, + const sdf::Collision* collision, AZ::EntityId entityId, const AZStd::string& generatedName, const AZ::Data::Asset& materialAsset); void AddColliderToEntity( - urdf::CollisionSharedPtr collision, AZ::EntityId entityId, const AZ::Data::Asset& materialAsset) const; + const sdf::Collision* collision, AZ::EntityId entityId, const AZ::Data::Asset& materialAsset) const; AZ::Data::Asset m_wheelMaterial; AZStd::shared_ptr m_urdfAssetsMapping; diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/InertialsMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/InertialsMaker.cpp index 7361d3386..ae7863076 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/InertialsMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/InertialsMaker.cpp @@ -15,13 +15,9 @@ namespace ROS2 { - void InertialsMaker::AddInertial(urdf::InertialSharedPtr inertial, AZ::EntityId entityId) const + void InertialsMaker::AddInertial(const gz::math::Inertiald& inertial, AZ::EntityId entityId) const { - if (!inertial) - { // it is ok not to have inertia in a link - return; - } - AZ_TracePrintf("AddInertial", "Processing inertial for entity id: %s\n", entityId.ToString().c_str()); + AZ_Trace("AddInertial", "Processing inertial for entity id: %s\n", entityId.ToString().c_str()); AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId); PhysX::EditorRigidBodyConfiguration rigidBodyConfiguration; @@ -31,22 +27,24 @@ namespace ROS2 physxSpecificConfiguration.m_solverVelocityIterations = AZStd::max(physxSpecificConfiguration.m_solverVelocityIterations, URDF::DefaultNumberVelSolver); - rigidBodyConfiguration.m_mass = inertial->mass; + rigidBodyConfiguration.m_mass = inertial.MassMatrix().Mass(); rigidBodyConfiguration.m_computeMass = false; - rigidBodyConfiguration.m_centerOfMassOffset = URDF::TypeConversions::ConvertVector3(inertial->origin.position); + rigidBodyConfiguration.m_centerOfMassOffset = URDF::TypeConversions::ConvertVector3(inertial.Pose().Pos()); rigidBodyConfiguration.m_computeCenterOfMass = false; - if (!URDF::TypeConversions::ConvertQuaternion(inertial->origin.rotation).IsIdentity()) + if (!URDF::TypeConversions::ConvertQuaternion(inertial.Pose().Rot()).IsIdentity()) { // There is a rotation component in URDF that we are not able to apply - AZ_Warning("AddInertial", false, "Ignoring URDF inertial origin rotation (no such field in rigid body configuration)"); + AZ_Warning("AddInertial", false, "Ignoring URDF/SDF inertial origin rotation (no such field in rigid body configuration)"); } + auto moi = inertial.Moi(); + // Inertia tensor is symmetrical auto inertiaMatrix = AZ::Matrix3x3::CreateFromRows( - AZ::Vector3(inertial->ixx, inertial->ixy, inertial->ixz), - AZ::Vector3(inertial->ixy, inertial->iyy, inertial->iyz), - AZ::Vector3(inertial->ixz, inertial->iyz, inertial->izz)); + AZ::Vector3(moi(0, 0), moi(0, 1), moi(0, 2)), + AZ::Vector3(moi(0, 1), moi(1, 1), moi(1, 2)), + AZ::Vector3(moi(0, 2), moi(1, 2), moi(2, 2))); rigidBodyConfiguration.m_inertiaTensor = inertiaMatrix; rigidBodyConfiguration.m_computeInertiaTensor = false; diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/InertialsMaker.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/InertialsMaker.h index 2475225a1..f16be09db 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/InertialsMaker.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/InertialsMaker.h @@ -18,8 +18,8 @@ namespace ROS2 { public: //! Add zero or one inertial elements to a given entity (depending on link content). - //! @param inertial A pointer to a parsed URDF inertial structure, might be null. + //! @param inertial A pointer to a parsed SDF inertial structure, might be null. //! @param entityId A non-active entity which will be populated according to inertial content. - void AddInertial(urdf::InertialSharedPtr inertial, AZ::EntityId entityId) const; + void AddInertial(const gz::math::Inertiald& inertial, AZ::EntityId entityId) const; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.cpp index 0e7b84d9a..f44e64a36 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.cpp @@ -18,40 +18,45 @@ namespace ROS2 { - JointsMaker::JointsMakerResult JointsMaker::AddJointComponent( - urdf::JointSharedPtr joint, AZ::EntityId followColliderEntityId, AZ::EntityId leadColliderEntityId) const + const sdf::Joint* joint, AZ::EntityId followColliderEntityId, AZ::EntityId leadColliderEntityId) const { AZ::Entity* followColliderEntity = AzToolsFramework::GetEntityById(followColliderEntityId); PhysX::EditorJointComponent* jointComponent = nullptr; - // URDF has a joint axis configurable by a normalized vector - that is given by the 'axis' sub-element in the joint element. + // URDF/SDF has a joint axis configurable by a normalized vector - that is given by the 'axis' sub-element in the joint element. // The o3de has a slightly different way of configuring the axis of the joint. The o3de has an axis around positive `X` and rotation // with Euler angles can be applied to configure the desirable direction of the joint. A quaternion that transforms a unit vector X - // {1,0,0} to a vector given by the URDF joint need to be found. Heavily suboptimal element in this conversion is needed of + // {1,0,0} to a vector given by the URDF/SDF joint need to be found. Heavily suboptimal element in this conversion is needed of // converting the unit quaternion to Euler vector. - const AZ::Vector3 o3de_joint_dir{ 1.0, 0.0, 0.0 }; - const AZ::Vector3 joint_axis = URDF::TypeConversions::ConvertVector3(joint->axis); - const auto quaternion = - joint_axis.IsZero() ? AZ::Quaternion::CreateIdentity() : AZ::Quaternion::CreateShortestArc(o3de_joint_dir, joint_axis); + const AZ::Vector3 o3deJointDir{ 1.0, 0.0, 0.0 }; + const sdf::JointAxis* jointAxis = joint->Axis(); + AZ::Vector3 jointCoordinateAxis = AZ::Vector3::CreateZero(); + auto quaternion = AZ::Quaternion::CreateIdentity(); + if (jointAxis != nullptr) + { + jointCoordinateAxis = URDF::TypeConversions::ConvertVector3(jointAxis->Xyz()); + quaternion = + jointCoordinateAxis.IsZero() ? AZ::Quaternion::CreateIdentity() : AZ::Quaternion::CreateShortestArc(o3deJointDir, jointCoordinateAxis); + } AZ_Printf( "JointsMaker", - "Quaternion from URDF to o3de %f, %f, %f, %f", + "Quaternion from URDF/SDF to o3de %f, %f, %f, %f", quaternion.GetX(), quaternion.GetY(), quaternion.GetZ(), quaternion.GetW()); const AZ::Vector3 rotation = quaternion.GetEulerDegrees(); - switch (joint->type) + switch (joint->Type()) { - case urdf::Joint::FIXED: + case sdf::JointType::FIXED: { jointComponent = followColliderEntity->CreateComponent(); } break; - case urdf::Joint::PRISMATIC: + case sdf::JointType::PRISMATIC: { jointComponent = followColliderEntity->CreateComponent(); followColliderEntity->Activate(); @@ -62,16 +67,18 @@ namespace ROS2 PhysX::JointsComponentModeCommon::ParameterNames::Rotation, rotation); - PhysX::EditorJointRequestBus::Event( - AZ::EntityComponentIdPair(followColliderEntityId, jointComponent->GetId()), - &PhysX::EditorJointRequests::SetLinearValuePair, - PhysX::JointsComponentModeCommon::ParameterNames::LinearLimits, - PhysX::AngleLimitsFloatPair(joint->limits->upper, joint->limits->lower)); - + if (jointAxis != nullptr) + { + PhysX::EditorJointRequestBus::Event( + AZ::EntityComponentIdPair(followColliderEntityId, jointComponent->GetId()), + &PhysX::EditorJointRequests::SetLinearValuePair, + PhysX::JointsComponentModeCommon::ParameterNames::LinearLimits, + PhysX::AngleLimitsFloatPair(jointAxis->Upper(), jointAxis->Lower())); + } followColliderEntity->Deactivate(); } break; - case urdf::Joint::CONTINUOUS: + case sdf::JointType::CONTINUOUS: { // Implemented as Hinge with angular limit disabled jointComponent = followColliderEntity->CreateComponent(); followColliderEntity->Activate(); @@ -89,35 +96,48 @@ namespace ROS2 followColliderEntity->Deactivate(); } break; - case urdf::Joint::REVOLUTE: + case sdf::JointType::REVOLUTE: { // Hinge jointComponent = followColliderEntity->CreateComponent(); followColliderEntity->Activate(); - const double limitUpper = AZ::RadToDeg(joint->limits->upper); - const double limitLower = AZ::RadToDeg(joint->limits->lower); - AZ_Printf( - "JointsMaker", - "Setting limits : upper: %.1f lower: %.1f (URDF:%f,%f)", - limitUpper, - limitLower, - joint->limits->upper, - joint->limits->lower); - PhysX::EditorJointRequestBus::Event( - AZ::EntityComponentIdPair(followColliderEntityId, jointComponent->GetId()), - [&rotation, &limitLower, &limitUpper](PhysX::EditorJointRequests* editorJointRequest) - { - editorJointRequest->SetVector3Value(PhysX::JointsComponentModeCommon::ParameterNames::Rotation, rotation); - editorJointRequest->SetLinearValuePair( - PhysX::JointsComponentModeCommon::ParameterNames::TwistLimits, - PhysX::AngleLimitsFloatPair(limitUpper, limitLower)); - }); + if (jointAxis != nullptr) + { + using LimitType = decltype(jointAxis->Upper()); + const bool enableJointLimits = jointAxis->Upper() != AZStd::numeric_limits::infinity() + || jointAxis->Lower() != -AZStd::numeric_limits::infinity(); + const double limitUpper = jointAxis->Upper() != AZStd::numeric_limits::infinity() + ? AZ::RadToDeg(jointAxis->Upper()) + : AZ::RadToDeg(AZ::Constants::TwoPi); + const double limitLower = jointAxis->Lower() != -AZStd::numeric_limits::infinity() + ? AZ::RadToDeg(jointAxis->Lower()) + : -AZ::RadToDeg(AZ::Constants::TwoPi); + AZ_Printf( + "JointsMaker", + "Setting limits : upper: %.1f lower: %.1f (URDF/SDF:%f,%f)", + limitUpper, + limitLower, + jointAxis->Upper(), + jointAxis->Lower()); + PhysX::EditorJointRequestBus::Event( + AZ::EntityComponentIdPair(followColliderEntityId, jointComponent->GetId()), + [&rotation, enableJointLimits, &limitLower, &limitUpper](PhysX::EditorJointRequests* editorJointRequest) + { + editorJointRequest->SetVector3Value(PhysX::JointsComponentModeCommon::ParameterNames::Rotation, rotation); + editorJointRequest->SetLinearValuePair( + PhysX::JointsComponentModeCommon::ParameterNames::TwistLimits, + PhysX::AngleLimitsFloatPair(limitUpper, limitLower)); + editorJointRequest->SetBoolValue( + PhysX::JointsComponentModeCommon::ParameterNames::EnableLimits, enableJointLimits); + }); + } followColliderEntity->Deactivate(); } break; default: - AZ_Warning("AddJointComponent", false, "Unknown or unsupported joint type %d for joint %s", joint->type, joint->name.c_str()); - return AZ::Failure(AZStd::string::format("unsupported joint type : %d", joint->type)); + AZ_Warning("AddJointComponent", false, "Unknown or unsupported joint type %d for joint %s", + static_cast(joint->Type()), joint->Name().c_str()); + return AZ::Failure(AZStd::string::format("unsupported joint type : %d", static_cast(joint->Type()))); } followColliderEntity->Activate(); diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.h index cdf2f4253..5ce2accc8 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.h @@ -16,18 +16,18 @@ namespace ROS2 { //! Populates a given entity with all the contents of the tag in robot description. - //! In URDF, joints are specified between two given links, but in PhysX they are between two Bodies / Colliders. + //! In URDF/SDF, joints are specified between two given links, but in PhysX they are between two Bodies / Colliders. class JointsMaker { public: using JointsMakerResult = AZ::Outcome; - //! Add a joint to an entity and sets it accordingly to urdf::Joint + //! Add a joint to an entity and sets it accordingly to sdf::Joint //! @param joint Joint data //! @param followColliderEntityId A non-active entity which will be populated with Joint components. //! @param leadColliderEntityId An entity higher in hierarchy which is connected through the joint with the child entity. //! @returns created components Id or string with fail JointsMakerResult AddJointComponent( - urdf::JointSharedPtr joint, AZ::EntityId followColliderEntityId, AZ::EntityId leadColliderEntityId) const; + const sdf::Joint* joint, AZ::EntityId followColliderEntityId, AZ::EntityId leadColliderEntityId) const; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp index c0e5476db..7c0e0774e 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp @@ -13,6 +13,8 @@ #include #include #include +#include +#include #include #include #include @@ -43,10 +45,10 @@ namespace ROS2::PrefabMakerUtils return assetPath; } - void SetEntityTransformLocal(const urdf::Pose& origin, AZ::EntityId entityId) + void SetEntityTransformLocal(const gz::math::Pose3d& origin, AZ::EntityId entityId) { - urdf::Vector3 urdfPosition = origin.position; - urdf::Rotation urdfRotation = origin.rotation; + gz::math::Vector3 urdfPosition = origin.Pos(); + gz::math::Quaternion urdfRotation = origin.Rot(); AZ::Quaternion azRotation = URDF::TypeConversions::ConvertQuaternion(urdfRotation); AZ::Vector3 azPosition = URDF::TypeConversions::ConvertVector3(urdfPosition); AZ::Transform tf(azPosition, azRotation, 1.0f); @@ -64,27 +66,47 @@ namespace ROS2::PrefabMakerUtils AzToolsFramework::Prefab::PrefabEntityResult CreateEntity(AZ::EntityId parentEntityId, const AZStd::string& name) { - auto* prefabInterface = AZ::Interface::Get(); - auto createEntityResult = prefabInterface->CreateEntity(parentEntityId, AZ::Vector3()); - if (!createEntityResult.IsSuccess()) + // Create an entity with the appropriate Editor components, but in a not-yet-activated state. + AZ::EntityId entityId; + AzToolsFramework::EntityUtilityBus::BroadcastResult( + entityId, &AzToolsFramework::EntityUtilityBus::Events::CreateEditorReadyEntity, name); + + if (entityId.IsValid() == false) { - return createEntityResult; + return AZ::Failure(AZStd::string("Invalid id for created entity")); } - - // Verify that a valid entity is created. - AZ::EntityId entityId = createEntityResult.GetValue(); - if (!entityId.IsValid()) + AZ_Trace("CreateEntity", "Processing entity id: %s with name: %s\n", entityId.ToString().c_str(), name.c_str()); + + // If the parent is invalid, parent to the container of the currently focused prefab if one exists. + if (!parentEntityId.IsValid()) { - return AZ::Failure(AZStd::string("Invalid id for created entity")); + AzFramework::EntityContextId editorEntityContextId = AzToolsFramework::GetEntityContextId(); + + auto prefabFocusPublicInterface = AZ::Interface::Get(); + if (prefabFocusPublicInterface) + { + parentEntityId = prefabFocusPublicInterface->GetFocusedPrefabContainerEntityId(editorEntityContextId); + } } - AZ_TracePrintf("CreateEntity", "Processing entity id: %s with name: %s\n", entityId.ToString().c_str(), name.c_str()); - AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId); - entity->SetName(name); - entity->Deactivate(); - AddRequiredComponentsToEntity(entityId); - return createEntityResult; + SetEntityParent(entityId, parentEntityId); + + return entityId; + } + + void SetEntityParent(AZ::EntityId entityId, AZ::EntityId parentEntityId) + { + auto* entity = AzToolsFramework::GetEntityById(entityId); + AZ_Assert(entity, "Unknown entity %s", entityId.ToString().c_str()); + AZ_Assert( + (entity->GetState() == AZ::Entity::State::Constructed) || (entity->GetState() == AZ::Entity::State::Init), + "Entity must be inactive when getting reparented."); + + if (auto* transformComponent = entity->FindComponent(); transformComponent) + { + transformComponent->SetParent(parentEntityId); + } } void AddRequiredComponentsToEntity(AZ::EntityId entityId) diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h index ffabeee69..553dffc90 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h @@ -32,30 +32,35 @@ namespace ROS2::PrefabMakerUtils //! Set the transform for an entity. //! @param origin pose for the entity to set. //! @param entityId entity which will be modified. - void SetEntityTransformLocal(const urdf::Pose& origin, AZ::EntityId entityId); + void SetEntityTransformLocal(const gz::math::Pose3d& origin, AZ::EntityId entityId); - //! Create a prefab entity in hierarchy. + //! Create a prefab entity in a hierarchy. The new entity will not yet be active. //! @param parentEntityId id of parent entity for this new entity. //! Passing an invalid id would get the entity in the current context (for example, an entity which is currently open in the Editor). //! @param name name for the new entity. //! @return a result which is either a created prefab entity or an error. AzToolsFramework::Prefab::PrefabEntityResult CreateEntity(AZ::EntityId parentEntityId, const AZStd::string& name); + //! Set the parent entity for an entity. The entity getting parent is expected to be inactive. + //! @param entityId the id for entity that needs a parent. + //! @param parentEntityId the id for the parent entity. + void SetEntityParent(AZ::EntityId entityId, AZ::EntityId parentEntityId); + //! Create an entity name from arguments. //! @param rootName root of entity's name. - //! @param type type of entity, depending on corresponding URDF tag. For example, "visual". + //! @param type type of entity, depending on corresponding SDF tag. For example, "visual". //! @param index index of entity, useful when multiple visuals or colliders are present for a single link. //! @return entity name, for example "robotBumper_visual_1". AZStd::string MakeEntityName(const AZStd::string& rootName, const AZStd::string& type, size_t index = 0); //! Get an Asset for a specified mesh given its path and mapping. - //! @param urdfAssetsMapping mapping of URDF assets. - //! @param urdfMeshPath a path to the mesh for which the Asset is requested. + //! @param sdfAssetsMapping mapping of SDF assets. + //! @param sdfMeshPath a path to the mesh for which the Asset is requested. //! @return Asset for the mesh, if found in the mapping. AZStd::optional GetAssetFromPath( - const Utils::UrdfAssetMap& urdfAssetsMapping, const AZStd::string& urdfMeshPath); + const Utils::UrdfAssetMap& sdfAssetsMapping, const AZStd::string& sdfMeshPath); //! Get Asset from path. Version for std::string. //! @see GetAssetFromPath. - AZStd::optional GetAssetFromPath(const Utils::UrdfAssetMap& urdfAssetsMapping, const std::string& urdfMeshPath); + AZStd::optional GetAssetFromPath(const Utils::UrdfAssetMap& sdfAssetsMapping, const std::string& sdfMeshPath); } // namespace ROS2::PrefabMakerUtils diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/SensorsMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/SensorsMaker.cpp new file mode 100644 index 000000000..e6dcb6bbc --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/SensorsMaker.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 "SensorsMaker.h" + +#include +#include +#include +#include + +#include +#include + +namespace ROS2 +{ + bool AddSensor(AZ::EntityId entityId, const sdf::Sensor* sensor) + { + SDFormat::SensorImporterHooksStorage sensorHooks; + ROS2::RobotImporterRequestBus::BroadcastResult(sensorHooks, &ROS2::RobotImporterRequest::GetSensorHooks); + for (const auto& hook : sensorHooks) + { + if (hook.m_sensorTypes.contains(sensor->Type())) + { + AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId); + hook.m_sdfSensorToComponentCallback(*entity, *sensor); + return true; + } + } + + return false; + } + + void SensorsMaker::AddSensors(const sdf::Model& model, const sdf::Link* link, AZ::EntityId entityId) const + { + for (size_t si = 0; si < link->SensorCount(); ++si) + { + const auto* sensor = link->SensorByIndex(si); + const bool success = AddSensor(entityId, sensor); + AZ_Warning("SensorMaker", success, "Cannot find a sensor hook for sensor %d", sensor->Type()); + } + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/SensorsMaker.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/SensorsMaker.h new file mode 100644 index 000000000..f89de2059 --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/SensorsMaker.h @@ -0,0 +1,32 @@ +/* + * 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 "UrdfParser.h" +#include +#include +#include + +#include + +namespace ROS2 +{ + //! Populates a given entity with all the contents of the tag. + //! Sensors are specified as children of link or joint in SDFormat. + class SensorsMaker + { + public: + //! Adds a sensor to an entity and sets it accordingly based on SDFormat description. + //! @param model A parsed SDF model which could hold information about sensor to be made. + //! @param link A parsed SDF tree link node used to identify link being currently processed. + //! @param entityId A non-active entity which will be affected. + //! @returns created components Id or string with fail + void AddSensors(const sdf::Model& model, const sdf::Link* link, AZ::EntityId entityId) const; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp index 36cad81f9..bdcebf5db 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp @@ -10,122 +10,225 @@ #include "CollidersMaker.h" #include "PrefabMakerUtils.h" #include +#include #include #include +#include +#include +#include #include #include +#include #include +#include +#include #include #include #include #include -#include #include +#include #include +#include namespace ROS2 { URDFPrefabMaker::URDFPrefabMaker( const AZStd::string& modelFilePath, - urdf::ModelInterfaceSharedPtr model, + const sdf::Root* root, AZStd::string prefabPath, const AZStd::shared_ptr urdfAssetsMapping, - bool useArticulations) - : m_model(model) - , m_visualsMaker(model->materials_, urdfAssetsMapping) + bool useArticulations, + const AZStd::optional spawnPosition) + : m_root(root) + , m_visualsMaker(urdfAssetsMapping) , m_collidersMaker(urdfAssetsMapping) , m_prefabPath(std::move(prefabPath)) , m_urdfAssetsMapping(urdfAssetsMapping) + , m_spawnPosition(spawnPosition) , m_useArticulations(useArticulations) { AZ_Assert(!m_prefabPath.empty(), "Prefab path is empty"); - AZ_Assert(m_model, "Model is nullptr"); + AZ_Assert(m_root, "SDF Root is nullptr"); } - void URDFPrefabMaker::BuildAssetsForLink(urdf::LinkSharedPtr link) - { - m_collidersMaker.BuildColliders(link); - for (auto childLink : link->child_links) - { - BuildAssetsForLink(childLink); - } - } - - AzToolsFramework::Prefab::CreatePrefabResult URDFPrefabMaker::CreatePrefabFromURDF() + URDFPrefabMaker::CreatePrefabTemplateResult URDFPrefabMaker::CreatePrefabTemplateFromUrdfOrSdf() { { AZStd::lock_guard lck(m_statusLock); m_status.clear(); } - // Begin an undo batch for prefab creation process - AzToolsFramework::UndoSystem::URSequencePoint* currentUndoBatch = nullptr; - AzToolsFramework::ToolsApplicationRequests::Bus::BroadcastResult( - currentUndoBatch, &AzToolsFramework::ToolsApplicationRequests::BeginUndoBatch, "Robot Importer prefab creation"); - if (currentUndoBatch == nullptr) + if (!ContainsModel()) { - AZ_Warning("URDF Prefab Maker", false, "Unable to start undobatch, EBus might not be listening"); + return AZ::Failure(AZStd::string("URDF/SDF doesn't contain any models.")); } - AZStd::unordered_map createdLinks; - AzToolsFramework::Prefab::PrefabEntityResult createEntityRoot = AddEntitiesForLink(m_model->root_link_, AZ::EntityId()); - AZStd::string rootName(m_model->root_link_->name.c_str(), m_model->root_link_->name.size()); - createdLinks[rootName] = createEntityRoot; - if (!createEntityRoot.IsSuccess()) + // Visit any nested models in the SDF as well + constexpr bool visitNestedModels = true; + + // Maintains references to all joints and a mapping from the joints to their parent and child links + struct JointsMapper { - // End undo batch labeled "Robot Importer prefab creation" preemptively if an error occurs - if (currentUndoBatch != nullptr) + struct JointToAttachedModel { - AzToolsFramework::ToolsApplicationRequests::Bus::Broadcast( - &AzToolsFramework::ToolsApplicationRequests::Bus::Events::EndUndoBatch); - } + AZStd::string m_fullyQualifiedName; + const sdf::Joint* m_joint; + const sdf::Model* m_attachedModel; + }; + // this is a unique ordered vector + AZStd::vector m_joints; + AZStd::unordered_map m_jointToParentLinks; + AZStd::unordered_map m_jointToChildLinks; + }; + JointsMapper jointsMapper; + auto GetAllJointsFromModel = [&jointsMapper](const sdf::Model& model, const Utils::ModelStack&) -> Utils::VisitModelResponse + { + // As the VisitModels function visits nested models by default, gatherNestedModelJoints is set to false + constexpr bool gatherNestedModelJoints = false; + auto jointsForModel = Utils::GetAllJoints(model, gatherNestedModelJoints); + for (const auto& [fullyQualifiedName, joint] : jointsForModel) + { + JointsMapper::JointToAttachedModel jointToAttachedModel{ + AZStd::string(fullyQualifiedName.c_str(), fullyQualifiedName.size()), joint, &model + }; + jointsMapper.m_joints.push_back(AZStd::move(jointToAttachedModel)); + + // add mapping from joint to child link + std::string childName = joint->ChildName(); + if (const sdf::Link* link = model.LinkByName(childName); link != nullptr) + { + // Add a mapping of joint to child link + jointsMapper.m_jointToChildLinks[joint] = link; + } - return AZ::Failure(AZStd::string(createEntityRoot.GetError())); - } + // add mapping from joint to parent link + std::string parentName = joint->ParentName(); + if (const sdf::Link* link = model.LinkByName(parentName); link != nullptr) + { + jointsMapper.m_jointToParentLinks[joint] = link; + } + } - auto links = Utils::GetAllLinks(m_model->root_link_->child_links); + return Utils::VisitModelResponse::VisitNestedAndSiblings; + }; + // Gather all Joints in SDF including in nested models + Utils::VisitModels(*m_root, GetAllJointsFromModel, visitNestedModels); - for (const auto& [name, linkPtr] : links) + // Maintains references to all Links in the SDF and a mapping of links to the model it is attached to + struct LinksMapper + { + struct LinkToAttachedModel + { + AZStd::string m_fullyQualifiedName; + const sdf::Link* m_link; + const sdf::Model* m_attachedModel; + }; + // this is a unique ordered vector + AZStd::vector m_links; + }; + LinksMapper linksMapper; + + auto GetAllLinksFromModel = [&linksMapper](const sdf::Model& model, const Utils::ModelStack&) -> Utils::VisitModelResponse + { + // As the VisitModels function visits nested models by default, gatherNestedModelLinks is set to false + constexpr bool gatherNestedModelLinks = false; + auto linksForModel = Utils::GetAllLinks(model, gatherNestedModelLinks); + for (const auto& [fullyQualifiedName, link] : linksForModel) + { + // Push back the mapping of link to attached model into the ordered vector + LinksMapper::LinkToAttachedModel linkToAttachedModel{ AZStd::string(fullyQualifiedName.c_str(), fullyQualifiedName.size()), + link, + &model }; + linksMapper.m_links.push_back(AZStd::move(linkToAttachedModel)); + } + return Utils::VisitModelResponse::VisitNestedAndSiblings; + }; + + // Gather all links from all the models in the SDF + Utils::VisitModels(*m_root, GetAllLinksFromModel, visitNestedModels); + + // Build up a list of all entities created as a part of processing the file. + AZStd::vector createdEntities; + AZStd::unordered_map createdLinks; + AZStd::unordered_map createdModels; + AZStd::unordered_map links; + for ([[maybe_unused]] const auto& [fullLinkName, linkPtr, attachedModel] : linksMapper.m_links) { - createdLinks[name] = AddEntitiesForLink(linkPtr, createEntityRoot.GetValue()); + // Create entities for the model containing the link + AZ::EntityId modelEntityId; + if (attachedModel != nullptr) + { + if (auto modelIt = createdModels.find(attachedModel); modelIt != createdModels.end()) + { + if (modelIt->second.IsSuccess()) + { + modelEntityId = modelIt->second.GetValue(); + } + } + else + { + if (AzToolsFramework::Prefab::PrefabEntityResult createModelEntityResult = CreateEntityForModel(*attachedModel); + createModelEntityResult) + { + modelEntityId = createModelEntityResult.GetValue(); + // Add the model entity to the created entity list + // so that it gets added to the prefab + createdEntities.emplace_back(modelEntityId); + + std::string modelName = attachedModel->Name(); + AZStd::string azModelName(modelName.c_str(), modelName.size()); + AZStd::lock_guard lck(m_statusLock); + m_status.emplace(azModelName, AZStd::string::format("created as: %s", modelEntityId.ToString().c_str())); + createdModels.emplace(attachedModel, createModelEntityResult); + } + } + + } + + // Add all link as children of their attached model entity by default + createdLinks[linkPtr] = AddEntitiesForLink(*linkPtr, attachedModel, modelEntityId, createdEntities); } - for (const auto& [name, result] : createdLinks) + for (const auto& [linkPtr, result] : createdLinks) { - AZ_TracePrintf( - "CreatePrefabFromURDF", + std::string linkName = linkPtr->Name(); + AZStd::string azLinkName(linkName.c_str(), linkName.size()); + AZ_Trace( + "CreatePrefabFromUrdfOrSdf", "Link with name %s was created as: %s\n", - name.c_str(), + linkName.c_str(), result.IsSuccess() ? (result.GetValue().ToString().c_str()) : ("[Failed]")); AZStd::lock_guard lck(m_statusLock); if (result.IsSuccess()) { - m_status.emplace(name, AZStd::string::format("created as: %s", result.GetValue().ToString().c_str())); + m_status.emplace(azLinkName, AZStd::string::format("created as: %s", result.GetValue().ToString().c_str())); } else { - m_status.emplace(name, AZStd::string::format("failed : %s", result.GetError().c_str())); + m_status.emplace(azLinkName, AZStd::string::format("failed : %s", result.GetError().c_str())); } } // Set the transforms of links - for (const auto& [name, linkPtr] : links) + for ([[maybe_unused]] const auto& [fullLinkName, linkPtr, _] : linksMapper.m_links) { - const auto this_entry = createdLinks.at(name); - if (this_entry.IsSuccess()) + if (const auto createLinkEntityResult = createdLinks.at(linkPtr); createLinkEntityResult.IsSuccess()) { + AZ::EntityId createdEntityId = createLinkEntityResult.GetValue(); + std::string linkName = linkPtr->Name(); AZ::Transform tf = Utils::GetWorldTransformURDF(linkPtr); - auto* entity = AzToolsFramework::GetEntityById(this_entry.GetValue()); + auto* entity = AzToolsFramework::GetEntityById(createdEntityId); if (entity) { auto* transformInterface = entity->FindComponent(); if (transformInterface) { - AZ_TracePrintf( - "CreatePrefabFromURDF", + AZ_Trace( + "CreatePrefabFromUrdfOrSdf", "Setting transform %s %s to [%f %f %f] [%f %f %f %f]\n", - name.c_str(), - this_entry.GetValue().ToString().c_str(), + linkName.c_str(), + createdEntityId.ToString().c_str(), tf.GetTranslation().GetX(), tf.GetTranslation().GetY(), tf.GetTranslation().GetZ(), @@ -137,115 +240,248 @@ namespace ROS2 } else { - AZ_TracePrintf( - "CreatePrefabFromURDF", "Setting transform failed: %s does not have transform interface\n", name.c_str()); + AZ_Trace( + "CreatePrefabFromUrdfOrSdf", + "Setting transform failed: %s does not have transform interface\n", + linkName.c_str()); } } } } // Set the hierarchy - for (const auto& [name, linkPtr] : links) + AZStd::vector linkEntityIdsWithoutParent; + for (const auto& [fullLinkName, linkPtr, attachedModel] : linksMapper.m_links) { - const auto thisEntry = createdLinks.at(name); - if (!thisEntry.IsSuccess()) + std::string linkName = linkPtr->Name(); + AZStd::string azLinkName(linkName.c_str(), linkName.size()); + const auto linkPrefabResult = createdLinks.at(linkPtr); + if (!linkPrefabResult.IsSuccess()) { - AZ_TracePrintf("CreatePrefabFromURDF", "Link %s creation failed\n", name.c_str()); + AZ_Trace("CreatePrefabFromUrdfOrSdf", "Link %s creation failed\n", fullLinkName.c_str()); continue; } - auto parentPtr = linkPtr->getParent(); - if (!parentPtr) + + AZStd::vector jointsWhereLinkIsChild; + bool gatherNestedModelJoints = true; + jointsWhereLinkIsChild = Utils::GetJointsForChildLink(*attachedModel, azLinkName, gatherNestedModelJoints); + + if (jointsWhereLinkIsChild.empty()) { - AZ_TracePrintf("CreatePrefabFromURDF", "Link %s has no parents\n", name.c_str()); + // emplace unique entry to the container of links that don't have a parent link associated with it + if (auto existingLinkIt = + AZStd::find(linkEntityIdsWithoutParent.begin(), linkEntityIdsWithoutParent.end(), linkPrefabResult.GetValue()); + existingLinkIt == linkEntityIdsWithoutParent.end()) + { + linkEntityIdsWithoutParent.emplace_back(linkPrefabResult.GetValue()); + } + AZ_Trace("CreatePrefabFromUrdfOrSdf", "Link %s has no parents\n", linkName.c_str()); continue; } - AZStd::string parentName(parentPtr->name.c_str(), parentPtr->name.size()); - const auto parentEntry = createdLinks.find(parentName); - if (parentEntry == createdLinks.end()) + + // For URDF, a link can only be child in a single joint + // a link can't be a child of two other links as URDF models a tree structure and not a graph + /* + Here is a snippet from the Pose Frame Semantics documentation for SDFormat that explains the differences + between URDF and SDF coordinate frame + http://sdformat.org/tutorials?tut=pose_frame_semantics&ver=1.5#parent-frames-in-urdf + > The most significant difference between URDF and SDFormat coordinate frames is related to links and joints. + > While SDFormat allows kinematic loops with the topology of a directed graph, + > URDF kinematics must have the topology of a directed tree, with each link being the child of at most one joint. + > URDF coordinate frames are defined recursively based on this tree structure, with each joint's tag + > defining the coordinate transformation from the parent link frame to the child link frame. + */ + + // Use the first joint where this link is a child to locate the parent link pointer. + const sdf::Joint* joint = jointsWhereLinkIsChild.front(); + std::string parentLinkName = joint->ParentName(); + AZStd::string parentName(parentLinkName.c_str(), parentLinkName.size()); + + // Lookup the entity created from the parent link using the JointMapper to locate the parent SDF link. + // followed by using SDF link address to lookup the O3DE created entity ID + auto parentLinkIter = jointsMapper.m_jointToParentLinks.find(joint); + auto parentEntityIter = + parentLinkIter != jointsMapper.m_jointToParentLinks.end() ? createdLinks.find(parentLinkIter->second) : createdLinks.end(); + if (parentEntityIter == createdLinks.end()) { - AZ_TracePrintf("CreatePrefabFromURDF", "Link %s has invalid parent name %s\n", name.c_str(), parentName.c_str()); + AZ_Trace("CreatePrefabFromUrdfOrSdf", "Link %s has invalid parent name %s\n", linkName.c_str(), parentName.c_str()); continue; } - if (!parentEntry->second.IsSuccess()) + if (!parentEntityIter->second.IsSuccess()) { - AZ_TracePrintf( - "CreatePrefabFromURDF", "Link %s has parent %s which has failed to create\n", name.c_str(), parentName.c_str()); + AZ_Trace( + "CreatePrefabFromUrdfOrSdf", + "Link %s has parent %s which has failed to create\n", + linkName.c_str(), + parentName.c_str()); continue; } - AZ_TracePrintf( - "CreatePrefabFromURDF", + AZ_Trace( + "CreatePrefabFromUrdfOrSdf", "Link %s setting parent to %s\n", - thisEntry.GetValue().ToString().c_str(), - parentEntry->second.GetValue().ToString().c_str()); - AZ_TracePrintf("CreatePrefabFromURDF", "Link %s setting parent to %s\n", name.c_str(), parentName.c_str()); - auto* entity = AzToolsFramework::GetEntityById(thisEntry.GetValue()); - entity->Activate(); - AZ::TransformBus::Event(thisEntry.GetValue(), &AZ::TransformBus::Events::SetParent, parentEntry->second.GetValue()); - entity->Deactivate(); + linkPrefabResult.GetValue().ToString().c_str(), + parentEntityIter->second.GetValue().ToString().c_str()); + AZ_Trace("CreatePrefabFromUrdfOrSdf", "Link %s setting parent to %s\n", linkName.c_str(), parentName.c_str()); + PrefabMakerUtils::SetEntityParent(linkPrefabResult.GetValue(), parentEntityIter->second.GetValue()); } - if (!m_useArticulations) + // Iterate over all the joints and locate the entity associated with the link + for ([[maybe_unused]] const auto& [fullJointName, jointPtr, _] : jointsMapper.m_joints) { - auto joints = Utils::GetAllJoints(m_model->root_link_->child_links); - for (const auto& [name, jointPtr] : joints) + std::string jointName = jointPtr->Name(); + AZStd::string azJointName(jointName.c_str(), jointName.size()); + std::string childLinkName = jointPtr->ChildName(); + std::string parentLinkName = jointPtr->ParentName(); + + // Look up the O3DE created entity by first locating the parent SDF link associated with the current joint + // and then using that SDF link to lookup the created entity + auto parentLinkIter = jointsMapper.m_jointToParentLinks.find(jointPtr); + auto parentEntityIter = + parentLinkIter != jointsMapper.m_jointToParentLinks.end() ? createdLinks.find(parentLinkIter->second) : createdLinks.end(); + if (parentEntityIter == createdLinks.end()) + { + AZ_Warning( + "CreatePrefabFromUrdfOrSdf", + false, + "Joint %s has no parent link %s. Cannot create", + azJointName.c_str(), + parentLinkName.c_str()); + continue; + } + auto leadEntity = parentEntityIter->second; + + // Use the joint to lookup the child SDF link which is used to look up the O3DE entity + auto childLinkIter = jointsMapper.m_jointToChildLinks.find(jointPtr); + auto childEntityIter = + childLinkIter != jointsMapper.m_jointToChildLinks.end() ? createdLinks.find(childLinkIter->second) : createdLinks.end(); + if (childEntityIter == createdLinks.end()) + { + AZ_Warning( + "CreatePrefabFromUrdfOrSdf", + false, + "Joint %s has no child link %s. Cannot create", + azJointName.c_str(), + childLinkName.c_str()); + continue; + } + auto childEntity = childEntityIter->second; + + AZ_Trace( + "CreatePrefabFromUrdfOrSdf", + "Creating joint %s : %s -> %s\n", + azJointName.c_str(), + parentLinkName.c_str(), + childLinkName.c_str()); + + AZ::Entity* childEntityPtr = AzToolsFramework::GetEntityById(childEntity.GetValue()); + if (childEntityPtr) + { + auto* component = Utils::GetGameOrEditorComponent(childEntityPtr); + if (component) + { + component->SetJointName(azJointName); + } + } + // check if both has RigidBody and we are not creating articulation + if (!m_useArticulations) { - AZ_Assert(jointPtr, "joint %s is null", name.c_str()); - AZ_TracePrintf( - "CreatePrefabFromURDF", - "Creating joint %s : %s -> %s\n", - name.c_str(), - jointPtr->parent_link_name.c_str(), - jointPtr->child_link_name.c_str()); - - auto leadEntity = createdLinks.at(jointPtr->parent_link_name.c_str()); - auto childEntity = createdLinks.at(jointPtr->child_link_name.c_str()); - // check if both has RigidBody if (leadEntity.IsSuccess() && childEntity.IsSuccess()) { AZStd::lock_guard lck(m_statusLock); auto result = m_jointsMaker.AddJointComponent(jointPtr, childEntity.GetValue(), leadEntity.GetValue()); m_status.emplace( - name, AZStd::string::format(" %s %llu", result.IsSuccess() ? "created as" : "Failed", result.GetValue())); + azJointName, AZStd::string::format(" %s: %llu", result.IsSuccess() ? "created as" : "failed", result.GetValue())); } else { - AZ_Warning("CreatePrefabFromURDF", false, "cannot create joint %s", name.c_str()); + AZ_Warning("CreatePrefabFromUrdfOrSdf", false, "cannot create joint %s", azJointName.c_str()); } } } - MoveEntityToDefaultSpawnPoint(createEntityRoot.GetValue()); - - auto contentEntityId = createEntityRoot.GetValue(); - AddRobotControl(contentEntityId); + // Use the first entity based on a link that is not parented to any other link + if (!linkEntityIdsWithoutParent.empty() && linkEntityIdsWithoutParent.front().IsValid()) + { + AZ::EntityId contentEntityId = linkEntityIdsWithoutParent.front(); + MoveEntityToDefaultSpawnPoint(contentEntityId, m_spawnPosition); + AddRobotControl(contentEntityId); + } // Create prefab, save it to disk immediately // Remove prefab, if it was already created. - AZ::EntityId entityId = createEntityRoot.GetValue(); - - auto prefabSystemComponent = AZ::Interface::Get(); + // clear out any previously created prefab template for this path + auto* prefabSystemComponentInterface = AZ::Interface::Get(); auto prefabLoaderInterface = AZ::Interface::Get(); - auto prefabInterface = AZ::Interface::Get(); + auto relativePath = prefabLoaderInterface->GenerateRelativePath(m_prefabPath.c_str()); + AzToolsFramework::Prefab::TemplateId prefabTemplateId = + prefabSystemComponentInterface->GetTemplateIdFromFilePath({ relativePath.c_str() }); + if (prefabTemplateId != AzToolsFramework::Prefab::InvalidTemplateId) + { + prefabSystemComponentInterface->RemoveTemplate(prefabTemplateId); + prefabTemplateId = AzToolsFramework::Prefab::InvalidTemplateId; + } - AZ::IO::Path relativeFilePath = prefabLoaderInterface->GenerateRelativePath(m_prefabPath.c_str()); + prefabTemplateId = AzToolsFramework::Prefab::InvalidTemplateId; - const auto templateId = prefabSystemComponent->GetTemplateIdFromFilePath(relativeFilePath); - AZ_TracePrintf("CreatePrefabFromURDF", "GetTemplateIdFromFilePath %s -> %d \n", m_prefabPath.c_str(), templateId); + // create prefab from the "set" of entities (currently just the single default entity) + AzToolsFramework::Prefab::PrefabSystemScriptingBus::BroadcastResult( + prefabTemplateId, + &AzToolsFramework::Prefab::PrefabSystemScriptingBus::Events::CreatePrefabTemplate, + createdEntities, + relativePath.String()); - if (templateId != AzToolsFramework::Prefab::InvalidTemplateId) + if (prefabTemplateId == AzToolsFramework::Prefab::InvalidTemplateId) { - AZ_TracePrintf("CreatePrefabFromURDF", "Prefab was already loaded\n"); - prefabSystemComponent->RemoveTemplate(templateId); + AZ_Error("CreatePrefabFromUrdfOrSdf", false, "Could not create a prefab template for entities."); + return AZ::Failure("Could not create a prefab template for entities."); } - auto outcome = prefabInterface->CreatePrefabInDisk(AzToolsFramework::EntityIdList{ entityId }, m_prefabPath.c_str()); - if (outcome.IsSuccess()) + AZ_Info("CreatePrefabFromUrdfOrSdf", "Successfully created prefab %s\n", m_prefabPath.c_str()); + + return AZ::Success(prefabTemplateId); + } + + URDFPrefabMaker::CreatePrefabTemplateResult URDFPrefabMaker::CreatePrefabFromUrdfOrSdf() + { + // Begin an undo batch for prefab creation process + AzToolsFramework::UndoSystem::URSequencePoint* currentUndoBatch = nullptr; + AzToolsFramework::ToolsApplicationRequests::Bus::BroadcastResult( + currentUndoBatch, &AzToolsFramework::ToolsApplicationRequests::BeginUndoBatch, "Robot Importer prefab creation"); + if (currentUndoBatch == nullptr) { - AZ::EntityId prefabContainerEntityId = outcome.GetValue(); - PrefabMakerUtils::AddRequiredComponentsToEntity(prefabContainerEntityId); + AZ_Warning("URDF Prefab Maker", false, "Unable to start undobatch, EBus might not be listening"); + } + + auto result = CreatePrefabTemplateFromUrdfOrSdf(); + if (!result.IsSuccess()) + { + // End undo batch labeled "Robot Importer prefab creation" preemptively if an error occurs + if (currentUndoBatch != nullptr) + { + AzToolsFramework::ToolsApplicationRequests::Bus::Broadcast( + &AzToolsFramework::ToolsApplicationRequests::Bus::Events::EndUndoBatch); + } + + return result; + } + + auto prefabLoaderInterface = AZ::Interface::Get(); + + // Save Template to file + auto relativePath = prefabLoaderInterface->GenerateRelativePath(m_prefabPath.c_str()); + bool saveResult = prefabLoaderInterface->SaveTemplateToFile(result.GetValue(), m_prefabPath.c_str()); + if (saveResult) + { + // If the template saved successfully, also instantiate it into the level. + auto prefabInterface = AZ::Interface::Get(); + [[maybe_unused]] auto createPrefabOutcome = + prefabInterface->InstantiatePrefab(relativePath.String(), AZ::EntityId(), AZ::Vector3::CreateZero()); + } + else + { + result = AZ::Failure(AZStd::string::format("Could not save the newly created prefab to '%s'", m_prefabPath.c_str())); } - AZ_TracePrintf("CreatePrefabFromURDF", "Successfully created prefab %s\n", m_prefabPath.c_str()); // End undo batch labeled "Robot Importer prefab creation" if (currentUndoBatch != nullptr) @@ -254,17 +490,94 @@ namespace ROS2 &AzToolsFramework::ToolsApplicationRequests::Bus::Events::EndUndoBatch); } - return outcome; + return result; } - AzToolsFramework::Prefab::PrefabEntityResult URDFPrefabMaker::AddEntitiesForLink(urdf::LinkSharedPtr link, AZ::EntityId parentEntityId) + AzToolsFramework::Prefab::PrefabEntityResult URDFPrefabMaker::CreateEntityForModel(const sdf::Model& model) { - if (!link) + auto createEntityResult = PrefabMakerUtils::CreateEntity(AZ::EntityId{}, model.Name().c_str()); + if (!createEntityResult.IsSuccess()) { - AZ::Failure(AZStd::string("Failed to create prefab entity - link is null")); + return createEntityResult; } - auto createEntityResult = PrefabMakerUtils::CreateEntity(parentEntityId, link->name.c_str()); + // Get the model entity and update its transform component with the pose information + AZ::EntityId entityId = createEntityResult.GetValue(); + AZStd::unique_ptr entity(AzToolsFramework::GetEntityById(entityId)); + + if (auto* transformComponent = entity->FindComponent(); + transformComponent != nullptr) + { + gz::math::Pose3d modelPose; + if (sdf::Errors poseResolveErrors = model.SemanticPose().Resolve(modelPose); !poseResolveErrors.empty()) + { + AZStd::string poseErrorMessages = Utils::JoinSdfErrorsToString(poseResolveErrors); + + auto poseErrorsForModel = AZStd::string::format( + R"(Unable to resolve semantic pose for model %s. Creation of Model entity has failed. Errors: "%s")", + model.Name().c_str(), + poseErrorMessages.c_str()); + + return AZ::Failure(poseErrorsForModel); + } + + AZ::Transform modelTransform = URDF::TypeConversions::ConvertPose(modelPose); + + // If the model is nested below another model, check if it has a placement frame + if (const sdf::Model* parentModel = Utils::GetModelContainingModel(*m_root, model); parentModel != nullptr) + { + if (const sdf::Frame* modelPlacementFrame = parentModel->FrameByName(model.PlacementFrameName()); modelPlacementFrame != nullptr) + { + gz::math::Pose3d placementFramePose; + if (sdf::Errors poseResolveErrors = modelPlacementFrame->SemanticPose().Resolve(placementFramePose); + !poseResolveErrors.empty()) + { + AZStd::string poseErrorMessages = Utils::JoinSdfErrorsToString(poseResolveErrors); + + auto poseErrorsForModel = AZStd::string::format( + R"(Unable to resolve semantic pose for model %s parent model %s. Creation of Model entity has failed. Errors: "%s")", + model.Name().c_str(), + parentModel->Name().c_str(), + poseErrorMessages.c_str()); + + return AZ::Failure(poseErrorsForModel); + } + + modelTransform = modelTransform * URDF::TypeConversions::ConvertPose(placementFramePose); + } + } + + if (const sdf::Frame* implicitFrame = model.FrameByName("__model__"); implicitFrame != nullptr) + { + gz::math::Pose3d implicitFramePose; + if (sdf::Errors poseResolveErrors = implicitFrame->SemanticPose().Resolve(implicitFramePose); !poseResolveErrors.empty()) + { + AZStd::string poseErrorMessages = Utils::JoinSdfErrorsToString(poseResolveErrors); + + auto poseErrorsForModel = AZStd::string::format( + R"(Unable to resolve semantic pose for model's implicit frame %s. Creation of Model entity has failed. Errors: "%s")", + implicitFrame->Name().c_str(), + poseErrorMessages.c_str()); + + return AZ::Failure(poseErrorsForModel); + } + + modelTransform = modelTransform * URDF::TypeConversions::ConvertPose(implicitFramePose); + } + + transformComponent->SetWorldTM(AZStd::move(modelTransform)); + } + + // Allow the created model entity to persist if there are no errors at ths point + entity.release(); + + return entityId; + } + + AzToolsFramework::Prefab::PrefabEntityResult URDFPrefabMaker::AddEntitiesForLink( + const sdf::Link& link, const sdf::Model* attachedModel, AZ::EntityId parentEntityId, AZStd::vector& createdEntities) + { + auto createEntityResult = PrefabMakerUtils::CreateEntity(parentEntityId, link.Name().c_str()); if (!createEntityResult.IsSuccess()) { return createEntityResult; @@ -272,24 +585,35 @@ namespace ROS2 AZ::EntityId entityId = createEntityResult.GetValue(); AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId); - const auto frameCompontentId = Utils::CreateComponent(entityId, ROS2FrameComponent::TYPEINFO_Uuid()); - if (frameCompontentId) + createdEntities.emplace_back(entityId); + + const auto frameComponentId = Utils::CreateComponent(entityId, ROS2FrameComponent::TYPEINFO_Uuid()); + if (frameComponentId) { auto* component = Utils::GetGameOrEditorComponent(entity); AZ_Assert(component, "ROS2 Frame Component does not exist for %s", entityId.ToString().c_str()); - component->SetFrameID(AZStd::string(link->name.c_str(), link->name.size())); + component->SetFrameID(AZStd::string(link.Name().c_str(), link.Name().size())); } - m_visualsMaker.AddVisuals(link, entityId); + auto createdVisualEntities = m_visualsMaker.AddVisuals(&link, entityId); + createdEntities.insert(createdEntities.end(), createdVisualEntities.begin(), createdVisualEntities.end()); + if (!m_useArticulations) { - m_inertialsMaker.AddInertial(link->inertial, entityId); + m_inertialsMaker.AddInertial(link.Inertial(), entityId); } else { - m_articulationsMaker.AddArticulationLink(link, entityId); + if (attachedModel != nullptr) + { + m_articulationsMaker.AddArticulationLink(*attachedModel, &link, entityId); + } } - m_collidersMaker.AddColliders(link, entityId); + if (attachedModel != nullptr) + { + m_collidersMaker.AddColliders(*attachedModel, &link, entityId); + m_sensorsMaker.AddSensors(*attachedModel, &link, entityId); + } return AZ::Success(entityId); } @@ -313,26 +637,27 @@ namespace ROS2 return m_prefabPath; } - void URDFPrefabMaker::MoveEntityToDefaultSpawnPoint(const AZ::EntityId& rootEntityId) + void URDFPrefabMaker::MoveEntityToDefaultSpawnPoint( + const AZ::EntityId& rootEntityId, AZStd::optional spawnPosition = AZStd::nullopt) { - auto spawner = ROS2::SpawnerInterface::Get(); - - if (spawner == nullptr) + if (!spawnPosition.has_value()) { - AZ_TracePrintf("URDF Importer", "Spawner not found - creating entity in (0,0,0)\n") return; + AZ_Trace("URDF Importer", "SpawnPosition is null - spawning in Editors default position\n"); + return; } - auto entity_ = AzToolsFramework::GetEntityById(rootEntityId); - auto* transformInterface_ = entity_->FindComponent(); - - if (transformInterface_ == nullptr) + if (auto entity_ = AzToolsFramework::GetEntityById(rootEntityId); entity_ != nullptr) { - AZ_TracePrintf("URDF Importer", "TransformComponent not found in created entity\n") return; - } + auto* transformInterface_ = entity_->FindComponent(); - auto pose = spawner->GetDefaultSpawnPose(); + if (transformInterface_ == nullptr) + { + AZ_Trace("URDF Importer", "TransformComponent not found in created entity\n") return; + } - transformInterface_->SetWorldTM(pose); + transformInterface_->SetWorldTM(*spawnPosition); + AZ_Trace("URDF Importer", "Successfully set spawn position\n") + } } AZStd::string URDFPrefabMaker::GetStatus() @@ -345,4 +670,17 @@ namespace ROS2 } return str; } + + bool URDFPrefabMaker::ContainsModel() const + { + const sdf::Model* sdfModel{}; + auto GetModelAndStopIteration = [&sdfModel](const sdf::Model& model, const Utils::ModelStack&) -> Utils::VisitModelResponse + { + sdfModel = &model; + // Return stop to prevent further visitation of additional models + return Utils::VisitModelResponse::Stop; + }; + Utils::VisitModels(*m_root, GetModelAndStopIteration); + return sdfModel != nullptr; + } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h index f6a68d610..78bb7f196 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h @@ -12,40 +12,54 @@ #include "CollidersMaker.h" #include "InertialsMaker.h" #include "JointsMaker.h" +#include "SensorsMaker.h" #include "UrdfParser.h" #include "VisualsMaker.h" #include +#include #include #include #include #include +#include #include #include +#include namespace ROS2 { - //! Encapsulates constructive mapping of URDF elements to a complete prefab with entities and components + //! Encapsulates constructive mapping of SDF elements to a complete prefab with entities and components class URDFPrefabMaker { public: - //! Construct URDFPrefabMaker from arguments. - //! @param modelFilePath path to the source URDF model. - //! @param model parsed model. + //! Construct PrefabMaker from arguments. + //! @param modelFilePath path to the source URDF/SDF model or world. + //! @param root parsed SDF root object. //! @param prefabPath path to the prefab which will be created as a result of import. - //! @param urdfAssetsMapping prepared mapping of URDF meshes to Assets. - //! @param useArticulations allows urdfImporter to create PhysXArticulations instead of multiple rigid bodies and joints. + //! @param urdfAssetsMapping prepared mapping of SDF meshes to Assets. + //! @param useArticulations allows sdfImporter to create PhysXArticulations instead of multiple rigid bodies and joints. URDFPrefabMaker( const AZStd::string& modelFilePath, - urdf::ModelInterfaceSharedPtr model, + const sdf::Root* root, AZStd::string prefabPath, - const AZStd::shared_ptr urdfAssetsMapping, - bool useArticulations = false); + const AZStd::shared_ptr sdfAssetsMapping, + bool useArticulations = false, + AZStd::optional spawnPosition = AZStd::nullopt); ~URDFPrefabMaker() = default; - //! Create and return a prefab corresponding to the URDF model as set through the constructor. - //! @return result which is either a prefab containing the imported model based on URDF or an error. - AzToolsFramework::Prefab::CreatePrefabResult CreatePrefabFromURDF(); + //! On prefab creation this will contain a prefab template id when successful, + //! and an error string on failure. + using CreatePrefabTemplateResult = AZ::Outcome; + + //! Create and return a prefab template corresponding to the SDF model as set through the constructor. + //! This will also instantiate the prefab template into the level. + //! @return result which is either the prefab template id containing the imported model or an error message. + CreatePrefabTemplateResult CreatePrefabFromUrdfOrSdf(); + + //! Create and return a prefab template id corresponding to the URDF/SDF model/world set in the constructor. + //! @return result which is either the prefab template id or an error message. + CreatePrefabTemplateResult CreatePrefabTemplateFromUrdfOrSdf(); //! Get path to the prefab resulting from the import. //! @return path to the prefab. @@ -56,15 +70,19 @@ namespace ROS2 AZStd::string GetStatus(); private: - AzToolsFramework::Prefab::PrefabEntityResult AddEntitiesForLink(urdf::LinkSharedPtr link, AZ::EntityId parentEntityId); - void BuildAssetsForLink(urdf::LinkSharedPtr link); + AzToolsFramework::Prefab::PrefabEntityResult CreateEntityForModel(const sdf::Model& model); + AzToolsFramework::Prefab::PrefabEntityResult AddEntitiesForLink(const sdf::Link& link, const sdf::Model* attachedModel, AZ::EntityId parentEntityId, AZStd::vector& createdEntities); void AddRobotControl(AZ::EntityId rootEntityId); - static void MoveEntityToDefaultSpawnPoint(const AZ::EntityId& rootEntityId); + static void MoveEntityToDefaultSpawnPoint(const AZ::EntityId& rootEntityId, AZStd::optional spawnPosition); - urdf::ModelInterfaceSharedPtr m_model; + // Returns if SDF document contains any model objects, be that at the root or within an tag + bool ContainsModel() const; + + const sdf::Root* m_root; AZStd::string m_prefabPath; VisualsMaker m_visualsMaker; CollidersMaker m_collidersMaker; + SensorsMaker m_sensorsMaker; InertialsMaker m_inertialsMaker; JointsMaker m_jointsMaker; ArticulationsMaker m_articulationsMaker; @@ -74,5 +92,7 @@ namespace ROS2 AZStd::shared_ptr m_urdfAssetsMapping; bool m_useArticulations{ false }; + + const AZStd::optional m_spawnPosition; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp index a5d81fe10..00f399232 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp @@ -8,91 +8,145 @@ #include "UrdfParser.h" -#include +#include #include +#include #include -#include -#include +#include +#include +#include -namespace ROS2 +namespace ROS2::UrdfParser { - namespace UrdfParser::Internal + class RedirectSDFOutputStream { - void CheckIfCurrentLocaleHasDotAsADecimalSeparator() + public: + // Copy the original console stream to restore in the destructor via RAII + // the console stream itself is referenced here and redirected to a local + // os stream by default + // @param consoleStream Reference to sdf::Console::ConsoleStream whose + // output will be redirected + // @param redirectStream reference to stream where console stream output is redirected to + RedirectSDFOutputStream(sdf::Console::ConsoleStream& consoleStream, std::ostream& redirectStream) + : m_consoleStreamRef(consoleStream) + , m_origConsoleStream(consoleStream) { - // Due to the fact that URDF parser takes into account the locale information, incompatibility between URDF file locale and - // system locale might lead to incorrect URDF parsing. Mainly it affects floating point numbers, and the decimal separator. When - // locales are set to system with comma as decimal separator and URDF file is created with dot as decimal separator, URDF parser - // will trim the floating point number after comma. For example, if parsing 0.1, URDF parser will parse it as 0. - // This might lead to incorrect URDF loading. If the current locale is not a dot (as per standard ROS locale), we warn the user. - std::locale currentLocale(""); - if (std::use_facet>(currentLocale).decimal_point() != '.') - { - AZ_Warning( - "UrdfParser", false, "Locale %s might be incompatible with the URDF file content.\n", currentLocale.name().c_str()); - } + // Redirect the Output stream to the supplied + m_consoleStreamRef = sdf::Console::ConsoleStream(&redirectStream); } - class CustomConsoleHandler : public console_bridge::OutputHandler + ~RedirectSDFOutputStream() { - private: - std::stringstream console_ss; + // Restore the original console stream + m_consoleStreamRef = AZStd::move(m_origConsoleStream); + } - public: - void log(const std::string& text, console_bridge::LogLevel level, const char* filename, int line) override final; + private: + sdf::Console::ConsoleStream& m_consoleStreamRef; + sdf::Console::ConsoleStream m_origConsoleStream; + }; - //! Clears accumulated log - void Clear(); + // Parser result member functions + sdf::Root& ParseResult::GetRoot() & + { + return m_root; + } + const sdf::Root& ParseResult::GetRoot() const& + { + return m_root; + } + sdf::Root&& ParseResult::GetRoot() && + { + return AZStd::move(m_root); + } - //! Read accumulated log to a string - AZStd::string GetLog(); - }; + const AZStd::string& ParseResult::GetParseMessages() const& + { + return m_parseMessages; + } - void CustomConsoleHandler::log(const std::string& text, console_bridge::LogLevel level, const char* filename, int line) - { - AZ_Printf("UrdfParser", "%s\n", text.c_str()); - console_ss << text << "\n"; - } + AZStd::string&& ParseResult::GetParseMessages() && + { + return AZStd::move(m_parseMessages); + } - void CustomConsoleHandler::Clear() - { - console_ss = std::stringstream(); - } + const sdf::Errors& ParseResult::GetSdfErrors() const& + { + return m_sdfErrors; + } - AZStd::string CustomConsoleHandler::GetLog() + sdf::Errors&& ParseResult::GetSdfErrors() && + { + return AZStd::move(m_sdfErrors); + } + + ParseResult::operator bool() const + { + return m_sdfErrors.empty(); + } + + bool ParseResult::UrdfParsedWithModifiedContent() const + { + return m_urdfModifications.duplicatedJoints.size() > 0 || m_urdfModifications.missingInertias.size() > 0 || + m_urdfModifications.incompleteInertias.size() > 0; + } + + RootObjectOutcome Parse(AZStd::string_view xmlString, const sdf::ParserConfig& parserConfig) + { + return Parse(std::string(xmlString.data(), xmlString.size()), parserConfig); + } + RootObjectOutcome Parse(const std::string& xmlString, const sdf::ParserConfig& parserConfig) + { + ParseResult parseResult; + std::ostringstream parseStringStream; { - return AZStd::string(console_ss.str().c_str(), console_ss.str().size()); + RedirectSDFOutputStream redirectConsoleStreamMsg(sdf::Console::Instance()->GetMsgStream(), parseStringStream); + parseResult.m_sdfErrors = parseResult.m_root.LoadSdfString(xmlString, parserConfig); } + // Get any captured sdf::Console messages + const auto& parseMessages = parseStringStream.str(); - CustomConsoleHandler customConsoleHandler; - } // namespace UrdfParser::Internal + // regular expression to escape console's color codes + const AZStd::regex escapeColor("\x1B\\[[0-9;]*[A-Za-z]"); - urdf::ModelInterfaceSharedPtr UrdfParser::Parse(const AZStd::string& xmlString) - { - console_bridge::useOutputHandler(&Internal::customConsoleHandler); - Internal::CheckIfCurrentLocaleHasDotAsADecimalSeparator(); - const auto ret = urdf::parseURDF(xmlString.c_str()); - console_bridge::restorePreviousOutputHandler(); - return ret; + parseResult.m_parseMessages = AZStd::regex_replace(AZStd::string(parseMessages.c_str(), parseMessages.size()),escapeColor, ""); + + // if there are no parse errors return the sdf Root object otherwise return the errors + return parseResult; } - urdf::ModelInterfaceSharedPtr UrdfParser::ParseFromFile(const AZStd::string& filePath) + RootObjectOutcome ParseFromFile( + AZ::IO::PathView filePath, const sdf::ParserConfig& parserConfig, const SdfAssetBuilderSettings& settings) { - std::ifstream istream(filePath.c_str()); + // Store path in a AZ::IO::FixedMaxPath which is stack based structure that provides memory + // for the path string and is null terminated. + // It is backed by an AZStd::fixed_string<1024> which is a char buffer of 1025 internally + AZ::IO::FixedMaxPath urdfFilePath = filePath.FixedMaxPathString(); + std::ifstream istream(urdfFilePath.c_str()); if (!istream) { - AZ_Error("UrdfParser", false, "File %s does not exist", filePath.c_str()); - return nullptr; + auto fileNotFoundMessage = AZStd::fixed_string<1024>::format("File %.*s does not exist", AZ_PATH_ARG(urdfFilePath)); + ParseResult fileNotFoundResult; + fileNotFoundResult.m_sdfErrors.emplace_back( + sdf::ErrorCode::FILE_READ, + std::string{ fileNotFoundMessage.c_str(), fileNotFoundMessage.size() }, + std::string{ urdfFilePath.c_str(), urdfFilePath.Native().size() }); + return fileNotFoundResult; } std::string xmlStr((std::istreambuf_iterator(istream)), std::istreambuf_iterator()); - return Parse(xmlStr.c_str()); - } + if (Utils::IsFileUrdf(filePath) && settings.m_fixURDF) + { + // modify in memory + auto [modifiedXmlStr, modifiedElements] = (ROS2::Utils::ModifyURDFInMemory(xmlStr)); - AZStd::string UrdfParser::GetUrdfParsingLog() - { - return Internal::customConsoleHandler.GetLog(); + auto result = Parse(modifiedXmlStr, parserConfig); + result.m_urdfModifications = AZStd::move(modifiedElements); + result.m_modifiedURDFContent = AZStd::move(modifiedXmlStr); + return result; + } + return Parse(xmlStr, parserConfig); } -} // namespace ROS2 +} // namespace ROS2::UrdfParser diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h index 094aeb699..7231c6169 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h @@ -8,27 +8,102 @@ #pragma once -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -namespace ROS2 +namespace ROS2::UrdfParser { - //! Class for parsing URDF data. - namespace UrdfParser + //! Functions for parsing URDF/SDF data. + + //! Stores the result of parsing URDF/SDF data + //! The operator bool returns true if the sdf::Errors vector is empty + struct ParseResult { - //! Parse string with URDF data and generate model. - //! @param xmlString a string that contains URDF data (XML format). - //! @return model represented as a tree of parsed links. - urdf::ModelInterfaceSharedPtr Parse(const AZStd::string& xmlString); - - //! Parse file with URDF data and generate model. - //! @param filePath is a path to file with URDF data that will be loaded and parsed. - //! @return model represented as a tree of parsed links. - urdf::ModelInterfaceSharedPtr ParseFromFile(const AZStd::string& filePath); - - //! Retrieve console log from URDF parsing - //! @return a log with output from urdf_parser - AZStd::string GetUrdfParsingLog(); - - }; // namespace UrdfParser -} // namespace ROS2 \ No newline at end of file + private: + // Provides custom sdf::ErrorCode values when parsing is done through O3DE + inline static constexpr auto O3DESdfErrorCodeStart = static_cast(1000); + + public: + inline static constexpr auto O3DESdfErrorParseNotStarted = static_cast(static_cast(O3DESdfErrorCodeStart) + 1); + + //! Ref qualifier overloads for retrieving sdf::Root + //! it supports a non-const lvalue overload to allow + //! modification of the sdf::Root object directly + sdf::Root& GetRoot() &; + const sdf::Root& GetRoot() const&; + sdf::Root&& GetRoot() &&; + + //! Property getters for retrieving parsing messages + //! logged during libsdformat parsing of the URDF content + //! This does not support a non-const lvalue overload + //! As modification the result structure parser messages + //! have no need to be modified inplace + const AZStd::string& GetParseMessages() const&; + AZStd::string&& GetParseMessages() &&; + + //! Returns the sdf::Error vector containing any parser errors + //! This does not support a non-const lvalue overload + //! As modification the result structure sdf Errors + //! have no need to be modified inplace + const sdf::Errors& GetSdfErrors() const&; + sdf::Errors&& GetSdfErrors() &&; + + //! Returns if the parsing of the SDF file has succeeded + //! This will return false if the sdf::Errors vector is non-empty + explicit operator bool() const; + + //! Returns if the URDF content was modified during parsing + bool UrdfParsedWithModifiedContent() const; + + //! Root SDF object containing parsed SDF XML content if successful + sdf::Root m_root; + //! Contains any messages that are output to the sdf::Console during parsing + //! These messages are captured and provided to the caller to allow outputting in + //! any manner they choose + AZStd::string m_parseMessages; + //! Vector structure populated with sdf::Error objects indicating that parsing of the .sdf/.urdf + //! file has failed with an error + sdf::Errors m_sdfErrors{ sdf::Error{ O3DESdfErrorParseNotStarted, std::string{ "No Parsing has occurred yet" } } }; + + //! Stores the modified URDF content after parsing, empty if no modification occurred + std::string m_modifiedURDFContent; + + //! Stores description of URDF modifications, empty if no modification occurred + Utils::UrdfModifications m_urdfModifications; + }; + using RootObjectOutcome = ParseResult; + + //! Parse string with URDF/SDF data and generate root SDF object. + //! @param xmlString a string that contains URDF data (XML format). + //! @param parserConfig structure that contains configuration options for the SDFormatter parser + //! The relevant ParserConfig functions for URDF importing are + //! URDFPreserveFixedJoint() function to prevent merging of robot links bound by fixed joint + //! AddURIPath() function to provide a mapping of package:// and model:// references to the local filesystem + //! @return SDF root object containing parsed or tags + RootObjectOutcome Parse(AZStd::string_view xmlString, const sdf::ParserConfig& parserConfig); + RootObjectOutcome Parse(const std::string& xmlString, const sdf::ParserConfig& parserConfig); + + //! Parse file with URDF data and generate model. + //! @param filePath is a path to file with URDF data that will be loaded and parsed. + //! @param parserConfig structure that contains configuration options for the SDFormater parser + //! The relevant ParserConfig functions for URDF importing are + //! URDFPreserveFixedJoint() function to prevent merging of robot links bound by fixed joint + //! AddURIPath() function to provide a mapping of package:// and model:// references to the local filesystem + //! @param settings structure that contains configuration options for the SDFAssetBuilder + //! @return SDF root object containing parsed or tags + RootObjectOutcome ParseFromFile( + AZ::IO::PathView filePath, const sdf::ParserConfig& parserConfig, const SdfAssetBuilderSettings& settings); +} // namespace ROS2::UrdfParser diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp index ca8e02e64..79a47b0b5 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp @@ -10,7 +10,9 @@ #include "RobotImporter/URDF/PrefabMakerUtils.h" #include "RobotImporter/Utils/TypeConversions.h" +#include #include +#include #include #include #include @@ -18,209 +20,529 @@ #include #include #include -#include -#include -#include -#include + +#include +#include namespace ROS2 { - VisualsMaker::VisualsMaker( - const std::map& materials, const AZStd::shared_ptr& urdfAssetsMapping) + VisualsMaker::VisualsMaker() = default; + VisualsMaker::VisualsMaker(const AZStd::shared_ptr& urdfAssetsMapping) : m_urdfAssetsMapping(urdfAssetsMapping) { - AZStd::ranges::for_each( - materials, - [&](const auto& p) - { - m_materials[AZStd::string(p.first.c_str(), p.first.size())] = p.second; - }); } - void VisualsMaker::AddVisuals(urdf::LinkSharedPtr link, AZ::EntityId entityId) const + AZStd::vector VisualsMaker::AddVisuals(const sdf::Link* link, AZ::EntityId entityId) const { + AZStd::vector createdEntities; + const AZStd::string typeString = "visual"; - if (link->visual_array.size() < 1) - { // For zero visuals, element is used - AddVisual(link->visual, entityId, PrefabMakerUtils::MakeEntityName(link->name.c_str(), typeString)); - return; + if (link->VisualCount() < 1) + { + // For zero visuals, element is used + auto createdEntity = AddVisual(nullptr, entityId, PrefabMakerUtils::MakeEntityName(link->Name().c_str(), typeString)); + if (createdEntity.IsValid()) + { + createdEntities.emplace_back(createdEntity); + } } - size_t nameSuffixIndex = 0; // For disambiguation when multiple unnamed visuals are present. The order does not matter here + else + { + // For one or more visuals, an array is used + size_t nameSuffixIndex = 0; // For disambiguation when multiple unnamed visuals are present. The order does not matter here - for (auto visual : link->visual_array) - { // For one or more visuals, an array is used - AddVisual(visual, entityId, PrefabMakerUtils::MakeEntityName(link->name.c_str(), typeString, nameSuffixIndex)); - nameSuffixIndex++; + for (uint64_t index = 0; index < link->VisualCount(); index++) + { + auto createdEntity = AddVisual( + link->VisualByIndex(index), + entityId, + PrefabMakerUtils::MakeEntityName(link->Name().c_str(), typeString, nameSuffixIndex)); + if (createdEntity.IsValid()) + { + createdEntities.emplace_back(createdEntity); + } + nameSuffixIndex++; + } } + return createdEntities; } - void VisualsMaker::AddVisual(urdf::VisualSharedPtr visual, AZ::EntityId entityId, const AZStd::string& generatedName) const + AZ::EntityId VisualsMaker::AddVisual(const sdf::Visual* visual, AZ::EntityId entityId, const AZStd::string& generatedName) const { if (!visual) { // It is ok not to have a visual in a link - return; + return AZ::EntityId(); } - if (!visual->geometry) + if (!visual->Geom()) { // Non-empty visual should have a geometry. Warn if no geometry present AZ_Warning("AddVisual", false, "No Geometry for a visual"); - return; + return AZ::EntityId(); } - AZ_TracePrintf("AddVisual", "Processing visual for entity id:%s\n", entityId.ToString().c_str()); + AZ_Trace("AddVisual", "Processing visual for entity id:%s\n", entityId.ToString().c_str()); // Use a name generated from the link unless specific name is defined for this visual - const char* subEntityName = visual->name.empty() ? generatedName.c_str() : visual->name.c_str(); + AZStd::string subEntityName = visual->Name().empty() ? generatedName.c_str() : visual->Name().c_str(); // Since O3DE does not allow origin for visuals, we need to create a sub-entity and store visual there - auto createEntityResult = PrefabMakerUtils::CreateEntity(entityId, subEntityName); + auto createEntityResult = PrefabMakerUtils::CreateEntity(entityId, subEntityName.c_str()); if (!createEntityResult.IsSuccess()) { - AZ_Error("AddVisual", false, "Unable to create a sub-entity for visual element %s\n", subEntityName); - return; + AZ_Error("AddVisual", false, "Unable to create a sub-entity for visual element %s\n", subEntityName.c_str()); + return AZ::EntityId(); } auto visualEntityId = createEntityResult.GetValue(); - AddVisualToEntity(visual, visualEntityId); - AddMaterialForVisual(visual, visualEntityId); + auto visualAssetId = AddVisualToEntity(visual, visualEntityId); + AddMaterialForVisual(visual, visualEntityId, visualAssetId); + return visualEntityId; } - void VisualsMaker::AddVisualToEntity(urdf::VisualSharedPtr visual, AZ::EntityId entityId) const + AZ::Data::AssetId VisualsMaker::AddVisualToEntity(const sdf::Visual* visual, AZ::EntityId entityId) const { + // Asset ID for the asset added to the visual entity, if any. + AZ::Data::AssetId assetId; + // Apply transform as per origin - PrefabMakerUtils::SetEntityTransformLocal(visual->origin, entityId); + PrefabMakerUtils::SetEntityTransformLocal(visual->RawPose(), entityId); - AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId); - auto geometry = visual->geometry; - switch (geometry->type) + auto geometry = visual->Geom(); + switch (geometry->Type()) { - case urdf::Geometry::SPHERE: + case sdf::GeometryType::SPHERE: { - auto sphereGeometry = std::dynamic_pointer_cast(geometry); + auto sphereGeometry = geometry->SphereShape(); AZ_Assert(sphereGeometry, "geometry is not Sphere"); - entity->CreateComponent(LmbrCentral::EditorSphereShapeComponentTypeId); - entity->Activate(); - LmbrCentral::SphereShapeComponentRequestsBus::Event( - entityId, &LmbrCentral::SphereShapeComponentRequests::SetRadius, sphereGeometry->radius); - LmbrCentral::EditorShapeComponentRequestsBus::Event( - entityId, &LmbrCentral::EditorShapeComponentRequests::SetVisibleInGame, true); - entity->Deactivate(); + // Convert radius to diameter: the `_sphere_1x1.fbx.azmodel` model has a diameter of 1 + const AZ::Vector3 sphereDimensions(sphereGeometry->Radius() * 2.0f); + + // The `_sphere_1x1.fbx.azmodel` is created by Asset Processor based on O3DE `PrimitiveAssets` Gem source. + const char* sphereAssetRelPath = "objects/_primitives/_sphere_1x1.fbx.azmodel"; // relative path to cache folder. + AZ::Data::AssetCatalogRequestBus::BroadcastResult( + assetId, + &AZ::Data::AssetCatalogRequestBus::Events::GetAssetIdByPath, + sphereAssetRelPath, + AZ::Data::s_invalidAssetType, + false); + AZ_Warning("AddVisual", assetId.IsValid(), "There is no product asset for %s.", sphereAssetRelPath); + + AddVisualAssetToEntity(entityId, assetId, sphereDimensions); } break; - case urdf::Geometry::CYLINDER: + case sdf::GeometryType::CYLINDER: { - auto cylinderGeometry = std::dynamic_pointer_cast(geometry); + auto cylinderGeometry = geometry->CylinderShape(); AZ_Assert(cylinderGeometry, "geometry is not Cylinder"); - entity->CreateComponent(LmbrCentral::EditorCylinderShapeComponentTypeId); - entity->Activate(); - LmbrCentral::CylinderShapeComponentRequestsBus::Event( - entityId, &LmbrCentral::CylinderShapeComponentRequests::SetRadius, cylinderGeometry->radius); - LmbrCentral::CylinderShapeComponentRequestsBus::Event( - entityId, &LmbrCentral::CylinderShapeComponentRequests::SetHeight, cylinderGeometry->length); - LmbrCentral::EditorShapeComponentRequestsBus::Event( - entityId, &LmbrCentral::EditorShapeComponentRequests::SetVisibleInGame, true); - entity->Deactivate(); + // Convert radius to diameter: the `_cylinder_1x1.fbx.azmodel` model has a diameter of 1 + const AZ::Vector3 cylinderDimensions( + cylinderGeometry->Radius() * 2.0f, cylinderGeometry->Radius() * 2.0f, cylinderGeometry->Length()); + + // The `_cylinder_1x1.fbx.azmodel` is created by Asset Processor based on O3DE `PrimitiveAssets` Gem source. + const char* cylinderAssetRelPath = "objects/_primitives/_cylinder_1x1.fbx.azmodel"; // relative path to cache folder. + AZ::Data::AssetCatalogRequestBus::BroadcastResult( + assetId, + &AZ::Data::AssetCatalogRequestBus::Events::GetAssetIdByPath, + cylinderAssetRelPath, + AZ::Data::s_invalidAssetType, + false); + AZ_Warning("AddVisual", assetId.IsValid(), "There is no product asset for %s.", cylinderAssetRelPath); + + AddVisualAssetToEntity(entityId, assetId, cylinderDimensions); } break; - case urdf::Geometry::BOX: + case sdf::GeometryType::BOX: { - auto boxGeometry = std::dynamic_pointer_cast(geometry); + auto boxGeometry = geometry->BoxShape(); AZ_Assert(boxGeometry, "geometry is not Box"); - entity->CreateComponent(LmbrCentral::EditorBoxShapeComponentTypeId); - AZ::Vector3 boxDimensions = URDF::TypeConversions::ConvertVector3(boxGeometry->dim); - entity->Activate(); - LmbrCentral::BoxShapeComponentRequestsBus::Event( - entityId, &LmbrCentral::BoxShapeComponentRequests::SetBoxDimensions, boxDimensions); - LmbrCentral::EditorShapeComponentRequestsBus::Event( - entityId, &LmbrCentral::EditorShapeComponentRequests::SetVisibleInGame, true); - entity->Deactivate(); + const AZ::Vector3 boxDimensions = URDF::TypeConversions::ConvertVector3(boxGeometry->Size()); + + // The `_box_1x1.fbx.azmodel` is created by Asset Processor based on O3DE `PrimitiveAssets` Gem source. + const char* boxAssetRelPath = "objects/_primitives/_box_1x1.fbx.azmodel"; // relative path to cache folder. + AZ::Data::AssetCatalogRequestBus::BroadcastResult( + assetId, + &AZ::Data::AssetCatalogRequestBus::Events::GetAssetIdByPath, + boxAssetRelPath, + AZ::Data::s_invalidAssetType, + false); + AZ_Warning("AddVisual", assetId.IsValid(), "There is no product asset for %s.", boxAssetRelPath); + + AddVisualAssetToEntity(entityId, assetId, boxDimensions); } break; - case urdf::Geometry::MESH: + case sdf::GeometryType::MESH: { - auto meshGeometry = std::dynamic_pointer_cast(geometry); + auto meshGeometry = geometry->MeshShape(); AZ_Assert(meshGeometry, "geometry is not Mesh"); + const AZ::Vector3 scaleVector = URDF::TypeConversions::ConvertVector3(meshGeometry->Scale()); - const auto asset = PrefabMakerUtils::GetAssetFromPath(*m_urdfAssetsMapping, meshGeometry->filename); + const auto asset = PrefabMakerUtils::GetAssetFromPath(*m_urdfAssetsMapping, AZStd::string(meshGeometry->Uri().c_str())); + AZ_Warning("AddVisual", asset, "There is no source asset for %s.", meshGeometry->Uri().c_str()); if (asset) { - entity->CreateComponent(AZ::Render::EditorMeshComponentTypeId); + assetId = Utils::GetModelProductAssetId(asset->m_sourceGuid); + AZ_Warning("AddVisual", assetId.IsValid(), "There is no product asset for %s.", asset->m_sourceAssetRelativePath.c_str()); + } + + AddVisualAssetToEntity(entityId, assetId, scaleVector); + } + break; + default: + AZ_Warning("AddVisual", false, "Unsupported visual geometry type, %d", (int)geometry->Type()); + break; + } + + return assetId; + } + + void VisualsMaker::AddVisualAssetToEntity(AZ::EntityId entityId, const AZ::Data::AssetId& assetId, const AZ::Vector3& scale) const + { + if (!assetId.IsValid()) + { + return; + } + + AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId); + auto editorMeshComponent = entity->CreateComponent(AZ::Render::EditorMeshComponentTypeId); + + // Prepare scale + bool isUniformScale = AZ::IsClose(scale.GetMaxElement(), scale.GetMinElement(), AZ::Constants::FloatEpsilon); + if (!isUniformScale) + { + entity->CreateComponent(); + } + + if (editorMeshComponent) + { + auto editorBaseComponent = azrtti_cast(editorMeshComponent); + AZ_Assert(editorBaseComponent, "EditorMeshComponent didn't derive from EditorComponentBase."); + editorBaseComponent->SetPrimaryAsset(assetId); + } + + entity->Activate(); + + // Set scale, uniform or non-uniform + if (isUniformScale) + { + AZ::TransformBus::Event(entityId, &AZ::TransformBus::Events::SetLocalUniformScale, scale.GetX()); + } + else + { + AZ::NonUniformScaleRequestBus::Event(entityId, &AZ::NonUniformScaleRequests::SetScale, scale); + } + entity->Deactivate(); + } + + static void OverrideScriptMaterial(const sdf::Material* material, AZ::Render::MaterialAssignmentMap& overrides) + { + AZStd::string materialName(material->ScriptName().c_str(), material->ScriptName().size()); + if (materialName.empty()) + { + return; + } + + // Make sure the material name is lowercased before checking the path in the Asset Cache + AZStd::to_lower(materialName); + // If a material has a