├── .clang-format ├── .gitignore ├── CMakeLists.txt ├── Code ├── CMakeLists.txt ├── FindRGL.cmake ├── Include │ └── RGL │ │ ├── RGLBus.h │ │ └── SceneConfiguration.h ├── Platform │ └── Common │ │ ├── Clang │ │ └── rgl_static_clang.cmake │ │ ├── GCC │ │ └── rgl_static_gcc.cmake │ │ └── msvc │ │ └── rgl_static_msvc.cmake ├── Source │ ├── Entity │ │ ├── ActorEntityManager.cpp │ │ ├── ActorEntityManager.h │ │ ├── EntityManager.cpp │ │ ├── EntityManager.h │ │ ├── MaterialEntityManager.cpp │ │ ├── MaterialEntityManager.h │ │ ├── MeshEntityManager.cpp │ │ ├── MeshEntityManager.h │ │ └── Terrain │ │ │ ├── TerrainData.cpp │ │ │ ├── TerrainData.h │ │ │ ├── TerrainEntityManagerEditorSystemComponent.cpp │ │ │ ├── TerrainEntityManagerEditorSystemComponent.h │ │ │ ├── TerrainEntityManagerSystemComponent.cpp │ │ │ └── TerrainEntityManagerSystemComponent.h │ ├── Lidar │ │ ├── LidarRaycaster.cpp │ │ ├── LidarRaycaster.h │ │ ├── LidarSystem.cpp │ │ ├── LidarSystem.h │ │ ├── LidarSystemNotificationBus.h │ │ ├── PipelineGraph.cpp │ │ └── PipelineGraph.h │ ├── Model │ │ ├── ModelLibrary.cpp │ │ ├── ModelLibrary.h │ │ └── ModelLibraryBus.h │ ├── RGLEditorModule.cpp │ ├── RGLEditorSystemComponent.cpp │ ├── RGLEditorSystemComponent.h │ ├── RGLModule.cpp │ ├── RGLModuleInterface.h │ ├── RGLSystemComponent.cpp │ ├── RGLSystemComponent.h │ ├── SceneConfiguration.cpp │ ├── SceneConfigurationComponent.cpp │ ├── SceneConfigurationComponent.h │ ├── Utilities │ │ ├── RGLUtils.cpp │ │ └── RGLUtils.h │ └── Wrappers │ │ ├── RglEntity.cpp │ │ ├── RglEntity.h │ │ ├── RglMesh.cpp │ │ ├── RglMesh.h │ │ ├── RglTexture.cpp │ │ └── RglTexture.h ├── rgl_editor_files.cmake ├── rgl_editor_shared_files.cmake ├── rgl_files.cmake ├── rgl_header_files.cmake └── rgl_shared_files.cmake ├── LICENSE ├── README.md ├── Registry └── assetprocessor_settings.setreg ├── gem.json ├── preview.png └── static ├── PipelineGraph.mmd ├── gif ├── rgl_gem_preview1.gif └── rgl_gem_preview2.gif └── png ├── excluded_entities1.png ├── excluded_entities2.png ├── excluded_entities3.png ├── lidar_sphere.png ├── usage_instruction1.png ├── usage_instruction2.png └── usage_instruction3.png /.clang-format: -------------------------------------------------------------------------------- 1 | Language: Cpp 2 | 3 | AccessModifierOffset: -4 4 | AlignAfterOpenBracket: AlwaysBreak 5 | AlignConsecutiveAssignments: false 6 | AlignConsecutiveDeclarations: false 7 | AlignEscapedNewlines: Right 8 | AlignOperands: false 9 | AlignTrailingComments: false 10 | AllowAllArgumentsOnNextLine: true 11 | AllowAllParametersOfDeclarationOnNextLine: true 12 | AllowShortFunctionsOnASingleLine: None 13 | AllowShortLambdasOnASingleLine: None 14 | AlwaysBreakAfterReturnType: None 15 | AlwaysBreakTemplateDeclarations: true 16 | BinPackArguments: false 17 | BinPackParameters: false 18 | BreakBeforeBraces: Custom 19 | BraceWrapping: 20 | AfterClass: true 21 | AfterControlStatement: true 22 | AfterEnum: true 23 | AfterFunction: true 24 | AfterNamespace: true 25 | BeforeLambdaBody: true 26 | AfterStruct: true 27 | BeforeElse: true 28 | SplitEmptyFunction: true 29 | BreakBeforeTernaryOperators: true 30 | BreakConstructorInitializers: BeforeComma 31 | BreakInheritanceList: BeforeComma 32 | ColumnLimit: 140 33 | ConstructorInitializerIndentWidth: 4 34 | ContinuationIndentWidth: 4 35 | Cpp11BracedListStyle: false 36 | FixNamespaceComments: true 37 | IncludeBlocks: Preserve 38 | IndentCaseBlocks: true 39 | IndentCaseLabels: false 40 | IndentPPDirectives: None 41 | IndentWidth: 4 42 | KeepEmptyLinesAtTheStartOfBlocks: false 43 | MaxEmptyLinesToKeep: 1 44 | NamespaceIndentation: All 45 | PenaltyReturnTypeOnItsOwnLine: 1000 46 | PointerAlignment: Left 47 | SortIncludes: true 48 | SpaceAfterLogicalNot: false 49 | SpaceAfterTemplateKeyword: false 50 | SpaceBeforeAssignmentOperators: true 51 | SpaceBeforeCpp11BracedList: false 52 | SpaceBeforeCtorInitializerColon: true 53 | SpaceBeforeInheritanceColon: true 54 | SpaceBeforeParens: ControlStatements 55 | SpaceBeforeRangeBasedForLoopColon: true 56 | SpaceInEmptyParentheses: false 57 | SpacesInAngles: false 58 | SpacesInCStyleCastParentheses: false 59 | SpacesInParentheses: false 60 | Standard: c++17 61 | UseTab: Never -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | Code/3rdParty 2 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/Code") 2 | 3 | set(gem_path ${CMAKE_CURRENT_LIST_DIR}) 4 | set(gem_json ${gem_path}/gem.json) 5 | o3de_restricted_path(${gem_json} gem_restricted_path gem_parent_relative_path) 6 | 7 | ly_add_external_target_path(${CMAKE_CURRENT_LIST_DIR}/3rdParty) 8 | 9 | add_subdirectory(Code) 10 | -------------------------------------------------------------------------------- /Code/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | include(FindRGL.cmake) 16 | ly_add_target( 17 | NAME RGL.Static STATIC 18 | NAMESPACE Gem 19 | FILES_CMAKE 20 | rgl_header_files.cmake 21 | rgl_files.cmake 22 | PLATFORM_INCLUDE_FILES 23 | ${CMAKE_CURRENT_LIST_DIR}/Platform/Common/${PAL_TRAIT_COMPILER_ID}/rgl_static_${PAL_TRAIT_COMPILER_ID_LOWERCASE}.cmake 24 | INCLUDE_DIRECTORIES 25 | PUBLIC 26 | Include 27 | ${RGL_INCLUDE_DIR} 28 | PRIVATE 29 | Source 30 | BUILD_DEPENDENCIES 31 | PUBLIC 32 | AZ::AzCore 33 | AZ::AzFramework 34 | Gem::Atom_RPI.Public 35 | Gem::AtomLyIntegration_CommonFeatures.Static 36 | Gem::ROS2.Static 37 | Gem::EMotionFX.Static 38 | ) 39 | 40 | ly_target_link_libraries(RGL.Static PUBLIC ${RGL_SO_DIR}) 41 | 42 | ly_add_target_files( 43 | TARGETS 44 | RGL.Static 45 | FILES 46 | ${RGL_SO_DIR} 47 | ) 48 | 49 | ly_add_target( 50 | NAME RGL.API HEADERONLY 51 | NAMESPACE Gem 52 | FILES_CMAKE 53 | rgl_header_files.cmake 54 | INCLUDE_DIRECTORIES 55 | INTERFACE 56 | Include 57 | ) 58 | 59 | # Here add RGL target, it depends on the RGL.Static 60 | ly_add_target( 61 | NAME RGL ${PAL_TRAIT_MONOLITHIC_DRIVEN_MODULE_TYPE} 62 | NAMESPACE Gem 63 | FILES_CMAKE 64 | rgl_shared_files.cmake 65 | INCLUDE_DIRECTORIES 66 | PUBLIC 67 | Include 68 | PRIVATE 69 | Source 70 | BUILD_DEPENDENCIES 71 | PRIVATE 72 | Gem::RGL.Static 73 | ) 74 | 75 | # By default, we will specify that the above target RGL would be used by 76 | # Client and Server type targets when this gem is enabled. If you don't want it 77 | # active in Clients or Servers by default, delete one of both of the following lines: 78 | ly_create_alias(NAME RGL.Clients NAMESPACE Gem TARGETS Gem::RGL) 79 | ly_create_alias(NAME RGL.Servers NAMESPACE Gem TARGETS Gem::RGL) 80 | 81 | if(PAL_TRAIT_BUILD_HOST_TOOLS) 82 | ly_add_target( 83 | NAME RGL.Editor.Static STATIC 84 | NAMESPACE Gem 85 | FILES_CMAKE 86 | rgl_editor_files.cmake 87 | PLATFORM_INCLUDE_FILES 88 | ${CMAKE_CURRENT_LIST_DIR}/Platform/Common/${PAL_TRAIT_COMPILER_ID}/rgl_static_${PAL_TRAIT_COMPILER_ID_LOWERCASE}.cmake 89 | INCLUDE_DIRECTORIES 90 | PRIVATE 91 | Source 92 | PUBLIC 93 | Include 94 | BUILD_DEPENDENCIES 95 | PUBLIC 96 | AZ::AzToolsFramework 97 | Gem::RGL.Static 98 | ) 99 | 100 | ly_add_target( 101 | NAME RGL.Editor GEM_MODULE 102 | NAMESPACE Gem 103 | AUTOMOC 104 | FILES_CMAKE 105 | rgl_editor_shared_files.cmake 106 | INCLUDE_DIRECTORIES 107 | PRIVATE 108 | Source 109 | PUBLIC 110 | Include 111 | BUILD_DEPENDENCIES 112 | PUBLIC 113 | Gem::RGL.Editor.Static 114 | ) 115 | 116 | # By default, we will specify that the above target RGL would be used by 117 | # Tool and Builder type targets when this gem is enabled. If you don't want it 118 | # active in Tools or Builders by default, delete one of both of the following lines: 119 | ly_create_alias(NAME RGL.Tools NAMESPACE Gem TARGETS Gem::RGL.Editor) 120 | ly_create_alias(NAME RGL.Builders NAMESPACE Gem TARGETS Gem::RGL.Editor) 121 | endif() 122 | -------------------------------------------------------------------------------- /Code/FindRGL.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | set(RGL_VERSION 0.19.0) 15 | set(RGL_TAG v${RGL_VERSION}) 16 | 17 | # Metadata files used to determine if RGL download is required 18 | set(RGL_VERSION_METADATA_FILE ${CMAKE_CURRENT_BINARY_DIR}/RGL_VERSION) 19 | set(ROS_DISTRO_METADATA_FILE ${CMAKE_CURRENT_BINARY_DIR}/ROS_DISTRO) 20 | 21 | # Determine RGL binary to download based on ROS distro 22 | set(ROS_DISTRO $ENV{ROS_DISTRO}) 23 | if($ENV{ROS_DISTRO} STREQUAL "humble") 24 | set(RGL_LINUX_ZIP_FILENAME_BASE RGL-full-linux-x64-humble) 25 | elseif($ENV{ROS_DISTRO} STREQUAL "jazzy") 26 | set(RGL_LINUX_ZIP_FILENAME_BASE RGL-full-linux-x64-jazzy) 27 | else() 28 | message(FATAL_ERROR "ROS not found or ROS distro not supported. Please use one of {humble, jazzy}.") 29 | endif () 30 | set(RGL_LINUX_ZIP_FILENAME ${RGL_LINUX_ZIP_FILENAME_BASE}.zip) 31 | 32 | set(RGL_LINUX_ZIP_URL https://github.com/RobotecAI/RobotecGPULidar/releases/download/${RGL_TAG}/${RGL_LINUX_ZIP_FILENAME}) 33 | set(RGL_SRC_ROOT_URL https://raw.githubusercontent.com/RobotecAI/RobotecGPULidar/${RGL_TAG}) 34 | 35 | set(DEST_SO_DIR ${CMAKE_CURRENT_BINARY_DIR}/3rdParty/RobotecGPULidar) 36 | set(DEST_API_DIR ${DEST_SO_DIR}/include/rgl/api) 37 | 38 | set(SO_FILENAME libRobotecGPULidar.so) 39 | 40 | set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) 41 | 42 | # This check is performed to mitigate Clion multi-profile project reload issues 43 | # (each profile would execute the file download, extraction and removal without it). 44 | # Note: This check does not provide a full assurance (not atomic) but is good enough 45 | # since this is a Clion-specific issue. 46 | set(RGL_DOWNLOAD_IN_PROGRESS_FILE ${CMAKE_CURRENT_BINARY_DIR}/RGL_DOWNLOAD_IN_PROGRESS) 47 | if (NOT EXISTS ${RGL_DOWNLOAD_IN_PROGRESS_FILE}) 48 | FILE(TOUCH ${RGL_DOWNLOAD_IN_PROGRESS_FILE}) 49 | 50 | # Read metadata 51 | set(RGL_VERSION_METADATA " ") 52 | set(ROS_DISTRO_METADATA " ") 53 | if (EXISTS ${RGL_VERSION_METADATA_FILE}) 54 | file(READ ${RGL_VERSION_METADATA_FILE} RGL_VERSION_METADATA) 55 | endif () 56 | if (EXISTS ${ROS_DISTRO_METADATA_FILE}) 57 | file(READ ${ROS_DISTRO_METADATA_FILE} ROS_DISTRO_METADATA) 58 | endif () 59 | 60 | # If metadata does not match, download RGL 61 | if ((NOT ${RGL_VERSION_METADATA} STREQUAL ${RGL_VERSION}) OR (NOT ${ROS_DISTRO_METADATA} STREQUAL ${ROS_DISTRO})) 62 | message("Downloading RGL " ${RGL_VERSION} " for ROS " ${ROS_DISTRO} "...") 63 | 64 | # Download the RGL archive files 65 | file(DOWNLOAD 66 | ${RGL_LINUX_ZIP_URL} 67 | ${DEST_SO_DIR}/${RGL_LINUX_ZIP_FILENAME} 68 | ) 69 | 70 | # Extract the contents of the downloaded archive files 71 | file(ARCHIVE_EXTRACT INPUT ${DEST_SO_DIR}/${RGL_LINUX_ZIP_FILENAME} 72 | DESTINATION ${DEST_SO_DIR} 73 | PATTERNS ${SO_FILENAME} 74 | VERBOSE 75 | ) 76 | 77 | # Remove the unwanted byproducts 78 | file(REMOVE ${DEST_SO_DIR}/${RGL_LINUX_ZIP_FILENAME}) 79 | 80 | # Download API headers 81 | file(DOWNLOAD 82 | ${RGL_SRC_ROOT_URL}/include/rgl/api/core.h 83 | ${DEST_API_DIR}/core.h 84 | ) 85 | file(DOWNLOAD 86 | ${RGL_SRC_ROOT_URL}/extensions/ros2/include/rgl/api/extensions/ros2.h 87 | ${DEST_API_DIR}/extensions/ros2.h 88 | ) 89 | 90 | # Save current metadata 91 | file(WRITE ${RGL_VERSION_METADATA_FILE} ${RGL_VERSION}) 92 | file(WRITE ${ROS_DISTRO_METADATA_FILE} ${ROS_DISTRO}) 93 | endif () 94 | 95 | # Remove the unwanted byproducts 96 | file(REMOVE ${RGL_DOWNLOAD_IN_PROGRESS_FILE}) 97 | else () 98 | message(WARNING "Omitting the RobotecGPULidar library download. This is intended when using the Clion multi-profile" 99 | " project reload. This may also happen due to interruption of previous project configurations. If you have" 100 | " any issues related to the libRobotecGPULidar.so file please clear cmake cache before next build attempt." 101 | ) 102 | endif () 103 | 104 | # Paths used by external targets 105 | set(RGL_SO_DIR ${DEST_SO_DIR}/${SO_FILENAME}) 106 | set(RGL_INCLUDE_DIR ${DEST_SO_DIR}/include) 107 | -------------------------------------------------------------------------------- /Code/Include/RGL/RGLBus.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace RGL 23 | { 24 | class RGLRequests 25 | { 26 | public: 27 | AZ_RTTI(RGLRequests, "{f301342c-3b17-11ed-a261-0242ac120002}"); 28 | 29 | //! Excludes an entity from raycasting. 30 | //! @param excludedEntityId EntityId of the excluded entity. 31 | virtual void ExcludeEntity(const AZ::EntityId& excludedEntityId) = 0; 32 | 33 | virtual void SetSceneConfiguration(const SceneConfiguration& config) = 0; 34 | [[nodiscard]] virtual const SceneConfiguration& GetSceneConfiguration() const = 0; 35 | 36 | //! Updates scene to the RGL 37 | virtual void UpdateScene() = 0; 38 | 39 | protected: 40 | ~RGLRequests() = default; 41 | }; 42 | 43 | class RGLBusTraits : public AZ::EBusTraits 44 | { 45 | public: 46 | ////////////////////////////////////////////////////////////////////////// 47 | // EBusTraits overrides 48 | static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Single; 49 | static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::Single; 50 | ////////////////////////////////////////////////////////////////////////// 51 | }; 52 | 53 | using RGLRequestBus = AZ::EBus; 54 | using RGLInterface = AZ::Interface; 55 | 56 | class RGLNotifications : public AZ::EBusTraits 57 | { 58 | public: 59 | ////////////////////////////////////////////////////////////////////////// 60 | // EBusTraits overrides 61 | static const AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Multiple; 62 | static const AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::Single; 63 | ////////////////////////////////////////////////////////////////////////// 64 | 65 | virtual void OnSceneConfigurationSet(const SceneConfiguration& config) 66 | { 67 | } 68 | 69 | //! Signals that at least one lidar that uses RGL implementation exists. 70 | //! Used for GPU memory consumption optimizations. 71 | virtual void OnAnyLidarExists() {} 72 | 73 | //! Signals that the last lidar that used RGL implementation was destroyed. 74 | //! Used for GPU memory consumption optimizations. 75 | virtual void OnNoLidarExists() {} 76 | ////////////////////////////////////////////////////////////////////////// 77 | }; 78 | using RGLNotificationBus = AZ::EBus; 79 | } // namespace RGL 80 | -------------------------------------------------------------------------------- /Code/Include/RGL/SceneConfiguration.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2024, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | namespace RGL 21 | { 22 | //! Structure used to describe global terrain intensity configuration. 23 | struct TerrainIntensityConfiguration 24 | { 25 | AZ_TYPE_INFO(TerrainIntensityConfiguration, "{6cf06491-3d18-4aad-88f6-d1990d6f791f}"); 26 | static void Reflect(AZ::ReflectContext* context); 27 | 28 | AZ::Data::Asset m_colorImageAsset{ AZ::Data::AssetLoadBehavior::QueueLoad }; 29 | AZ::u8 m_defaultValue{ 0U }; 30 | bool m_isTiled{ true }; 31 | }; 32 | 33 | //! Structure used to describe all global scene parameters. 34 | struct SceneConfiguration 35 | { 36 | AZ_TYPE_INFO(SceneConfiguration, "{7e55de90-e26c-4567-9e06-822c6ce62b9c}"); 37 | static void Reflect(AZ::ReflectContext* context); 38 | 39 | TerrainIntensityConfiguration m_terrainIntensityConfig; 40 | // clang-format off 41 | bool m_isSkinnedMeshUpdateEnabled{ true }; //!< If set to true, all skinned meshes will be updated. Otherwise they will remain unchanged. 42 | // clang-format on 43 | }; 44 | } // namespace RGL 45 | -------------------------------------------------------------------------------- /Code/Platform/Common/Clang/rgl_static_clang.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | set(LY_COMPILE_OPTIONS 15 | PRIVATE 16 | -frtti 17 | ) 18 | -------------------------------------------------------------------------------- /Code/Platform/Common/GCC/rgl_static_gcc.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | set(LY_COMPILE_OPTIONS 15 | PRIVATE 16 | -frtti 17 | ) 18 | -------------------------------------------------------------------------------- /Code/Platform/Common/msvc/rgl_static_msvc.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | set(LY_COMPILE_OPTIONS 15 | PRIVATE 16 | /EHsc 17 | ) 18 | -------------------------------------------------------------------------------- /Code/Source/Entity/ActorEntityManager.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace RGL 32 | { 33 | ActorEntityManager::ActorEntityManager(AZ::EntityId entityId) 34 | : MaterialEntityManager(entityId) 35 | { 36 | AZ::EntityBus::Handler::BusConnect(m_entityId); 37 | EMotionFX::Integration::ActorComponentNotificationBus::Handler::BusConnect(entityId); 38 | } 39 | 40 | ActorEntityManager::~ActorEntityManager() 41 | { 42 | AZ::Render::MaterialComponentNotificationBus::Handler::BusDisconnect(); 43 | EMotionFX::Integration::ActorComponentNotificationBus::Handler::BusDisconnect(); 44 | AZ::EntityBus::Handler::BusDisconnect(); 45 | } 46 | 47 | void ActorEntityManager::Update() 48 | { 49 | if (!m_entities.empty() && RGLInterface::Get()->GetSceneConfiguration().m_isSkinnedMeshUpdateEnabled) 50 | { 51 | UpdateMeshVertices(); 52 | } 53 | EntityManager::Update(); 54 | } 55 | 56 | void ActorEntityManager::OnActorInstanceCreated(EMotionFX::ActorInstance* actorInstance) 57 | { 58 | m_actorInstance = actorInstance; 59 | EMotionFX::Actor* actor = actorInstance->GetActor(); 60 | [[maybe_unused]] const AZ::Entity* actorEntity = actorInstance->GetEntity(); 61 | 62 | static constexpr size_t LodLevel = 0U; // Highest LOD. 63 | static constexpr size_t NodeIdx = 0U; // Default mesh node. 64 | EMotionFX::Mesh* mesh = actor->GetMesh(LodLevel, NodeIdx); 65 | if (!mesh) 66 | { 67 | AZ_Assert(false, "No mesh found at joint 0. Unable to process the actor instance."); 68 | return; 69 | } 70 | 71 | if (ProcessEfxMesh(*mesh)) 72 | { 73 | m_emotionFxMesh = mesh; 74 | UpdateMaterialSlots(*actor); 75 | m_isPoseUpdateNeeded = true; 76 | } 77 | } 78 | 79 | void ActorEntityManager::OnActorInstanceDestroyed([[maybe_unused]] EMotionFX::ActorInstance* actorInstance) 80 | { 81 | AZ::Render::MaterialComponentNotificationBus::Handler::BusDisconnect(); 82 | ClearActorData(); 83 | } 84 | 85 | void ActorEntityManager::OnEntityDeactivated(const AZ::EntityId& entityId) 86 | { 87 | AZ::Render::MaterialComponentNotificationBus::Handler::BusDisconnect(); 88 | ClearActorData(); 89 | EMotionFX::Integration::ActorComponentNotificationBus::Handler::BusDisconnect(); 90 | MaterialEntityManager::OnEntityDeactivated(entityId); 91 | } 92 | 93 | AZStd::vector ActorEntityManager::CollectIndexData(const EMotionFX::Mesh& mesh) 94 | { 95 | const size_t indexCount = mesh.GetNumIndices(); 96 | const size_t triangleCount = indexCount / 3LU; 97 | 98 | uint32* indices = mesh.GetIndices(); 99 | AZStd::vector rglIndices; 100 | rglIndices.reserve(triangleCount); 101 | 102 | const size_t subMeshCount = mesh.GetNumSubMeshes(); 103 | for (size_t subMeshNr = 0; subMeshNr < subMeshCount; ++subMeshNr) 104 | { 105 | EMotionFX::SubMesh* subMesh = mesh.GetSubMesh(subMeshNr); 106 | const size_t subMeshIndexCount = subMesh->GetNumIndices(); 107 | const size_t subMeshTriangleCount = subMeshIndexCount / 3LU; 108 | 109 | const size_t indexBase = subMesh->GetStartIndex(); 110 | for (size_t triangle = 0LU; triangle < subMeshTriangleCount; ++triangle) 111 | { 112 | const size_t triangleFirstIndex = triangle * 3LU + indexBase; 113 | 114 | // Note: We cast the uint32 to int32_t but this conversion is unlikely to result in negative values. 115 | rglIndices.push_back({ 116 | aznumeric_cast(indices[triangleFirstIndex]), 117 | aznumeric_cast(indices[triangleFirstIndex + 1LU]), 118 | aznumeric_cast(indices[triangleFirstIndex + 2LU]), 119 | }); 120 | } 121 | } 122 | 123 | return rglIndices; 124 | } 125 | 126 | AZStd::vector ActorEntityManager::GetMeshVertexPositions(const EMotionFX::Mesh& mesh) 127 | { 128 | const size_t vertexCount = mesh.GetNumVertices(); 129 | const auto* vertices = static_cast(mesh.FindVertexData(EMotionFX::Mesh::ATTRIB_POSITIONS)); 130 | const auto vertexSpan = AZStd::span(vertices, vertexCount); 131 | 132 | AZStd::vector rglVertices(vertexCount); 133 | AZStd::transform(vertexSpan.begin(), vertexSpan.end(), rglVertices.begin(), Utils::RglVector3FromAzVec3f); 134 | return rglVertices; 135 | } 136 | 137 | AZStd::optional> ActorEntityManager::CollectUvData(const EMotionFX::Mesh& mesh) const 138 | { 139 | const size_t vertexCount = mesh.GetNumVertices(); 140 | AZStd::vector rglUvs(vertexCount); 141 | 142 | const auto* uvs = static_cast(mesh.FindVertexData(EMotionFX::Mesh::ATTRIB_UVCOORDS)); 143 | if (uvs == nullptr) 144 | { 145 | AZ_Warning( 146 | __func__, false, "Unable to collect UV data from an actor component of entity with ID: %s.", m_entityId.ToString().c_str()); 147 | 148 | return AZStd::nullopt; 149 | } 150 | 151 | auto uvSpan = AZStd::span(uvs, vertexCount); 152 | AZStd::transform(uvSpan.begin(), uvSpan.end(), rglUvs.begin(), Utils::RglVec2fFromAzVector2); 153 | return rglUvs; 154 | } 155 | 156 | void ActorEntityManager::UpdateMaterialSlots(const EMotionFX::Actor& actor) 157 | { 158 | const AZ::Data::Asset& modelAsset = actor.GetMeshAsset(); 159 | const auto lodAssets = modelAsset->GetLodAssets(); 160 | // Get Highest LOD 161 | const auto modelLodAsset = lodAssets.begin()->Get(); 162 | const auto meshes = modelLodAsset->GetMeshes(); 163 | 164 | // Each mesh of the model is associated with one EFX sub mesh. 165 | for (size_t subMeshIdx = 0; subMeshIdx < meshes.size(); ++subMeshIdx) 166 | { 167 | const AZ::RPI::ModelMaterialSlot& slot = modelAsset->FindMaterialSlot(meshes[subMeshIdx].GetMaterialSlotId()); 168 | AssignMaterialSlotIdForMesh(slot.m_stableId, subMeshIdx); 169 | } 170 | 171 | // We can use material info only when the model is ready. 172 | AZ::Render::MaterialComponentNotificationBus::Handler::BusConnect(m_entityId); 173 | } 174 | 175 | void ActorEntityManager::UpdateMeshVertices() 176 | { 177 | if (!m_emotionFxMesh || !m_actorInstance) 178 | { 179 | return; 180 | } 181 | 182 | m_actorInstance->UpdateMeshDeformers(0.0f); 183 | 184 | const auto vertexPositions = GetMeshVertexPositions(*m_emotionFxMesh); 185 | const size_t subMeshCount = m_emotionFxMesh->GetNumSubMeshes(); 186 | for (size_t subMeshNr = 0; subMeshNr < subMeshCount; ++subMeshNr) 187 | { 188 | const EMotionFX::SubMesh* subMesh = m_emotionFxMesh->GetSubMesh(subMeshNr); 189 | const size_t vertexBase = subMesh->GetStartVertex(); 190 | const size_t subMeshVertexCount = subMesh->GetNumVertices(); 191 | 192 | m_entities[subMeshNr].ApplyExternalAnimation(vertexPositions.data() + vertexBase, subMeshVertexCount); 193 | } 194 | } 195 | 196 | bool ActorEntityManager::ProcessEfxMesh(const EMotionFX::Mesh& mesh) 197 | { 198 | const auto vertexPositions = GetMeshVertexPositions(mesh); 199 | 200 | const size_t subMeshCount = mesh.GetNumSubMeshes(); 201 | const auto indices = CollectIndexData(mesh); 202 | const auto uvData = CollectUvData(mesh); 203 | for (size_t subMeshNr = 0; subMeshNr < subMeshCount; ++subMeshNr) 204 | { 205 | const EMotionFX::SubMesh* subMesh = mesh.GetSubMesh(subMeshNr); 206 | const size_t vertexBase = subMesh->GetStartVertex(); 207 | const size_t vertexCount = subMesh->GetNumVertices(); 208 | 209 | const size_t indexBase = subMesh->GetStartIndex() / 3U; // RGL uses index triples. 210 | const size_t indexCount = subMesh->GetNumIndices() / 3U; 211 | 212 | auto rglMesh = Wrappers::RglMesh(vertexPositions.data() + vertexBase, vertexCount, indices.data() + indexBase, indexCount); 213 | 214 | if (rglMesh.IsValid()) 215 | { 216 | if (uvData.has_value()) 217 | { 218 | rglMesh.SetTextureCoordinates(uvData->data() + vertexBase, vertexCount); 219 | } 220 | 221 | m_rglSubMeshes.push_back(AZStd::move(rglMesh)); 222 | } 223 | else 224 | { 225 | m_rglSubMeshes.clear(); 226 | AZ_Error( 227 | "RGL", 228 | false, 229 | "[Entity: %s] Rgl mesh creation failed for one of the sub meshes of an entity with an actor component. Some geometry " 230 | "will not be present.", 231 | m_entityId.ToString().c_str()); 232 | return false; 233 | } 234 | } 235 | 236 | m_entities.reserve(m_rglSubMeshes.size()); 237 | for (size_t i = 0; i < m_rglSubMeshes.size(); ++i) 238 | { 239 | if (Wrappers::RglEntity subMeshEntity(m_rglSubMeshes[i]); subMeshEntity.IsValid()) 240 | { 241 | if (m_packedRglEntityId.has_value()) 242 | { 243 | subMeshEntity.SetId(m_packedRglEntityId.value()); 244 | } 245 | 246 | m_entities.emplace_back(AZStd::move(subMeshEntity)); 247 | } 248 | else 249 | { 250 | m_rglSubMeshes.clear(); 251 | m_entities.clear(); 252 | AZ_Error( 253 | "RGL", 254 | false, 255 | "[Entity: %s] Rgl entity creation failed for one of the sub meshes of an entity with an actor component. Some geometry " 256 | "will not be present.", 257 | m_entityId.ToString().c_str()); 258 | return false; 259 | } 260 | } 261 | 262 | return true; 263 | } 264 | 265 | void ActorEntityManager::ClearActorData() 266 | { 267 | ResetMaterialsMapping(); 268 | m_entities.clear(); 269 | m_rglSubMeshes.clear(); 270 | m_emotionFxMesh = nullptr; 271 | m_actorInstance = nullptr; 272 | } 273 | } // namespace RGL 274 | -------------------------------------------------------------------------------- /Code/Source/Entity/ActorEntityManager.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace EMotionFX 25 | { 26 | class Mesh; 27 | class ActorInstance; 28 | } // namespace EMotionFX 29 | 30 | namespace RGL 31 | { 32 | class ActorEntityManager 33 | : public MaterialEntityManager 34 | , public EMotionFX::Integration::ActorComponentNotificationBus::Handler 35 | { 36 | public: 37 | explicit ActorEntityManager(AZ::EntityId entityId); 38 | ActorEntityManager(const ActorEntityManager& other) = delete; 39 | ActorEntityManager(ActorEntityManager&& other) = delete; 40 | ActorEntityManager& operator=(ActorEntityManager&& rhs) = delete; 41 | ActorEntityManager& operator=(const ActorEntityManager&) = delete; 42 | ~ActorEntityManager(); 43 | 44 | void Update() override; 45 | 46 | protected: 47 | // ActorComponentNotificationBus overrides 48 | void OnActorInstanceCreated(EMotionFX::ActorInstance* actorInstance) override; 49 | void OnActorInstanceDestroyed(EMotionFX::ActorInstance* actorInstance) override; 50 | 51 | // AZ::EntityBus::Handler implementation overrides 52 | void OnEntityDeactivated(const AZ::EntityId& entityId) override; 53 | 54 | private: 55 | static AZStd::vector CollectIndexData(const EMotionFX::Mesh& mesh); 56 | static AZStd::vector GetMeshVertexPositions(const EMotionFX::Mesh& mesh); 57 | AZStd::optional> CollectUvData(const EMotionFX::Mesh& mesh) const; 58 | 59 | void UpdateMaterialSlots(const EMotionFX::Actor& actor); 60 | void UpdateMeshVertices(); 61 | //! Loads mesh's vertex position data into the m_tempVertexPositions buffer. 62 | bool ProcessEfxMesh(const EMotionFX::Mesh& mesh); 63 | void ClearActorData(); 64 | 65 | EMotionFX::ActorInstance* m_actorInstance = nullptr; 66 | // We do not use the ModelLibrary since the actor mesh is 67 | // skinned and the mesh sharing would not be useful. 68 | EMotionFX::Mesh* m_emotionFxMesh; 69 | AZStd::vector m_rglSubMeshes; 70 | }; 71 | } // namespace RGL 72 | -------------------------------------------------------------------------------- /Code/Source/Entity/EntityManager.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace RGL 23 | { 24 | EntityManager::EntityManager(AZ::EntityId entityId) 25 | : m_entityId{ entityId } 26 | , m_segmentationEntityId{ Utils::GenerateSegmentationEntityId() } 27 | { 28 | } 29 | 30 | EntityManager::~EntityManager() 31 | { 32 | AZ::EntityBus::Handler::BusDisconnect(); 33 | } 34 | 35 | void EntityManager::Update() 36 | { 37 | if (!m_isPoseUpdateNeeded) 38 | { 39 | return; 40 | } 41 | 42 | UpdatePose(); 43 | } 44 | 45 | void EntityManager::OnEntityActivated(const AZ::EntityId& entityId) 46 | { 47 | //// Transform 48 | // Register transform changed event handler 49 | AZ::TransformBus::Event(entityId, &AZ::TransformBus::Events::BindTransformChangedEventHandler, m_transformChangedHandler); 50 | // Get current transform 51 | AZ::TransformBus::EventResult(m_worldTm, entityId, &AZ::TransformBus::Events::GetWorldTM); 52 | 53 | //// Non-uniform scale 54 | // Register non-uniform scale changed event handler 55 | AZ::NonUniformScaleRequestBus::Event( 56 | entityId, &AZ::NonUniformScaleRequests::RegisterScaleChangedEvent, m_nonUniformScaleChangedHandler); 57 | // Get current non-uniform scale (if there is no non-uniform scale added, the value won't be changed (nullopt)) 58 | AZ::NonUniformScaleRequestBus::EventResult(m_nonUniformScale, entityId, &AZ::NonUniformScaleRequests::GetScale); 59 | 60 | m_isPoseUpdateNeeded = true; 61 | SetPackedRglEntityId(); 62 | } 63 | 64 | void EntityManager::OnEntityDeactivated(const AZ::EntityId& entityId) 65 | { 66 | m_transformChangedHandler.Disconnect(); 67 | m_nonUniformScaleChangedHandler.Disconnect(); 68 | } 69 | 70 | void EntityManager::UpdatePose() 71 | { 72 | if (m_entities.empty()) 73 | { 74 | m_isPoseUpdateNeeded = false; 75 | return; 76 | } 77 | 78 | AZ::Matrix3x4 transform3x4f = AZ::Matrix3x4::CreateFromTransform(m_worldTm); 79 | if (m_nonUniformScale.has_value()) 80 | { 81 | transform3x4f *= AZ::Matrix3x4::CreateScale(m_nonUniformScale.value()); 82 | } 83 | 84 | const rgl_mat3x4f entityPoseRgl = Utils::RglMat3x4FromAzMatrix3x4(transform3x4f); 85 | for (Wrappers::RglEntity& entity : m_entities) 86 | { 87 | entity.SetTransform(entityPoseRgl); 88 | } 89 | 90 | m_isPoseUpdateNeeded = false; 91 | } 92 | 93 | void EntityManager::SetPackedRglEntityId() 94 | { 95 | m_packedRglEntityId = CalculatePackedRglEntityId(); 96 | for (Wrappers::RglEntity& entity : m_entities) 97 | { 98 | entity.SetId(m_packedRglEntityId.value()); 99 | } 100 | } 101 | 102 | int32_t EntityManager::CalculatePackedRglEntityId() const 103 | { 104 | return Utils::PackRglEntityId( 105 | ROS2::SegmentationIds{ m_segmentationEntityId, ROS2::SegmentationUtils::FetchClassIdForEntity(m_entityId) }); 106 | } 107 | } // namespace RGL 108 | -------------------------------------------------------------------------------- /Code/Source/Entity/EntityManager.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace RGL 28 | { 29 | //! Base class for Entity Manager. 30 | //! Although it already implements the EntityBus handler, 31 | //! the derived classes have to handle bus connection 32 | //! through BusConnect and BusDisconnect function calls. 33 | //! This is to allow for further overrides. 34 | class EntityManager 35 | : public AZ::EntityBus::Handler 36 | { 37 | public: 38 | explicit EntityManager(AZ::EntityId entityId); 39 | EntityManager(const EntityManager& other) = delete; 40 | EntityManager(EntityManager&& other) = delete; 41 | EntityManager& operator=(EntityManager&& rhs) = delete; 42 | EntityManager& operator=(const EntityManager&) = delete; 43 | virtual ~EntityManager(); 44 | 45 | virtual void Update(); 46 | 47 | protected: 48 | // AZ::EntityBus::Handler implementation overrides 49 | void OnEntityActivated(const AZ::EntityId& entityId) override; 50 | void OnEntityDeactivated(const AZ::EntityId& entityId) override; 51 | 52 | //! Updates poses of all RGL entities managed by this EntityManager. 53 | virtual void UpdatePose(); 54 | 55 | AZ::EntityId m_entityId; 56 | AZStd::vector m_entities; 57 | AZStd::optional m_packedRglEntityId; 58 | bool m_isPoseUpdateNeeded{ false }; 59 | 60 | private: 61 | void SetPackedRglEntityId(); 62 | int32_t CalculatePackedRglEntityId() const; 63 | 64 | // clang-format off 65 | AZ::TransformChangedEvent::Handler m_transformChangedHandler{[this]( 66 | [[maybe_unused]] const AZ::Transform& local, const AZ::Transform& world) 67 | { 68 | m_worldTm = world; 69 | m_isPoseUpdateNeeded = true; 70 | }}; 71 | 72 | AZ::NonUniformScaleChangedEvent::Handler m_nonUniformScaleChangedHandler{[this]( 73 | const AZ::Vector3& scale) 74 | { 75 | m_nonUniformScale = scale; 76 | m_isPoseUpdateNeeded = true; 77 | }}; 78 | // clang-format on 79 | 80 | AZ::Transform m_worldTm{ AZ::Transform::CreateIdentity() }; 81 | AZStd::optional m_nonUniformScale{ AZStd::nullopt }; 82 | int32_t m_segmentationEntityId{ 0 }; 83 | }; 84 | } // namespace RGL 85 | -------------------------------------------------------------------------------- /Code/Source/Entity/MaterialEntityManager.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2024, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace RGL 21 | { 22 | MaterialEntityManager::MaterialEntityManager(AZ::EntityId entityId) 23 | : EntityManager(entityId) 24 | { 25 | } 26 | 27 | void MaterialEntityManager::OnMaterialsUpdated(const AZ::Render::MaterialAssignmentMap& materials) 28 | { 29 | if (m_entities.empty()) 30 | { 31 | AZ_Warning(__func__, false, "Skipping material update. The entities were not yet created."); 32 | return; 33 | } 34 | 35 | for (const auto& [assignmentId, assignment] : materials) 36 | { 37 | const Wrappers::RglTexture& materialTexture = ModelLibraryInterface::Get()->StoreMaterialAsset(assignment.m_materialAsset); 38 | if (materialTexture.IsValid()) 39 | { 40 | size_t meshEntityIdx = GetMeshEntityIdxForMaterialSlotId(assignmentId.m_materialSlotStableId); 41 | if (meshEntityIdx == -1) 42 | { 43 | continue; 44 | } 45 | 46 | m_entities[meshEntityIdx].SetIntensityTexture(materialTexture); 47 | } 48 | } 49 | } 50 | 51 | void MaterialEntityManager::AssignMaterialSlotIdForMesh(AZ::RPI::ModelMaterialSlot::StableId materialSlotId, size_t meshEntityIdx) 52 | { 53 | m_materialSlotMeshIdMap.emplace(materialSlotId, meshEntityIdx); 54 | } 55 | 56 | void MaterialEntityManager::ResetMaterialsMapping() 57 | { 58 | m_materialSlotMeshIdMap.clear(); 59 | } 60 | 61 | size_t MaterialEntityManager::GetMeshEntityIdxForMaterialSlotId(AZ::RPI::ModelMaterialSlot::StableId materialSlotId) const 62 | { 63 | auto it = m_materialSlotMeshIdMap.find(materialSlotId); 64 | if (it == m_materialSlotMeshIdMap.end()) 65 | { 66 | AZ_Error(__func__, false, "Programmer error: Unable to find mesh entity associated with provided material slot id."); 67 | return -1; 68 | } 69 | 70 | return it->second; 71 | } 72 | } // namespace RGL 73 | -------------------------------------------------------------------------------- /Code/Source/Entity/MaterialEntityManager.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2024, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | namespace RGL 22 | { 23 | //! Base class used for managing RGL's representation of an Entity with a Material component. 24 | //! Although it already implements the EntityBus and MaterialComponentNotificationBus handlers, 25 | //! the derived classes have to handle bus connections 26 | //! through BusConnect and BusDisconnect function calls. 27 | //! This is to allow for further overrides. 28 | class MaterialEntityManager 29 | : public EntityManager 30 | , protected AZ::Render::MaterialComponentNotificationBus::Handler 31 | { 32 | public: 33 | explicit MaterialEntityManager(AZ::EntityId entityId); 34 | MaterialEntityManager(const MaterialEntityManager& other) = delete; 35 | MaterialEntityManager(MaterialEntityManager&& other) = delete; 36 | MaterialEntityManager& operator=(MaterialEntityManager&& rhs) = delete; 37 | MaterialEntityManager& operator=(const MaterialEntityManager&) = delete; 38 | ~MaterialEntityManager() = default; 39 | 40 | protected: 41 | size_t GetMeshEntityIdxForMaterialSlotId(AZ::RPI::ModelMaterialSlot::StableId materialSlotId) const; 42 | 43 | void AssignMaterialSlotIdForMesh(AZ::RPI::ModelMaterialSlot::StableId materialSlotId, size_t meshEntityIdx); 44 | void ResetMaterialsMapping(); 45 | 46 | private: 47 | // AZ::Render::MaterialComponentNotificationBus implementation overrides 48 | void OnMaterialsUpdated(const AZ::Render::MaterialAssignmentMap& materials) override; 49 | 50 | AZStd::unordered_map m_materialSlotMeshIdMap; 51 | }; 52 | } // namespace RGL 53 | -------------------------------------------------------------------------------- /Code/Source/Entity/MeshEntityManager.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace RGL 25 | { 26 | MeshEntityManager::MeshEntityManager(AZ::EntityId entityId) 27 | : MaterialEntityManager{ entityId } 28 | { 29 | AZ::EntityBus::Handler::BusConnect(m_entityId); 30 | } 31 | 32 | MeshEntityManager::~MeshEntityManager() 33 | { 34 | AZ::Render::MaterialComponentNotificationBus::Handler::BusDisconnect(); 35 | AZ::Render::MeshComponentNotificationBus::Handler::BusDisconnect(); 36 | AZ::EntityBus::Handler::BusDisconnect(); 37 | } 38 | 39 | void MeshEntityManager::OnEntityActivated(const AZ::EntityId& entityId) 40 | { 41 | MaterialEntityManager::OnEntityActivated(entityId); 42 | AZ::Render::MeshComponentNotificationBus::Handler::BusConnect(entityId); 43 | } 44 | 45 | void MeshEntityManager::OnEntityDeactivated(const AZ::EntityId& entityId) 46 | { 47 | AZ::Render::MaterialComponentNotificationBus::Handler::BusDisconnect(); 48 | AZ::Render::MeshComponentNotificationBus::Handler::BusDisconnect(); 49 | MaterialEntityManager::OnEntityDeactivated(entityId); 50 | } 51 | 52 | void MeshEntityManager::OnModelReady( 53 | const AZ::Data::Asset& modelAsset, [[maybe_unused]] const AZ::Data::Instance& model) 54 | { 55 | AZ_Assert(m_entities.empty(), "Entity Manager for entity with ID: %s has an invalid state.", m_entityId.ToString().c_str()); 56 | auto* modelLibrary = ModelLibraryInterface::Get(); 57 | const MeshMaterialSlotPairList& meshes = modelLibrary->StoreModelAsset(modelAsset); 58 | 59 | if (meshes.empty()) 60 | { 61 | AZ_Assert( 62 | false, "MeshEntityManager with ID: %s did not receive any mesh from the ModelLibrary.", m_entityId.ToString().c_str()); 63 | return; 64 | } 65 | 66 | m_entities.reserve(meshes.size()); 67 | size_t entityIdx = 0; 68 | for (const auto& [mesh, matSlot] : meshes) 69 | { 70 | Wrappers::RglEntity entity(mesh); 71 | if (entity.IsValid()) 72 | { 73 | AssignMaterialSlotIdForMesh(matSlot.m_stableId, entityIdx); 74 | const Wrappers::RglTexture& texture = modelLibrary->StoreMaterialAsset(matSlot.m_defaultMaterialAsset); 75 | if (texture.IsValid()) 76 | { 77 | entity.SetIntensityTexture(texture); 78 | } 79 | 80 | if (m_packedRglEntityId.has_value()) 81 | { 82 | entity.SetId(m_packedRglEntityId.value()); 83 | } 84 | 85 | m_entities.emplace_back(AZStd::move(entity)); 86 | ++entityIdx; 87 | } 88 | } 89 | 90 | m_isPoseUpdateNeeded = true; 91 | 92 | // We can use material info only when the model is ready. 93 | AZ::Render::MaterialComponentNotificationBus::Handler::BusConnect(m_entityId); 94 | } 95 | 96 | void MeshEntityManager::OnModelPreDestroy() 97 | { 98 | AZ::Render::MaterialComponentNotificationBus::Handler::BusDisconnect(); 99 | ResetMaterialsMapping(); 100 | m_entities.clear(); 101 | } 102 | } // namespace RGL 103 | -------------------------------------------------------------------------------- /Code/Source/Entity/MeshEntityManager.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | namespace RGL 21 | { 22 | //! Class used for managing RGL's representation of an Entity with a MeshComponent. 23 | class MeshEntityManager 24 | : public MaterialEntityManager 25 | , protected AZ::Render::MeshComponentNotificationBus::Handler 26 | { 27 | public: 28 | explicit MeshEntityManager(AZ::EntityId entityId); 29 | MeshEntityManager(const MeshEntityManager& other) = delete; 30 | MeshEntityManager(MeshEntityManager&& other) = delete; 31 | MeshEntityManager& operator=(MeshEntityManager&& rhs) = delete; 32 | MeshEntityManager& operator=(const MeshEntityManager&) = delete; 33 | ~MeshEntityManager(); 34 | 35 | protected: 36 | // AZ::EntityBus::Handler implementation overrides 37 | void OnEntityActivated(const AZ::EntityId& entityId) override; 38 | void OnEntityDeactivated(const AZ::EntityId& entityId) override; 39 | 40 | // AZ::Render::MeshComponentNotificationBus overrides 41 | void OnModelReady( 42 | const AZ::Data::Asset& modelAsset, 43 | [[maybe_unused]] const AZ::Data::Instance& model) override; 44 | void OnModelPreDestroy() override; 45 | }; 46 | } // namespace RGL -------------------------------------------------------------------------------- /Code/Source/Entity/Terrain/TerrainData.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace RGL 21 | { 22 | bool TerrainData::UpdateBounds(const AZ::Aabb& newWorldBounds) 23 | { 24 | if (newWorldBounds == m_currentWorldBounds) 25 | { 26 | // This is only for world bound creation, not for update. 27 | return false; 28 | } 29 | 30 | size_t heightfieldGridColumns{}, heightfieldGridRows{}; 31 | Physics::HeightfieldProviderRequestsBus::BroadcastResult( 32 | heightfieldGridColumns, &Physics::HeightfieldProviderRequests::GetHeightfieldGridColumns); 33 | Physics::HeightfieldProviderRequestsBus::BroadcastResult( 34 | heightfieldGridRows, &Physics::HeightfieldProviderRequests::GetHeightfieldGridRows); 35 | 36 | if (heightfieldGridColumns < 2 || heightfieldGridRows < 2) 37 | { 38 | return false; 39 | } 40 | 41 | AZ::Vector2 heightfieldGridSpacing{}; 42 | Physics::HeightfieldProviderRequestsBus::BroadcastResult( 43 | heightfieldGridSpacing, &Physics::HeightfieldProviderRequests::GetHeightfieldGridSpacing); 44 | 45 | m_gridColumns = heightfieldGridColumns; 46 | m_gridRows = heightfieldGridRows; 47 | 48 | 49 | m_currentWorldBounds = newWorldBounds; 50 | 51 | m_vertices.clear(); 52 | 53 | const size_t vertexCount = m_gridColumns * m_gridRows; 54 | m_vertices.reserve(vertexCount); 55 | 56 | const AZ::Vector3 worldMin = newWorldBounds.GetMin(); 57 | const AZ::Vector2 constrictedAlignedStartPoint = (AZ::Vector2(worldMin) / heightfieldGridSpacing).GetCeil() * heightfieldGridSpacing; 58 | 59 | for (size_t vertexIndexX = 0LU; vertexIndexX < m_gridColumns; ++vertexIndexX) 60 | { 61 | for (size_t vertexIndexY = 0LU; vertexIndexY < m_gridRows; ++vertexIndexY) 62 | { 63 | m_vertices.emplace_back(rgl_vec3f{ 64 | constrictedAlignedStartPoint.GetX() + aznumeric_cast(vertexIndexX) * heightfieldGridSpacing.GetX(), 65 | constrictedAlignedStartPoint.GetY() + aznumeric_cast(vertexIndexY) * heightfieldGridSpacing.GetY(), 66 | 0.0f, 67 | }); 68 | } 69 | } 70 | 71 | UpdateUvs(); 72 | 73 | m_indices.clear(); 74 | m_indices.reserve((m_gridColumns - 1) * (m_gridRows - 1) * TrianglesPerSector); 75 | for (size_t sectorIndexX = 0LU; sectorIndexX < m_gridColumns - 1; ++sectorIndexX) 76 | { 77 | for (size_t sectorIndexY = 0LU; sectorIndexY < m_gridRows - 1; ++sectorIndexY) 78 | { 79 | const auto lowerLeft = aznumeric_cast(sectorIndexY + sectorIndexX * m_gridRows); 80 | const auto lowerRight = aznumeric_cast(lowerLeft + m_gridRows); 81 | const auto upperLeft = aznumeric_cast(lowerLeft + 1); 82 | const auto upperRight = aznumeric_cast(lowerRight + 1); 83 | 84 | m_indices.emplace_back(rgl_vec3i{ upperLeft, lowerRight, upperRight }); 85 | m_indices.emplace_back(rgl_vec3i{ upperLeft, lowerLeft, lowerRight }); 86 | } 87 | } 88 | 89 | return true; 90 | } 91 | 92 | void TerrainData::UpdateDirtyRegion(const AZ::Aabb& dirtyRegion) 93 | { 94 | const AZ::Aabb dirtyRegion2D = AZ::Aabb::CreateFromMinMaxValues( 95 | dirtyRegion.GetMin().GetX(), 96 | dirtyRegion.GetMin().GetY(), 97 | AZStd::numeric_limits::lowest(), 98 | dirtyRegion.GetMax().GetX(), 99 | dirtyRegion.GetMax().GetY(), 100 | AZStd::numeric_limits::max()); 101 | 102 | for (rgl_vec3f& vertex : m_vertices) 103 | { 104 | if (dirtyRegion2D.Contains(Utils::AzVector3FromRglVec3f(vertex))) 105 | { 106 | float height = AZStd::numeric_limits::lowest(); 107 | bool terrainExists = false; 108 | 109 | // Sampler::Exact is used because mesh vertices are created directly on grid provided from heightfield. 110 | AzFramework::Terrain::TerrainDataRequestBus::BroadcastResult( 111 | height, 112 | &AzFramework::Terrain::TerrainDataRequestBus::Events::GetHeightFromFloats, 113 | vertex.value[0], 114 | vertex.value[1], 115 | AzFramework::Terrain::TerrainDataRequests::Sampler::EXACT, 116 | &terrainExists); 117 | 118 | if (height != AZStd::numeric_limits::lowest() && terrainExists) 119 | { 120 | vertex.value[2] = height; 121 | } 122 | } 123 | } 124 | } 125 | 126 | void TerrainData::Clear() 127 | { 128 | m_currentWorldBounds = AZ::Aabb::CreateFromPoint(AZ::Vector3::CreateZero()); 129 | m_gridColumns = m_gridRows = 0U; 130 | m_vertices.clear(); 131 | m_indices.clear(); 132 | m_uvs.clear(); 133 | } 134 | 135 | void TerrainData::SetIsTiled(bool isTiled) 136 | { 137 | if (m_isTiled == isTiled) 138 | { 139 | return; 140 | } 141 | 142 | m_isTiled = isTiled; 143 | UpdateUvs(); 144 | } 145 | 146 | const AZStd::vector& TerrainData::GetVertices() const 147 | { 148 | return m_vertices; 149 | } 150 | 151 | const AZStd::vector& TerrainData::GetIndices() const 152 | { 153 | return m_indices; 154 | } 155 | 156 | const AZStd::vector& TerrainData::GetUvs() const 157 | { 158 | return m_uvs; 159 | } 160 | 161 | void TerrainData::UpdateUvs() 162 | { 163 | m_uvs.clear(); 164 | m_uvs.reserve(m_gridColumns * m_gridRows); 165 | 166 | for (size_t x = 0; x < m_gridColumns; ++x) 167 | { 168 | for (size_t y = 0; y < m_gridRows; ++y) 169 | { 170 | auto uv = rgl_vec2f{ 171 | aznumeric_cast(x), 172 | aznumeric_cast(y), 173 | }; 174 | 175 | // Terrain Macro Material's image 176 | // is inverted on the v axis. 177 | if (!m_isTiled) 178 | { 179 | uv.value[0] /= aznumeric_cast(m_gridColumns); 180 | uv.value[1] /= aznumeric_cast(m_gridRows); 181 | uv.value[1] = 1.0f - uv.value[1]; 182 | } 183 | 184 | m_uvs.emplace_back(uv); 185 | } 186 | } 187 | } 188 | } // namespace RGL 189 | -------------------------------------------------------------------------------- /Code/Source/Entity/Terrain/TerrainData.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | namespace RGL 21 | { 22 | class TerrainData 23 | { 24 | public: 25 | //! Returns whether or not newWorldBounds resulted in terrain update. 26 | bool UpdateBounds(const AZ::Aabb& newWorldBounds); 27 | void UpdateDirtyRegion(const AZ::Aabb& dirtyRegion); 28 | 29 | void Clear(); 30 | void SetIsTiled(bool isTiled); 31 | 32 | [[nodiscard]] const AZStd::vector& GetVertices() const; 33 | [[nodiscard]] const AZStd::vector& GetIndices() const; 34 | [[nodiscard]] const AZStd::vector& GetUvs() const; 35 | 36 | private: 37 | void UpdateUvs(); 38 | 39 | static constexpr size_t TrianglesPerSector = 2LU; 40 | 41 | AZ::Aabb m_currentWorldBounds = { AZ::Aabb::CreateFromPoint(AZ::Vector3::CreateZero()) }; 42 | 43 | AZStd::vector m_vertices; 44 | AZStd::vector m_indices; 45 | AZStd::vector m_uvs; 46 | 47 | size_t m_gridRows{ 0U }, m_gridColumns{ 0U }; 48 | 49 | bool m_isTiled{ true }; 50 | }; 51 | 52 | } // namespace RGL 53 | -------------------------------------------------------------------------------- /Code/Source/Entity/Terrain/TerrainEntityManagerEditorSystemComponent.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #include 16 | #include 17 | 18 | namespace RGL 19 | { 20 | void TerrainEntityManagerEditorSystemComponent::Reflect(AZ::ReflectContext* context) 21 | { 22 | if (auto serializeContext = azrtti_cast(context)) 23 | { 24 | serializeContext->Class()->Version(0); 25 | } 26 | } 27 | 28 | void TerrainEntityManagerEditorSystemComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) 29 | { 30 | BaseSystemComponent::GetProvidedServices(provided); 31 | provided.push_back(AZ_CRC_CE("TerrainEntityManagerEditorService")); 32 | } 33 | 34 | void TerrainEntityManagerEditorSystemComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) 35 | { 36 | BaseSystemComponent::GetIncompatibleServices(incompatible); 37 | incompatible.push_back(AZ_CRC_CE("TerrainEntityManagerEditorService")); 38 | } 39 | 40 | void TerrainEntityManagerEditorSystemComponent::GetRequiredServices( 41 | [[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) 42 | { 43 | BaseSystemComponent::GetRequiredServices(required); 44 | } 45 | 46 | void TerrainEntityManagerEditorSystemComponent::Activate() 47 | { 48 | TerrainEntityManagerSystemComponent::Activate(); 49 | } 50 | 51 | void TerrainEntityManagerEditorSystemComponent::Deactivate() 52 | { 53 | TerrainEntityManagerSystemComponent::Deactivate(); 54 | } 55 | } // namespace RGL 56 | -------------------------------------------------------------------------------- /Code/Source/Entity/Terrain/TerrainEntityManagerEditorSystemComponent.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | 19 | namespace RGL 20 | { 21 | class TerrainEntityManagerEditorSystemComponent : public TerrainEntityManagerSystemComponent 22 | { 23 | using BaseSystemComponent = TerrainEntityManagerSystemComponent; 24 | 25 | public: 26 | AZ_COMPONENT( 27 | TerrainEntityManagerEditorSystemComponent, "{a5b7f4dc-82bc-48c7-ab57-c18bc7225886}", TerrainEntityManagerSystemComponent); 28 | static void Reflect(AZ::ReflectContext* context); 29 | 30 | static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); 31 | static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible); 32 | static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); 33 | 34 | TerrainEntityManagerEditorSystemComponent() = default; 35 | ~TerrainEntityManagerEditorSystemComponent() = default; 36 | 37 | private: 38 | void Activate() override; 39 | void Deactivate() override; 40 | }; 41 | } // namespace RGL 42 | -------------------------------------------------------------------------------- /Code/Source/Entity/Terrain/TerrainEntityManagerSystemComponent.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace RGL 23 | { 24 | void TerrainEntityManagerSystemComponent::OnTerrainDataCreateEnd() 25 | { 26 | UpdateWorldBounds(); 27 | } 28 | 29 | void TerrainEntityManagerSystemComponent::OnTerrainDataDestroyEnd() 30 | { 31 | m_terrainData.Clear(); 32 | EnsureRGLEntityDestroyed(); 33 | } 34 | 35 | void TerrainEntityManagerSystemComponent::Activate() 36 | { 37 | m_packedRglEntityId = Utils::PackRglEntityId(ROS2::SegmentationIds{ Utils::GenerateSegmentationEntityId(), ROS2::TerrainClassId }); 38 | AzFramework::Terrain::TerrainDataNotificationBus::Handler::BusConnect(); 39 | RGLNotificationBus::Handler::BusConnect(); 40 | } 41 | 42 | void TerrainEntityManagerSystemComponent::Deactivate() 43 | { 44 | AzFramework::Terrain::TerrainDataNotificationBus::Handler::BusDisconnect(); 45 | RGLNotificationBus::Handler::BusDisconnect(); 46 | m_terrainData.Clear(); 47 | EnsureRGLEntityDestroyed(); 48 | } 49 | 50 | Wrappers::RglTexture TerrainEntityManagerSystemComponent::CreateTextureFromConfig(const TerrainIntensityConfiguration& intensityConfig) 51 | { 52 | Wrappers::RglTexture terrainTexture = Wrappers::RglTexture::CreateInvalid(); 53 | 54 | if (intensityConfig.m_colorImageAsset.IsReady()) 55 | { 56 | terrainTexture = Wrappers::RglTexture::CreateFromImageAsset(intensityConfig.m_colorImageAsset.GetId()); 57 | if (terrainTexture.IsValid()) 58 | { 59 | return terrainTexture; 60 | } 61 | } 62 | 63 | return { &intensityConfig.m_defaultValue, 1U, 1U }; 64 | } 65 | 66 | void TerrainEntityManagerSystemComponent::OnSceneConfigurationSet(const SceneConfiguration& config) 67 | { 68 | const auto& intensityConfig = RGLInterface::Get()->GetSceneConfiguration().m_terrainIntensityConfig; 69 | m_rglTexture = AZStd::move(CreateTextureFromConfig(intensityConfig)); 70 | if (m_rglTexture.IsValid() && m_rglEntity.IsValid()) 71 | { 72 | m_rglEntity.SetIntensityTexture(m_rglTexture); 73 | } 74 | 75 | m_terrainData.SetIsTiled(intensityConfig.m_isTiled); 76 | } 77 | 78 | void TerrainEntityManagerSystemComponent::OnAnyLidarExists() 79 | { 80 | AzFramework::Terrain::TerrainDataNotificationBus::Handler::BusConnect(); 81 | } 82 | 83 | void TerrainEntityManagerSystemComponent::OnNoLidarExists() 84 | { 85 | AzFramework::Terrain::TerrainDataNotificationBus::Handler::BusDisconnect(); 86 | m_terrainData.Clear(); 87 | EnsureRGLEntityDestroyed(); 88 | } 89 | 90 | void TerrainEntityManagerSystemComponent::Reflect(AZ::ReflectContext* context) 91 | { 92 | if (auto serializeContext = azrtti_cast(context)) 93 | { 94 | serializeContext->Class()->Version(0); 95 | 96 | if (AZ::EditContext* editContext = serializeContext->GetEditContext()) 97 | { 98 | editContext->Class("Terrain Entity Manager", "Manages the RGL Terrain entity.") 99 | ->ClassElement(AZ::Edit::ClassElements::EditorData, "") 100 | ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("System")) 101 | ->Attribute(AZ::Edit::Attributes::Category, "RGL") 102 | ->Attribute(AZ::Edit::Attributes::AutoExpand, true); 103 | } 104 | } 105 | } 106 | 107 | void TerrainEntityManagerSystemComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) 108 | { 109 | provided.push_back(AZ_CRC_CE("TerrainEntityManagerService")); 110 | } 111 | 112 | void TerrainEntityManagerSystemComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) 113 | { 114 | incompatible.push_back(AZ_CRC_CE("TerrainEntityManagerService")); 115 | } 116 | 117 | void TerrainEntityManagerSystemComponent::GetRequiredServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) 118 | { 119 | required.push_back(AZ_CRC_CE("RGLService")); 120 | } 121 | 122 | void TerrainEntityManagerSystemComponent::EnsureRGLEntityDestroyed() 123 | { 124 | m_rglEntity = AZStd::move(Wrappers::RglEntity::CreateInvalid()); 125 | m_rglMesh = AZStd::move(Wrappers::RglMesh::CreateInvalid()); 126 | } 127 | 128 | void TerrainEntityManagerSystemComponent::UpdateWorldBounds() 129 | { 130 | AZ::Aabb newWorldBounds = AZ::Aabb::CreateFromPoint(AZ::Vector3::CreateZero()); 131 | AzFramework::Terrain::TerrainDataRequestBus::BroadcastResult( 132 | newWorldBounds, &AzFramework::Terrain::TerrainDataRequests::GetTerrainAabb); 133 | 134 | if (!m_terrainData.UpdateBounds(newWorldBounds)) 135 | { 136 | // The terrain data was not updated. No need to update the RGL mesh. 137 | return; 138 | } 139 | 140 | EnsureRGLEntityDestroyed(); 141 | 142 | const auto& vertices = m_terrainData.GetVertices(); 143 | const auto& indices = m_terrainData.GetIndices(); 144 | 145 | m_rglMesh = AZStd::move(Wrappers::RglMesh(vertices.data(), vertices.size(), indices.data(), indices.size())); 146 | if (!m_rglMesh.IsValid()) 147 | { 148 | AZ_Assert(false, "The TerrainEntityManager was unable to create an RGL mesh."); 149 | return; 150 | } 151 | 152 | const auto& uvs = m_terrainData.GetUvs(); 153 | m_rglMesh.SetTextureCoordinates(uvs.data(), uvs.size()); 154 | 155 | m_rglEntity = AZStd::move(Wrappers::RglEntity(m_rglMesh)); 156 | if (!m_rglEntity.IsValid()) 157 | { 158 | AZ_Assert(false, "The TerrainEntityManager was unable to create an RGL entity."); 159 | return; 160 | } 161 | 162 | m_rglEntity.SetId(m_packedRglEntityId); 163 | m_rglEntity.SetTransform(Utils::IdentityTransform); 164 | if (m_rglTexture.IsValid()) 165 | { 166 | m_rglEntity.SetIntensityTexture(m_rglTexture); 167 | } 168 | } 169 | 170 | void TerrainEntityManagerSystemComponent::UpdateDirtyRegion(const AZ::Aabb& dirtyRegion) 171 | { 172 | const auto& vertices = m_terrainData.GetVertices(); 173 | if (vertices.empty()) 174 | { 175 | // Dirty regions can only be updated after data creation and before destruction. (See TerrainDataRequests). 176 | return; 177 | } 178 | 179 | m_terrainData.UpdateDirtyRegion(dirtyRegion); 180 | m_rglEntity.ApplyExternalAnimation(vertices.data(), vertices.size()); 181 | } 182 | 183 | void TerrainEntityManagerSystemComponent::OnTerrainDataChanged(const AZ::Aabb& dirtyRegion, TerrainDataChangedMask dataChangedMask) 184 | { 185 | if ((dataChangedMask & TerrainDataChangedMask::Settings) != TerrainDataChangedMask::None) 186 | { 187 | UpdateWorldBounds(); 188 | } 189 | 190 | if ((dataChangedMask & TerrainDataChangedMask::HeightData) != TerrainDataChangedMask::None) 191 | { 192 | UpdateDirtyRegion(dirtyRegion); 193 | } 194 | } 195 | } // namespace RGL 196 | -------------------------------------------------------------------------------- /Code/Source/Entity/Terrain/TerrainEntityManagerSystemComponent.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace RGL 27 | { 28 | struct TerrainIntensityConfiguration; 29 | 30 | //! Queries the TerrainDataRequestBus for terrain heights and constructs a mesh using them. 31 | //! Terrain area is split into square sectors of predetermined width. 32 | //! The constructed mesh has a uniform vertex distribution along the xy - plane. 33 | class TerrainEntityManagerSystemComponent 34 | : public AZ::Component 35 | , private AzFramework::Terrain::TerrainDataNotificationBus::Handler 36 | , private RGLNotificationBus::Handler 37 | { 38 | public: 39 | AZ_COMPONENT(TerrainEntityManagerSystemComponent, "{6de4556f-5621-4ec3-a587-b28988f79d8a}"); 40 | static void Reflect(AZ::ReflectContext* context); 41 | 42 | static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); 43 | static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible); 44 | static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); 45 | 46 | TerrainEntityManagerSystemComponent() = default; 47 | 48 | // AzFramework::Terrain::TerrainDataNotificationBus overrides 49 | void OnTerrainDataCreateEnd() override; 50 | void OnTerrainDataDestroyEnd() override; 51 | void OnTerrainDataChanged(const AZ::Aabb& dirtyRegion, TerrainDataChangedMask dataChangedMask) override; 52 | 53 | protected: 54 | void Activate() override; 55 | void Deactivate() override; 56 | 57 | private: 58 | static Wrappers::RglTexture CreateTextureFromConfig(const TerrainIntensityConfiguration& intensityConfig); 59 | 60 | // RGLNotificationBus overrides 61 | void OnSceneConfigurationSet(const SceneConfiguration& config) override; 62 | void OnAnyLidarExists() override; 63 | void OnNoLidarExists() override; 64 | 65 | void EnsureRGLEntityDestroyed(); 66 | 67 | void UpdateWorldBounds(); 68 | void UpdateDirtyRegion(const AZ::Aabb& dirtyRegion); 69 | 70 | Wrappers::RglMesh m_rglMesh = Wrappers::RglMesh::CreateInvalid(); 71 | Wrappers::RglEntity m_rglEntity = Wrappers::RglEntity::CreateInvalid(); 72 | Wrappers::RglTexture m_rglTexture = Wrappers::RglTexture::CreateInvalid(); 73 | 74 | TerrainData m_terrainData; 75 | 76 | int32_t m_packedRglEntityId; 77 | }; 78 | } // namespace RGL 79 | -------------------------------------------------------------------------------- /Code/Source/Lidar/LidarRaycaster.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace RGL 23 | { 24 | LidarRaycaster::LidarRaycaster(const AZ::Uuid& uuid) 25 | : m_uuid{ uuid } 26 | { 27 | ROS2::LidarRaycasterRequestBus::Handler::BusConnect(ROS2::LidarId(uuid)); 28 | LidarSystemNotificationBus::Broadcast(&LidarSystemNotifications::OnLidarCreated); 29 | } 30 | 31 | LidarRaycaster::LidarRaycaster(LidarRaycaster&& other) 32 | : m_uuid{ other.m_uuid } 33 | , m_isMaxRangeEnabled{ other.m_isMaxRangeEnabled } 34 | , m_range{ other.m_range } 35 | , m_graph{ std::move(other.m_graph) } 36 | , m_rayTransforms{ AZStd::move(other.m_rayTransforms) } 37 | , m_rglRaycastResults{ AZStd::move(other.m_rglRaycastResults) } 38 | { 39 | other.BusDisconnect(); 40 | 41 | // Ensure proper destruction of the movee. 42 | other.m_uuid = AZ::Uuid::CreateNull(); 43 | ROS2::LidarRaycasterRequestBus::Handler::BusConnect(ROS2::LidarId(m_uuid)); 44 | } 45 | 46 | LidarRaycaster::~LidarRaycaster() 47 | { 48 | if (!m_uuid.IsNull()) 49 | { 50 | ROS2::LidarRaycasterRequestBus::Handler::BusDisconnect(); 51 | LidarSystemNotificationBus::Broadcast(&LidarSystemNotifications::OnLidarDestroyed); 52 | } 53 | } 54 | 55 | void LidarRaycaster::ConfigureRayOrientations(const AZStd::vector& orientations) 56 | { 57 | ValidateRayOrientations(orientations); 58 | AZStd::vector rglRayTransforms; 59 | rglRayTransforms.reserve(orientations.size()); 60 | for (const AZ::Vector3& orientation : orientations) 61 | { 62 | // Since we provide a transform for the Z axis unit vector we need an additional PI / 2 added to the pitch. 63 | const AZ::Matrix3x4 rayTransform = AZ::Matrix3x4::CreateFromQuaternion(AZ::Quaternion::CreateFromEulerRadiansZYX({ 64 | orientation.GetX(), 65 | -orientation.GetY() + AZ::Constants::HalfPi, 66 | orientation.GetZ(), 67 | })); 68 | 69 | rglRayTransforms.push_back(Utils::RglMat3x4FromAzMatrix3x4(rayTransform)); 70 | } 71 | 72 | m_rayTransforms.clear(); 73 | m_rayTransforms.reserve(rglRayTransforms.size()); 74 | for (rgl_mat3x4f transform : rglRayTransforms) 75 | { 76 | m_rayTransforms.push_back(Utils::AzMatrix3x4FromRglMat3x4(transform)); 77 | } 78 | 79 | m_graph.ConfigureRayPosesNode(rglRayTransforms); 80 | } 81 | 82 | void LidarRaycaster::ConfigureRayRange(ROS2::RayRange range) 83 | { 84 | m_range = range; 85 | 86 | UpdateNonHitValues(); 87 | 88 | m_graph.ConfigureRayRangesNode(range.m_min, range.m_max); 89 | } 90 | 91 | void LidarRaycaster::ConfigureRaycastResultFlags(ROS2::RaycastResultFlags flags) 92 | { 93 | m_rglRaycastResults.m_fields.clear(); 94 | m_rglRaycastResults.m_xyz.clear(); 95 | m_rglRaycastResults.m_distance.clear(); 96 | 97 | m_raycastResults = ROS2::RaycastResults(flags); 98 | 99 | if (ROS2::IsFlagEnabled(ROS2::RaycastResultFlags::Point, flags)) 100 | { 101 | m_rglRaycastResults.m_fields.push_back(RGL_FIELD_XYZ_VEC3_F32); 102 | } 103 | 104 | if (ROS2::IsFlagEnabled(ROS2::RaycastResultFlags::Range, flags)) 105 | { 106 | m_rglRaycastResults.m_fields.push_back(RGL_FIELD_DISTANCE_F32); 107 | } 108 | 109 | if (ROS2::IsFlagEnabled(ROS2::RaycastResultFlags::Intensity, flags)) 110 | { 111 | m_rglRaycastResults.m_fields.push_back(RGL_FIELD_INTENSITY_F32); 112 | } 113 | 114 | if (ROS2::IsFlagEnabled(ROS2::RaycastResultFlags::SegmentationData, flags)) 115 | { 116 | m_rglRaycastResults.m_fields.push_back(RGL_FIELD_ENTITY_ID_I32); 117 | } 118 | 119 | m_graph.ConfigureYieldNodes(m_rglRaycastResults.m_fields.data(), m_rglRaycastResults.m_fields.size()); 120 | 121 | m_graph.SetIsCompactEnabled(ShouldEnableCompact()); 122 | m_graph.SetIsPcPublishingEnabled(ShouldEnablePcPublishing()); 123 | } 124 | 125 | AZ::Outcome LidarRaycaster::PerformRaycast(const AZ::Transform& lidarTransform) 126 | { 127 | AZ_Assert(m_range.has_value(), "Programmer error. Raycaster range is not fully configured."); 128 | AZ_Assert(m_raycastResults.has_value(), "Programmer error. Raycaster result fields not fully configured."); 129 | RGLInterface::Get()->UpdateScene(); 130 | 131 | const AZ::Matrix3x4 lidarPose = AZ::Matrix3x4::CreateFromTransform(lidarTransform); 132 | 133 | m_graph.ConfigureLidarTransformNode(lidarPose); 134 | if (m_graph.IsPcPublishingEnabled()) 135 | { 136 | // Transforms the obtained point-cloud from world to sensor frame of reference. 137 | m_graph.ConfigurePcTransformNode(lidarPose.GetInverseFull()); 138 | } 139 | 140 | m_graph.Run(); 141 | 142 | if (!m_graph.GetResults(m_rglRaycastResults)) 143 | { 144 | return AZ::Failure("Results returned by RGL did not match requested."); 145 | } 146 | 147 | const auto resultSize = GetRglResultsSize(m_rglRaycastResults, m_raycastResults.value()); 148 | if (!resultSize.has_value()) 149 | { 150 | return AZ::Failure("Results were of different sizes."); 151 | } 152 | 153 | ROS2::RaycastResults& raycastResults = m_raycastResults.value(); 154 | raycastResults.Resize(resultSize.value()); 155 | 156 | if (auto points = raycastResults.GetFieldSpan(); points.has_value()) 157 | { 158 | AZStd::transform( 159 | m_rglRaycastResults.m_xyz.begin(), m_rglRaycastResults.m_xyz.end(), points.value().begin(), Utils::AzVector3FromRglVec3f); 160 | } 161 | 162 | if (auto distance = raycastResults.GetFieldSpan(); distance.has_value()) 163 | { 164 | AZStd::copy(m_rglRaycastResults.m_distance.begin(), m_rglRaycastResults.m_distance.end(), distance.value().begin()); 165 | } 166 | 167 | if (auto intensity = raycastResults.GetFieldSpan(); intensity.has_value()) 168 | { 169 | AZStd::copy(m_rglRaycastResults.m_intensity.begin(), m_rglRaycastResults.m_intensity.end(), intensity.value().begin()); 170 | } 171 | 172 | if (auto segmentationData = raycastResults.GetFieldSpan(); segmentationData.has_value()) 173 | { 174 | AZStd::transform( 175 | m_rglRaycastResults.m_packedRglEntityId.begin(), 176 | m_rglRaycastResults.m_packedRglEntityId.end(), 177 | segmentationData.value().begin(), 178 | Utils::UnpackRglEntityId); 179 | } 180 | 181 | return AZ::Success(m_raycastResults.value()); 182 | } 183 | 184 | void LidarRaycaster::ConfigureNoiseParameters( 185 | float angularNoiseStdDev, float distanceNoiseStdDevBase, float distanceNoiseStdDevRisePerMeter) 186 | { 187 | m_graph.ConfigureAngularNoiseNode(angularNoiseStdDev); 188 | m_graph.ConfigureDistanceNoiseNode(distanceNoiseStdDevBase, distanceNoiseStdDevRisePerMeter); 189 | m_graph.SetIsNoiseEnabled(true); 190 | } 191 | 192 | void LidarRaycaster::ExcludeEntities(const AZStd::vector& excludedEntities) 193 | { 194 | for (const auto& entity : excludedEntities) 195 | { 196 | RGLInterface::Get()->ExcludeEntity(entity); 197 | } 198 | } 199 | 200 | void LidarRaycaster::UpdateNonHitValues() 201 | { 202 | float minRangeNonHitValue = -AZStd::numeric_limits::infinity(); 203 | float maxRangeNonHitValue = AZStd::numeric_limits::infinity(); 204 | 205 | if (m_isMaxRangeEnabled && m_range.has_value()) 206 | { 207 | minRangeNonHitValue = m_range.value().m_min; 208 | maxRangeNonHitValue = m_range.value().m_max; 209 | } 210 | 211 | m_graph.ConfigureRaytraceNodeNonHits(minRangeNonHitValue, maxRangeNonHitValue); 212 | } 213 | 214 | void LidarRaycaster::ConfigureMaxRangePointAddition(bool addMaxRangePoints) 215 | { 216 | m_isMaxRangeEnabled = addMaxRangePoints; 217 | 218 | UpdateNonHitValues(); 219 | 220 | // We need to configure if points should be compacted to minimize the CPU operations when retrieving raycast results. 221 | m_graph.SetIsCompactEnabled(ShouldEnableCompact()); 222 | m_graph.SetIsPcPublishingEnabled(ShouldEnablePcPublishing()); 223 | } 224 | 225 | void LidarRaycaster::ConfigurePointCloudPublisher( 226 | const AZStd::string& topicName, const AZStd::string& frameId, const ROS2::QoS& qosPolicy) 227 | { 228 | m_graph.ConfigurePcPublisherNode(topicName, frameId, qosPolicy); 229 | m_graph.SetIsPcPublishingEnabled(ShouldEnablePcPublishing()); 230 | } 231 | 232 | bool LidarRaycaster::CanHandlePublishing() 233 | { 234 | return m_graph.IsPcPublishingEnabled(); 235 | } 236 | 237 | AZStd::optional LidarRaycaster::GetRglResultsSize( 238 | const PipelineGraph::RaycastResults& rglResults, const ROS2::RaycastResults& results) 239 | { 240 | // Size consistent among all (present) fields. 241 | // If no consensus is reached or no fields are present, set to AZStd::nullopt. 242 | AZStd::optional resultsSize; 243 | if (results.IsFieldPresent()) 244 | { 245 | resultsSize = rglResults.m_xyz.size(); 246 | } 247 | 248 | if (results.IsFieldPresent()) 249 | { 250 | if (!resultsSize.has_value()) 251 | { 252 | resultsSize = rglResults.m_distance.size(); 253 | } 254 | else if (resultsSize != rglResults.m_distance.size()) 255 | { 256 | return AZStd::nullopt; 257 | } 258 | } 259 | 260 | if (results.IsFieldPresent()) 261 | { 262 | if (!resultsSize.has_value()) 263 | { 264 | resultsSize = rglResults.m_intensity.size(); 265 | } 266 | else if (resultsSize != rglResults.m_intensity.size()) 267 | { 268 | return AZStd::nullopt; 269 | } 270 | } 271 | 272 | if (results.IsFieldPresent()) 273 | { 274 | if (!resultsSize.has_value()) 275 | { 276 | resultsSize = rglResults.m_packedRglEntityId.size(); 277 | } 278 | else if (resultsSize != rglResults.m_packedRglEntityId.size()) 279 | { 280 | return AZStd::nullopt; 281 | } 282 | } 283 | 284 | return resultsSize; 285 | } 286 | 287 | void LidarRaycaster::UpdatePublisherTimestamp(AZ::u64 timestampNanoseconds) 288 | { 289 | RGL_CHECK(rgl_scene_set_time(nullptr, timestampNanoseconds)); 290 | } 291 | 292 | bool LidarRaycaster::ShouldEnableCompact() const 293 | { 294 | return !m_raycastResults->IsFieldPresent() && !m_isMaxRangeEnabled; 295 | } 296 | 297 | bool LidarRaycaster::ShouldEnablePcPublishing() const 298 | { 299 | return m_graph.IsPublisherConfigured() && !m_raycastResults->IsFieldPresent() && 300 | !m_isMaxRangeEnabled; 301 | } 302 | } // namespace RGL 303 | -------------------------------------------------------------------------------- /Code/Source/Lidar/LidarRaycaster.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace RGL 23 | { 24 | class LidarRaycaster : protected ROS2::LidarRaycasterRequestBus::Handler 25 | { 26 | public: 27 | explicit LidarRaycaster(const AZ::Uuid& uuid); 28 | LidarRaycaster(LidarRaycaster&& other); 29 | LidarRaycaster(const LidarRaycaster& other) = delete; 30 | ~LidarRaycaster() override; 31 | 32 | protected: 33 | // LidarRaycasterRequestBus overrides 34 | void ConfigureRayOrientations(const AZStd::vector& orientations) override; 35 | void ConfigureRayRange(ROS2::RayRange range) override; 36 | void ConfigureRaycastResultFlags(ROS2::RaycastResultFlags flags) override; 37 | bool CanHandlePublishing() override; 38 | 39 | // Returns the size of required results. 40 | // If obtained result sizes do not match, 41 | // the returned optional does not contain a value. 42 | static AZStd::optional GetRglResultsSize( 43 | const PipelineGraph::RaycastResults& rglResults, const ROS2::RaycastResults& results); 44 | 45 | AZ::Outcome PerformRaycast(const AZ::Transform& lidarTransform) override; 46 | 47 | void ConfigureNoiseParameters( 48 | [[maybe_unused]] float angularNoiseStdDev, 49 | [[maybe_unused]] float distanceNoiseStdDevBase, 50 | [[maybe_unused]] float distanceNoiseStdDevRisePerMeter) override; 51 | void ExcludeEntities(const AZStd::vector& excludedEntities) override; 52 | void UpdateNonHitValues(); 53 | void ConfigureMaxRangePointAddition(bool addMaxRangePoints) override; 54 | 55 | void ConfigurePointCloudPublisher( 56 | [[maybe_unused]] const AZStd::string& topicName, 57 | [[maybe_unused]] const AZStd::string& frameId, 58 | [[maybe_unused]] const ROS2::QoS& qosPolicy) override; 59 | 60 | void UpdatePublisherTimestamp([[maybe_unused]] AZ::u64 timestampNanoseconds) override; 61 | 62 | private: 63 | AZ::Uuid m_uuid; 64 | 65 | bool m_isMaxRangeEnabled{ false }; //!< Determines whether max range point addition is enabled. 66 | 67 | AZStd::optional m_range{}; 68 | AZStd::vector m_rayTransforms{ AZ::Matrix3x4::CreateIdentity() }; 69 | 70 | PipelineGraph::RaycastResults m_rglRaycastResults; 71 | AZStd::optional m_raycastResults; 72 | 73 | PipelineGraph m_graph; 74 | 75 | [[nodiscard]] bool ShouldEnableCompact() const; 76 | [[nodiscard]] bool ShouldEnablePcPublishing() const; 77 | }; 78 | } // namespace RGL 79 | -------------------------------------------------------------------------------- /Code/Source/Lidar/LidarSystem.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #include 16 | #include 17 | 18 | namespace RGL 19 | { 20 | LidarSystem::LidarSystem(LidarSystem&& lidarSystem) 21 | : m_lidars{ AZStd::move(lidarSystem.m_lidars) } 22 | { 23 | lidarSystem.BusDisconnect(); 24 | } 25 | 26 | void LidarSystem::Activate() 27 | { 28 | const char* name = "RobotecGPULidar"; 29 | const char* description = "Mesh-based lidar implementation that uses the RobotecGPULidar API for GPU-enabled raycasting."; 30 | 31 | using Features = ROS2::LidarSystemFeatures; 32 | static constexpr auto SupportedFeatures = aznumeric_cast( 33 | Features::EntityExclusion | Features::MaxRangePoints | Features::Noise | Features::PointcloudPublishing | Features::Intensity | 34 | Features::Segmentation); 35 | 36 | ROS2::LidarSystemRequestBus::Handler::BusConnect(AZ_CRC(name)); 37 | 38 | auto* lidarSystemManagerInterface = ROS2::LidarRegistrarInterface::Get(); 39 | AZ_Assert(lidarSystemManagerInterface != nullptr, "The ROS2 LidarSystem Manager interface was inaccessible."); 40 | lidarSystemManagerInterface->RegisterLidarSystem(name, description, SupportedFeatures); 41 | } 42 | 43 | void LidarSystem::Deactivate() 44 | { 45 | ROS2::LidarSystemRequestBus::Handler::BusDisconnect(); 46 | } 47 | 48 | void LidarSystem::Clear() 49 | { 50 | m_lidars.clear(); 51 | } 52 | 53 | ROS2::LidarId LidarSystem::CreateLidar(AZ::EntityId lidarEntityId) 54 | { 55 | const AZ::Uuid lidarUuid = AZ::Uuid::CreateRandom(); 56 | m_lidars.emplace(lidarUuid, LidarRaycaster(lidarUuid)); 57 | return ROS2::LidarId(lidarUuid); 58 | } 59 | 60 | void LidarSystem::DestroyLidar(ROS2::LidarId lidarId) 61 | { 62 | m_lidars.erase(lidarId); 63 | } 64 | } // namespace RGL -------------------------------------------------------------------------------- /Code/Source/Lidar/LidarSystem.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | namespace RGL 21 | { 22 | class LidarSystem : protected ROS2::LidarSystemRequestBus::Handler 23 | { 24 | public: 25 | LidarSystem() = default; 26 | LidarSystem(LidarSystem&& lidarSystem); 27 | LidarSystem(const LidarSystem& lidarSystem) = delete; 28 | ~LidarSystem() = default; 29 | 30 | void Activate(); 31 | void Deactivate(); 32 | 33 | //! Deletes all lidar raycasters created by this system. 34 | void Clear(); 35 | 36 | protected: 37 | // LidarSystemRequestBus overrides 38 | ROS2::LidarId CreateLidar(AZ::EntityId lidarEntityId) override; 39 | void DestroyLidar(ROS2::LidarId lidarId) override; 40 | 41 | private: 42 | AZStd::unordered_map m_lidars; 43 | }; 44 | } // namespace RGL 45 | -------------------------------------------------------------------------------- /Code/Source/Lidar/LidarSystemNotificationBus.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2024, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace RGL 23 | { 24 | class LidarSystemNotifications : public AZ::EBusTraits 25 | { 26 | public: 27 | virtual ~LidarSystemNotifications() = default; 28 | 29 | ////////////////////////////////////////////////////////////////////////// 30 | // EBusTraits overrides 31 | static const AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Multiple; 32 | static const AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::Single; 33 | ////////////////////////////////////////////////////////////////////////// 34 | 35 | virtual void OnLidarCreated() 36 | { 37 | } 38 | 39 | virtual void OnLidarDestroyed() 40 | { 41 | } 42 | ////////////////////////////////////////////////////////////////////////// 43 | }; 44 | using LidarSystemNotificationBus = AZ::EBus; 45 | } // namespace RGL 46 | -------------------------------------------------------------------------------- /Code/Source/Lidar/PipelineGraph.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #include 16 | #include 17 | #include 18 | 19 | namespace RGL 20 | { 21 | PipelineGraph::PipelineGraph() 22 | { 23 | ConfigureRayPosesNode({ Utils::IdentityTransform }); 24 | ConfigureRayRangesNode(0.0f, 1.0f); 25 | ConfigureLidarTransformNode(AZ::Matrix3x4::CreateIdentity()); 26 | RGL_CHECK(rgl_node_raytrace(&m_nodes.m_rayTrace, nullptr)); 27 | RGL_CHECK(rgl_node_points_compact_by_field(&m_nodes.m_pointsCompact, RGL_FIELD_IS_HIT_I32)); 28 | ConfigureAngularNoiseNode(0.0f); 29 | ConfigureDistanceNoiseNode(0.0f, 0.0f); 30 | ConfigureYieldNodes(DefaultFields.data(), DefaultFields.size()); 31 | 32 | ConfigurePcTransformNode(AZ::Matrix3x4::CreateIdentity()); 33 | RGL_CHECK(rgl_node_points_format(&m_nodes.m_pcPublishFormat, DefaultFields.data(), aznumeric_cast(DefaultFields.size()))); 34 | 35 | // Non-conditional connections 36 | RGL_CHECK(rgl_graph_node_add_child(m_nodes.m_rayPoses, m_nodes.m_rayRanges)); 37 | RGL_CHECK(rgl_graph_node_add_child(m_nodes.m_rayRanges, m_nodes.m_lidarTransform)); 38 | RGL_CHECK(rgl_graph_node_add_child(m_nodes.m_compactYield, m_nodes.m_pointsYield)); 39 | RGL_CHECK(rgl_graph_node_add_child(m_nodes.m_pointCloudTransform, m_nodes.m_pcPublishFormat)); 40 | 41 | InitializeConditionalConnections(); 42 | } 43 | 44 | PipelineGraph::PipelineGraph(PipelineGraph&& other) 45 | : m_nodes{ other.m_nodes } 46 | , m_activeFeatures{ other.m_activeFeatures } 47 | , m_conditionalConnections(std::move(other.m_conditionalConnections)) 48 | { 49 | other.m_nodes = {}; 50 | other.m_conditionalConnections.clear(); 51 | } 52 | 53 | PipelineGraph::~PipelineGraph() 54 | { 55 | if (!m_nodes.m_rayPoses) 56 | { 57 | return; 58 | } 59 | 60 | // We enable all the features we can to destroy the whole graph with 61 | // one (or two) rgl_graph_destroy API call(s). 62 | SetIsNoiseEnabled(true); 63 | SetIsCompactEnabled(true); 64 | if (IsPublisherConfigured()) 65 | { 66 | SetIsPcPublishingEnabled(true); 67 | } 68 | else 69 | { 70 | rgl_graph_destroy(m_nodes.m_pointCloudTransform); 71 | } 72 | 73 | rgl_graph_destroy(m_nodes.m_rayPoses); 74 | } 75 | 76 | bool PipelineGraph::IsCompactEnabled() const 77 | { 78 | return IsFeatureEnabled(PipelineFeatureFlags::PointsCompact); 79 | } 80 | bool PipelineGraph::IsPcPublishingEnabled() const 81 | { 82 | return IsFeatureEnabled(PipelineFeatureFlags::PointCloudPublishing); 83 | } 84 | bool PipelineGraph::IsNoiseEnabled() const 85 | { 86 | return IsFeatureEnabled(PipelineFeatureFlags::Noise); 87 | } 88 | 89 | void PipelineGraph::ConfigureRayPosesNode(const AZStd::vector& rayPoses) 90 | { 91 | RGL_CHECK(rgl_node_rays_from_mat3x4f(&m_nodes.m_rayPoses, rayPoses.data(), aznumeric_cast(rayPoses.size()))); 92 | } 93 | 94 | void PipelineGraph::ConfigureRayRangesNode(float min, float max) 95 | { 96 | const rgl_vec2f range = { .value = { min, max } }; 97 | RGL_CHECK(rgl_node_rays_set_range(&m_nodes.m_rayRanges, &range, 1)); 98 | } 99 | 100 | void PipelineGraph::ConfigureYieldNodes(const rgl_field_t* fields, size_t size) 101 | { 102 | RGL_CHECK(rgl_node_points_yield(&m_nodes.m_pointsYield, fields, aznumeric_cast(size))); 103 | RGL_CHECK(rgl_node_points_yield(&m_nodes.m_rayTraceYield, fields, aznumeric_cast(size))); 104 | RGL_CHECK(rgl_node_points_yield(&m_nodes.m_compactYield, fields, aznumeric_cast(size))); 105 | } 106 | 107 | void PipelineGraph::ConfigureLidarTransformNode(const AZ::Matrix3x4& lidarTransform) 108 | { 109 | const rgl_mat3x4f RglLidarTransform = Utils::RglMat3x4FromAzMatrix3x4(lidarTransform); 110 | RGL_CHECK(rgl_node_rays_transform(&m_nodes.m_lidarTransform, &RglLidarTransform)); 111 | } 112 | 113 | void PipelineGraph::ConfigurePcTransformNode(const AZ::Matrix3x4& pcTransform) 114 | { 115 | const rgl_mat3x4f rglPcTransform = Utils::RglMat3x4FromAzMatrix3x4(pcTransform); 116 | RGL_CHECK(rgl_node_points_transform(&m_nodes.m_pointCloudTransform, &rglPcTransform)); 117 | } 118 | 119 | void PipelineGraph::ConfigureAngularNoiseNode(float angularNoiseStdDev) 120 | { 121 | RGL_CHECK(rgl_node_gaussian_noise_angular_ray(&m_nodes.m_angularNoise, 0.0f, angularNoiseStdDev, RGL_AXIS_Z)); 122 | } 123 | 124 | void PipelineGraph::ConfigureDistanceNoiseNode(float distanceNoiseStdDevBase, float distanceNoiseStdDevRisePerMeter) 125 | { 126 | RGL_CHECK( 127 | rgl_node_gaussian_noise_distance(&m_nodes.m_distanceNoise, 0.0f, distanceNoiseStdDevBase, distanceNoiseStdDevRisePerMeter)); 128 | } 129 | 130 | void PipelineGraph::ConfigurePcPublisherNode(const AZStd::string& topicName, const AZStd::string& frameId, const ROS2::QoS& qosPolicy) 131 | { 132 | const bool FirstConfiguration = !IsPublisherConfigured(); 133 | 134 | RGL_CHECK(rgl_node_points_ros2_publish_with_qos( 135 | &m_nodes.m_pointCloudPublish, 136 | topicName.c_str(), 137 | frameId.c_str(), 138 | static_cast(static_cast(qosPolicy.GetQoS().reliability())), 139 | static_cast(static_cast(qosPolicy.GetQoS().durability())), 140 | static_cast(static_cast(qosPolicy.GetQoS().history())), 141 | qosPolicy.GetQoS().depth())); 142 | 143 | if (FirstConfiguration) 144 | { 145 | // clang-format off 146 | AddConditionalConnection(m_nodes.m_pcPublishFormat, m_nodes.m_pointCloudPublish, [](const PipelineGraph& graph){ return graph.IsPcPublishingEnabled(); }); 147 | // clang-format on 148 | } 149 | } 150 | 151 | void PipelineGraph::ConfigureRaytraceNodeNonHits(float minRangeNonHitValue, float maxRangeNonHitValue) 152 | { 153 | RGL_CHECK(rgl_node_raytrace_configure_non_hits(m_nodes.m_rayTrace, minRangeNonHitValue, maxRangeNonHitValue)); 154 | } 155 | 156 | void PipelineGraph::SetIsCompactEnabled(bool value) 157 | { 158 | SetIsFeatureEnabled(PipelineFeatureFlags::PointsCompact, value); 159 | } 160 | 161 | void PipelineGraph::SetIsPcPublishingEnabled(bool value) 162 | { 163 | if (value && !IsPublisherConfigured()) 164 | { 165 | AZ_Assert(false, "Trying to enable publishing without the publisher node configured."); 166 | return; 167 | } 168 | SetIsFeatureEnabled(PipelineFeatureFlags::PointCloudPublishing, value); 169 | } 170 | 171 | void PipelineGraph::SetIsNoiseEnabled(bool value) 172 | { 173 | SetIsFeatureEnabled(PipelineFeatureFlags::Noise, value); 174 | } 175 | 176 | void PipelineGraph::Run() 177 | { 178 | RGL_CHECK(rgl_graph_run(m_nodes.m_rayPoses)); 179 | } 180 | 181 | bool PipelineGraph::GetResults(RaycastResults& results) const 182 | { 183 | bool success = true; 184 | for (rgl_field_t field : results.m_fields) 185 | { 186 | switch (field) 187 | { 188 | case RGL_FIELD_XYZ_VEC3_F32: 189 | success = success && GetResult(results.m_xyz, RGL_FIELD_XYZ_VEC3_F32); 190 | break; 191 | case RGL_FIELD_DISTANCE_F32: 192 | success = success && GetResult(results.m_distance, RGL_FIELD_DISTANCE_F32); 193 | break; 194 | case RGL_FIELD_INTENSITY_F32: 195 | success = success && GetResult(results.m_intensity, RGL_FIELD_INTENSITY_F32); 196 | break; 197 | case RGL_FIELD_ENTITY_ID_I32: 198 | success = success && GetResult(results.m_packedRglEntityId, RGL_FIELD_ENTITY_ID_I32); 199 | break; 200 | default: 201 | success = false; 202 | AZ_Assert(false, "Invalid result field type!"); 203 | break; 204 | } 205 | } 206 | 207 | return success; 208 | } 209 | 210 | bool PipelineGraph::IsFeatureEnabled(PipelineGraph::PipelineFeatureFlags feature) const 211 | { 212 | return m_activeFeatures & feature; 213 | } 214 | 215 | void PipelineGraph::SetIsFeatureEnabled(PipelineFeatureFlags feature, bool value) 216 | { 217 | if (value) 218 | { 219 | m_activeFeatures = static_cast(m_activeFeatures | feature); 220 | } 221 | else 222 | { 223 | m_activeFeatures = static_cast(m_activeFeatures & ~feature); 224 | } 225 | 226 | UpdateConnections(); 227 | } 228 | 229 | void PipelineGraph::InitializeConditionalConnections() 230 | { 231 | const ConditionType NoiseCondition = [](const PipelineGraph& graph) 232 | { 233 | return graph.IsNoiseEnabled(); 234 | }; 235 | 236 | const ConditionType CompactCondition = [](const PipelineGraph& graph) 237 | { 238 | return graph.IsCompactEnabled(); 239 | }; 240 | 241 | const ConditionType PublishingCondition = [](const PipelineGraph& graph) 242 | { 243 | return graph.IsPcPublishingEnabled(); 244 | }; 245 | 246 | // clang-format off 247 | AddConditionalNode(m_nodes.m_angularNoise, m_nodes.m_lidarTransform, m_nodes.m_rayTrace, NoiseCondition); 248 | AddConditionalNode(m_nodes.m_distanceNoise, m_nodes.m_rayTrace, m_nodes.m_rayTraceYield, NoiseCondition); 249 | AddConditionalNode(m_nodes.m_pointsCompact, m_nodes.m_rayTraceYield, m_nodes.m_compactYield, CompactCondition); 250 | AddConditionalConnection(m_nodes.m_compactYield, m_nodes.m_pointCloudTransform, PublishingCondition); 251 | // clang-format on 252 | if (IsPublisherConfigured()) 253 | { 254 | // clang-format off 255 | AddConditionalConnection( m_nodes.m_pcPublishFormat, m_nodes.m_pointCloudPublish, PublishingCondition); 256 | // clang-format on 257 | } 258 | } 259 | 260 | void PipelineGraph::UpdateConnections() 261 | { 262 | for (ConditionalConnection& connection : m_conditionalConnections) 263 | { 264 | connection.Update(*this); 265 | } 266 | } 267 | 268 | void PipelineGraph::AddConditionalNode(rgl_node_t node, rgl_node_t parent, rgl_node_t child, const ConditionType& condition) 269 | { 270 | AddConditionalConnection(parent, node, condition); 271 | AddConditionalConnection(node, child, condition); 272 | // clang-format off 273 | AddConditionalConnection(parent, child, [condition](const PipelineGraph& pipelineGraph){ return !condition(pipelineGraph); }); 274 | // clang-format on 275 | } 276 | 277 | void PipelineGraph::AddConditionalConnection(rgl_node_t parent, rgl_node_t child, const ConditionType& condition) 278 | { 279 | m_conditionalConnections.emplace_back(parent, child, condition, condition(*this)); 280 | } 281 | 282 | PipelineGraph::ConditionalConnection::ConditionalConnection( 283 | rgl_node_t parent, rgl_node_t child, const ConditionType& condition, bool activate) 284 | : m_parent(parent) 285 | , m_child(child) 286 | , m_condition(condition) 287 | , m_isActive(activate) 288 | { 289 | if (activate) 290 | { 291 | RGL_CHECK(rgl_graph_node_add_child(parent, child)); 292 | } 293 | } 294 | 295 | void PipelineGraph::ConditionalConnection::Update(const PipelineGraph& graph) 296 | { 297 | const bool IsConditionSatisfied = m_condition(graph); 298 | if (IsConditionSatisfied == m_isActive) 299 | { 300 | return; 301 | } 302 | 303 | m_isActive = IsConditionSatisfied; 304 | 305 | if (IsConditionSatisfied) 306 | { 307 | RGL_CHECK(rgl_graph_node_add_child(m_parent, m_child)); 308 | return; 309 | } 310 | 311 | RGL_CHECK(rgl_graph_node_remove_child(m_parent, m_child)); 312 | } 313 | } // namespace RGL -------------------------------------------------------------------------------- /Code/Source/Lidar/PipelineGraph.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | namespace RGL 24 | { 25 | //! Class that manages the RGL pipeline graph construction, which depends on 26 | //! three conditions: point-cloud compact, noise and publication. The diagram 27 | //! representation of this graph can be found under static/PipelineGraph.mmd. 28 | class PipelineGraph 29 | { 30 | private: 31 | static constexpr AZStd::array DefaultFields{ RGL_FIELD_IS_HIT_I32, RGL_FIELD_XYZ_VEC3_F32, RGL_FIELD_INTENSITY_F32 }; 32 | 33 | public: 34 | struct RaycastResults 35 | { 36 | AZStd::vector m_fields{ DefaultFields.data(), DefaultFields.data() + DefaultFields.size() }; 37 | AZStd::vector m_xyz; 38 | AZStd::vector m_distance; 39 | AZStd::vector m_intensity; 40 | AZStd::vector m_packedRglEntityId; 41 | }; 42 | 43 | struct Nodes 44 | { 45 | rgl_node_t m_rayPoses{ nullptr }, m_rayRanges{ nullptr }, m_lidarTransform{ nullptr }, m_angularNoise{ nullptr }, 46 | m_rayTrace{ nullptr }, m_distanceNoise{ nullptr }, m_rayTraceYield{ nullptr }, m_pointsCompact{ nullptr }, 47 | m_compactYield{ nullptr }, m_pointsYield{ nullptr }, m_pointCloudTransform{ nullptr }, m_pcPublishFormat{ nullptr }, 48 | m_pointCloudPublish{ nullptr }; 49 | }; 50 | 51 | PipelineGraph(); 52 | PipelineGraph(const PipelineGraph& other) = delete; 53 | PipelineGraph(PipelineGraph&& other); 54 | ~PipelineGraph(); 55 | 56 | [[nodiscard]] bool IsCompactEnabled() const; 57 | [[nodiscard]] bool IsPcPublishingEnabled() const; 58 | [[nodiscard]] bool IsNoiseEnabled() const; 59 | [[nodiscard]] bool IsPublisherConfigured() const 60 | { 61 | return m_nodes.m_pointCloudPublish; 62 | } 63 | 64 | void ConfigureRayPosesNode(const AZStd::vector& rayPoses); 65 | void ConfigureRayRangesNode(float min, float max); 66 | void ConfigureYieldNodes(const rgl_field_t* fields, size_t size); 67 | void ConfigureLidarTransformNode(const AZ::Matrix3x4& lidarTransform); 68 | void ConfigurePcTransformNode(const AZ::Matrix3x4& pcTransform); 69 | void ConfigureAngularNoiseNode(float angularNoiseStdDev); 70 | void ConfigureDistanceNoiseNode(float distanceNoiseStdDevBase, float distanceNoiseStdDevRisePerMeter); 71 | void ConfigurePcPublisherNode(const AZStd::string& topicName, const AZStd::string& frameId, const ROS2::QoS& qosPolicy); 72 | void ConfigureRaytraceNodeNonHits(float minRangeNonHitValue, float maxRangeNonHitValue); 73 | 74 | void SetIsCompactEnabled(bool value); 75 | void SetIsPcPublishingEnabled(bool value); 76 | void SetIsNoiseEnabled(bool value); 77 | 78 | void Run(); 79 | 80 | //! Get the raycast results. 81 | //! @param results Raycast results destination. 82 | //! @return If successful returns true, otherwise returns false. 83 | bool GetResults(RaycastResults& results) const; 84 | 85 | private: 86 | enum PipelineFeatureFlags : uint8_t 87 | // clang-format off 88 | { 89 | None = 0, 90 | Noise = 1, 91 | PointsCompact = 1 << 1, 92 | PointCloudPublishing = 1 << 2, 93 | All = Noise | PointsCompact | PointCloudPublishing, 94 | }; 95 | // clang-format on 96 | 97 | using ConditionType = AZStd::function; 98 | 99 | class ConditionalConnection 100 | { 101 | public: 102 | ConditionalConnection(rgl_node_t parent, rgl_node_t child, const ConditionType& condition, bool activate = false); 103 | void Update(const PipelineGraph& graph); 104 | 105 | private: 106 | bool m_isActive; 107 | ConditionType m_condition; 108 | rgl_node_t m_parent, m_child; 109 | }; 110 | 111 | [[nodiscard]] bool IsFeatureEnabled(PipelineFeatureFlags feature) const; 112 | 113 | //! Get a raycast result of specified field. 114 | //! @param result Raycast field result vector. 115 | //! @param rglFieldType Enum value representing the field type. 116 | //! @return If successful returns true, otherwise returns false. 117 | template 118 | bool GetResult(AZStd::vector& result, rgl_field_t rglFieldType) const 119 | { 120 | int32_t resultSize = -1; 121 | RGL_CHECK(rgl_graph_get_result_size(m_nodes.m_pointsYield, rglFieldType, &resultSize, nullptr)); 122 | 123 | if (resultSize <= 0) 124 | { 125 | return false; 126 | } 127 | 128 | result.resize(resultSize); 129 | bool success = false; 130 | Utils::ErrorCheck(rgl_graph_get_result_data(m_nodes.m_pointsYield, rglFieldType, result.data()), __FILE__, __LINE__, &success); 131 | return success; 132 | } 133 | 134 | void SetIsFeatureEnabled(PipelineFeatureFlags feature, bool value); 135 | void InitializeConditionalConnections(); 136 | void UpdateConnections(); 137 | //! Adds conditional connections that support conditional insertion / removal 138 | //! of a node from in between the parent and child node. 139 | //! @param condition When true, the node should connect with parent and child. 140 | //! Otherwise the node is not connected. 141 | void AddConditionalNode(rgl_node_t node, rgl_node_t parent, rgl_node_t child, const ConditionType& condition); 142 | void AddConditionalConnection(rgl_node_t parent, rgl_node_t child, const ConditionType& condition); 143 | 144 | PipelineFeatureFlags m_activeFeatures{ PointsCompact }; 145 | Nodes m_nodes; 146 | std::vector m_conditionalConnections; 147 | }; 148 | } // namespace RGL 149 | -------------------------------------------------------------------------------- /Code/Source/Model/ModelLibrary.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2024, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #include 16 | #include 17 | #include 18 | 19 | namespace RGL 20 | { 21 | ModelLibrary::ModelLibrary() 22 | { 23 | if (!ModelLibraryInterface::Get()) 24 | { 25 | ModelLibraryInterface::Register(this); 26 | } 27 | 28 | ModelLibraryRequestBus::Handler::BusConnect(); 29 | } 30 | 31 | ModelLibrary::ModelLibrary(ModelLibrary&& modelLibrary) 32 | : m_meshMap{ AZStd::move(modelLibrary.m_meshMap) } 33 | , m_textureMap{ AZStd::move(modelLibrary.m_textureMap) } 34 | , m_invalidTexture(AZStd::move(modelLibrary.m_invalidTexture)) 35 | { 36 | modelLibrary.BusDisconnect(); 37 | ModelLibraryInterface::Unregister(&modelLibrary); 38 | ModelLibraryInterface::Register(this); 39 | ModelLibraryRequestBus::Handler::BusConnect(); 40 | } 41 | 42 | ModelLibrary::~ModelLibrary() 43 | { 44 | if (ModelLibraryInterface::Get() == this) 45 | { 46 | ModelLibraryInterface::Unregister(this); 47 | } 48 | 49 | ModelLibraryRequestBus::Handler::BusDisconnect(); 50 | } 51 | 52 | void ModelLibrary::Clear() 53 | { 54 | m_meshMap.clear(); 55 | m_textureMap.clear(); 56 | } 57 | 58 | const MeshMaterialSlotPairList& ModelLibrary::StoreModelAsset(const AZ::Data::Asset& modelAsset) 59 | { 60 | const AZ::Data::AssetId& assetId = modelAsset.GetId(); 61 | if (auto meshPointersIt = m_meshMap.find(assetId); meshPointersIt != m_meshMap.end()) 62 | { 63 | return meshPointersIt->second; 64 | } 65 | 66 | const auto lodAssets = modelAsset->GetLodAssets(); 67 | // Get Highest LOD 68 | const auto modelLodAsset = lodAssets.begin()->Get(); 69 | const auto meshes = modelLodAsset->GetMeshes(); 70 | 71 | MeshMaterialSlotPairList modelMeshes; 72 | modelMeshes.reserve(meshes.size()); 73 | for (auto& mesh : meshes) 74 | { 75 | const AZStd::span vertices = mesh.GetSemanticBufferTyped(AZ::Name("POSITION")); 76 | const AZStd::span indices = mesh.GetIndexBufferTyped(); 77 | 78 | Wrappers::RglMesh rglMesh(vertices.data(), vertices.size(), indices.data(), indices.size()); 79 | if (!rglMesh.IsValid()) 80 | { 81 | continue; 82 | } 83 | 84 | const AZStd::span uvs = mesh.GetSemanticBufferTyped(AZ::Name("UV")); 85 | rglMesh.SetTextureCoordinates(uvs.data(), uvs.size()); 86 | 87 | const AZ::RPI::ModelMaterialSlot& slot = modelAsset->FindMaterialSlot(mesh.GetMaterialSlotId()); 88 | modelMeshes.emplace_back(AZStd::move(rglMesh), slot); 89 | } 90 | 91 | return m_meshMap.emplace(assetId, AZStd::move(modelMeshes)).first->second; 92 | } 93 | 94 | const Wrappers::RglTexture& ModelLibrary::StoreMaterialAsset(const AZ::Data::Asset& materialAsset) 95 | { 96 | const AZ::Data::AssetId& assetId = materialAsset.GetId(); 97 | if (auto textureIt = m_textureMap.find(assetId); textureIt != m_textureMap.end()) 98 | { 99 | return textureIt->second; 100 | } 101 | 102 | Wrappers::RglTexture materialTexture = AZStd::move(Wrappers::RglTexture::CreateFromMaterialAsset(materialAsset)); 103 | if (materialTexture.IsValid()) 104 | { 105 | return m_textureMap.emplace(assetId, AZStd::move(materialTexture)).first->second; 106 | } 107 | 108 | return m_invalidTexture; 109 | } 110 | } // namespace RGL 111 | -------------------------------------------------------------------------------- /Code/Source/Model/ModelLibrary.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2024, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace RGL 25 | { 26 | //! Class providing easy access to RGL's meshes. 27 | //! Each mesh has a corresponding modelAsset by which it is accessed. 28 | class ModelLibrary : protected ModelLibraryRequestBus::Handler 29 | { 30 | public: 31 | ModelLibrary(); 32 | ModelLibrary(ModelLibrary&& modelLibrary); 33 | ModelLibrary(const ModelLibrary& modelLibrary) = delete; 34 | ~ModelLibrary(); 35 | 36 | //! Deletes all meshes and textures stored by the Library. 37 | void Clear(); 38 | 39 | protected: 40 | // ModelLibraryRequestBus overrides 41 | const MeshMaterialSlotPairList& StoreModelAsset(const AZ::Data::Asset& modelAsset) override; 42 | const Wrappers::RglTexture& StoreMaterialAsset(const AZ::Data::Asset& materialAsset) override; 43 | Wrappers::RglTexture m_invalidTexture{ AZStd::move(Wrappers::RglTexture::CreateInvalid()) }; 44 | 45 | private: 46 | using MeshMap = AZStd::unordered_map; 47 | using TextureMap = AZStd::unordered_map; 48 | 49 | MeshMap m_meshMap; 50 | TextureMap m_textureMap; 51 | }; 52 | } // namespace RGL -------------------------------------------------------------------------------- /Code/Source/Model/ModelLibraryBus.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace RGL 25 | { 26 | namespace Wrappers 27 | { 28 | class RglTexture; 29 | class RglMesh; 30 | } // namespace Wrappers 31 | 32 | using MeshMaterialSlotPairList = AZStd::vector>; 33 | 34 | class ModelLibraryRequests 35 | { 36 | public: 37 | AZ_RTTI(ModelLibraryRequests, "{b84ccaae-5d0f-410a-821e-5ff8d449b851}"); 38 | 39 | //! Returns a vector of RGL meshes created from the modelAsset. 40 | //! If the provided modelAsset was not encountered before, created RGL meshes are stored by the library. 41 | //! On the other hand if the RGL meshes associated with the provided modelAsset were stored it will simply retrieve them. 42 | //! @param modelAsset Model asset provided for storage. 43 | //! @return List of RGL meshes created using the provided model asset. 44 | virtual const MeshMaterialSlotPairList& StoreModelAsset(const AZ::Data::Asset& modelAsset) = 0; 45 | 46 | //! Returns the texture created using provided materialAsset. 47 | //! The returned texture reference may point to an invalid texture. 48 | virtual const Wrappers::RglTexture& StoreMaterialAsset(const AZ::Data::Asset& materialAsset) = 0; 49 | 50 | protected: 51 | ~ModelLibraryRequests() = default; 52 | }; 53 | 54 | class ModelLibraryBusTraits : public AZ::EBusTraits 55 | { 56 | public: 57 | ////////////////////////////////////////////////////////////////////////// 58 | // EBusTraits overrides 59 | static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Single; 60 | static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::Single; 61 | ////////////////////////////////////////////////////////////////////////// 62 | }; 63 | 64 | using ModelLibraryRequestBus = AZ::EBus; 65 | using ModelLibraryInterface = AZ::Interface; 66 | } // namespace RGL -------------------------------------------------------------------------------- /Code/Source/RGLEditorModule.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace RGL 21 | { 22 | class RGLEditorModule : public RGLModuleInterface 23 | { 24 | public: 25 | AZ_RTTI(RGLEditorModule, "{06ADDE3E-333C-49E7-A63F-BE3E8F3CFB4C}", RGLModuleInterface); 26 | AZ_CLASS_ALLOCATOR(RGLEditorModule, AZ::SystemAllocator, 0); 27 | 28 | RGLEditorModule() 29 | { 30 | // Push results of [MyComponent]::CreateDescriptor() into m_descriptors here. 31 | // Add ALL components descriptors associated with this gem to m_descriptors. 32 | // This will associate the AzTypeInfo information for the components with the SerializeContext, BehaviorContext and 33 | // EditContext. This happens through the [MyComponent]::Reflect() function. 34 | m_descriptors.insert( 35 | m_descriptors.end(), 36 | { 37 | RGLEditorSystemComponent::CreateDescriptor(), 38 | TerrainEntityManagerEditorSystemComponent::CreateDescriptor(), 39 | SceneConfigurationComponent::CreateDescriptor(), 40 | }); 41 | } 42 | 43 | /** 44 | * Add required SystemComponents to the SystemEntity. 45 | * Non-SystemComponents should not be added here 46 | */ 47 | AZ::ComponentTypeList GetRequiredSystemComponents() const override 48 | { 49 | return AZ::ComponentTypeList{ 50 | azrtti_typeid(), 51 | azrtti_typeid(), 52 | azrtti_typeid(), 53 | }; 54 | } 55 | }; 56 | } // namespace RGL 57 | 58 | AZ_DECLARE_MODULE_CLASS(Gem_RGL, RGL::RGLEditorModule) 59 | -------------------------------------------------------------------------------- /Code/Source/RGLEditorSystemComponent.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #include 16 | 17 | namespace RGL 18 | { 19 | void RGLEditorSystemComponent::Reflect(AZ::ReflectContext* context) 20 | { 21 | if (auto serializeContext = azrtti_cast(context)) 22 | { 23 | serializeContext->Class()->Version(0); 24 | } 25 | } 26 | 27 | RGLEditorSystemComponent::RGLEditorSystemComponent() = default; 28 | 29 | RGLEditorSystemComponent::~RGLEditorSystemComponent() = default; 30 | 31 | void RGLEditorSystemComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) 32 | { 33 | RGLSystemComponent::GetProvidedServices(provided); 34 | provided.push_back(AZ_CRC_CE("RGLEditorService")); 35 | } 36 | 37 | void RGLEditorSystemComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible) 38 | { 39 | RGLSystemComponent::GetIncompatibleServices(incompatible); 40 | incompatible.push_back(AZ_CRC_CE("RGLEditorService")); 41 | } 42 | 43 | void RGLEditorSystemComponent::GetRequiredServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) 44 | { 45 | RGLSystemComponent::GetRequiredServices(required); 46 | } 47 | 48 | void RGLEditorSystemComponent::GetDependentServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) 49 | { 50 | RGLSystemComponent::GetDependentServices(dependent); 51 | } 52 | 53 | void RGLEditorSystemComponent::Activate() 54 | { 55 | RGLSystemComponent::Activate(); 56 | } 57 | 58 | void RGLEditorSystemComponent::Deactivate() 59 | { 60 | RGLSystemComponent::Deactivate(); 61 | } 62 | 63 | } // namespace RGL 64 | -------------------------------------------------------------------------------- /Code/Source/RGLEditorSystemComponent.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | 19 | namespace RGL 20 | { 21 | /// System component for the RGL editor 22 | class RGLEditorSystemComponent : public RGLSystemComponent 23 | { 24 | public: 25 | AZ_COMPONENT(RGLEditorSystemComponent, "{E36B695E-36C5-4162-BF86-EA68AA21217C}", RGLSystemComponent); 26 | static void Reflect(AZ::ReflectContext* context); 27 | 28 | RGLEditorSystemComponent(); 29 | ~RGLEditorSystemComponent(); 30 | 31 | private: 32 | static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); 33 | static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible); 34 | static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); 35 | static void GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent); 36 | 37 | // AZ::Component 38 | void Activate() override; 39 | void Deactivate() override; 40 | }; 41 | } // namespace RGL 42 | -------------------------------------------------------------------------------- /Code/Source/RGLModule.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #include 16 | #include 17 | 18 | namespace RGL 19 | { 20 | class RGLModule : public RGLModuleInterface 21 | { 22 | public: 23 | AZ_RTTI(RGLModule, "{06ADDE3E-333C-49E7-A63F-BE3E8F3CFB4C}", RGLModuleInterface); 24 | AZ_CLASS_ALLOCATOR(RGLModule, AZ::SystemAllocator, 0); 25 | }; 26 | } // namespace RGL 27 | 28 | AZ_DECLARE_MODULE_CLASS(Gem_RGL, RGL::RGLModule) 29 | -------------------------------------------------------------------------------- /Code/Source/RGLModuleInterface.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | namespace RGL 24 | { 25 | class RGLModuleInterface : public AZ::Module 26 | { 27 | public: 28 | AZ_RTTI(RGLModuleInterface, "{75F40506-8366-49E2-9E49-F94010953EE3}", AZ::Module); 29 | AZ_CLASS_ALLOCATOR(RGLModuleInterface, AZ::SystemAllocator, 0); 30 | 31 | RGLModuleInterface() 32 | { 33 | // Push results of [MyComponent]::CreateDescriptor() into m_descriptors here. 34 | // Add ALL components descriptors associated with this gem to m_descriptors. 35 | // This will associate the AzTypeInfo information for the components with the the SerializeContext, BehaviorContext and 36 | // EditContext. This happens through the [MyComponent]::Reflect() function. 37 | m_descriptors.insert( 38 | m_descriptors.end(), 39 | { 40 | RGLSystemComponent::CreateDescriptor(), 41 | TerrainEntityManagerSystemComponent::CreateDescriptor(), 42 | SceneConfigurationComponent::CreateDescriptor(), 43 | }); 44 | } 45 | 46 | //! Add required SystemComponents to the SystemEntity. 47 | AZ::ComponentTypeList GetRequiredSystemComponents() const override 48 | { 49 | return AZ::ComponentTypeList{ 50 | azrtti_typeid(), 51 | azrtti_typeid(), 52 | }; 53 | } 54 | }; 55 | } // namespace RGL 56 | -------------------------------------------------------------------------------- /Code/Source/RGLSystemComponent.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace RGL 28 | { 29 | void RGLSystemComponent::Reflect(AZ::ReflectContext* context) 30 | { 31 | if (AZ::SerializeContext* serializeContext = azrtti_cast(context)) 32 | { 33 | serializeContext->Class()->Version(0); 34 | 35 | if (AZ::EditContext* editContext = serializeContext->GetEditContext()) 36 | { 37 | editContext->Class("RGLSystemComponent", "[Description of functionality provided by this component]") 38 | ->ClassElement(AZ::Edit::ClassElements::EditorData, "") 39 | ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("System")) 40 | ->Attribute(AZ::Edit::Attributes::AutoExpand, true); 41 | } 42 | } 43 | } 44 | 45 | void RGLSystemComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided) 46 | { 47 | provided.push_back(AZ_CRC_CE("RGLService")); 48 | } 49 | 50 | void RGLSystemComponent::GetIncompatibleServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& incompatible) 51 | { 52 | incompatible.push_back(AZ_CRC_CE("RGLService")); 53 | } 54 | 55 | void RGLSystemComponent::GetRequiredServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) 56 | { 57 | required.push_back(AZ_CRC_CE("ROS2Service")); 58 | } 59 | 60 | void RGLSystemComponent::GetDependentServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) 61 | { 62 | } 63 | 64 | RGLSystemComponent::RGLSystemComponent() 65 | { 66 | RGL_CHECK(rgl_configure_logging(RGL_LOG_LEVEL_WARN, nullptr, true)); 67 | if (!RGLInterface::Get()) 68 | { 69 | RGLInterface::Register(this); 70 | } 71 | } 72 | 73 | RGLSystemComponent::~RGLSystemComponent() 74 | { 75 | if (RGLInterface::Get() == this) 76 | { 77 | RGLInterface::Unregister(this); 78 | } 79 | } 80 | 81 | void RGLSystemComponent::Activate() 82 | { 83 | AzFramework::EntityContextId gameEntityContextId; 84 | AzFramework::GameEntityContextRequestBus::BroadcastResult( 85 | gameEntityContextId, &AzFramework::GameEntityContextRequestBus::Events::GetGameEntityContextId); 86 | AZ_Assert(!gameEntityContextId.IsNull(), "Invalid GameEntityContextId"); 87 | 88 | AzFramework::EntityContextEventBus::Handler::BusConnect(gameEntityContextId); 89 | LidarSystemNotificationBus::Handler::BusConnect(); 90 | 91 | m_rglLidarSystem.Activate(); 92 | } 93 | 94 | void RGLSystemComponent::Deactivate() 95 | { 96 | m_rglLidarSystem.Deactivate(); 97 | LidarSystemNotificationBus::Handler::BusDisconnect(); 98 | AzFramework::EntityContextEventBus::Handler::BusDisconnect(); 99 | 100 | m_entityManagers.clear(); 101 | m_modelLibrary.Clear(); 102 | m_rglLidarSystem.Clear(); 103 | } 104 | 105 | void RGLSystemComponent::ExcludeEntity(const AZ::EntityId& excludedEntityId) 106 | { 107 | if (!m_entityManagers.erase(excludedEntityId)) 108 | { 109 | m_excludedEntities.insert(excludedEntityId); 110 | } 111 | } 112 | 113 | void RGLSystemComponent::SetSceneConfiguration(const SceneConfiguration& config) 114 | { 115 | m_sceneConfig = config; 116 | RGLNotificationBus::Broadcast(&RGLNotifications::OnSceneConfigurationSet, config); 117 | } 118 | 119 | const SceneConfiguration& RGLSystemComponent::GetSceneConfiguration() const 120 | { 121 | return m_sceneConfig; 122 | } 123 | 124 | void RGLSystemComponent::OnEntityContextCreateEntity(AZ::Entity& entity) 125 | { 126 | if (m_excludedEntities.contains(entity.GetId())) 127 | { 128 | return; 129 | } 130 | 131 | if (m_activeLidarCount < 1U) 132 | { 133 | m_unprocessedEntities.emplace(entity.GetId()); 134 | return; 135 | } 136 | 137 | ProcessEntity(entity); 138 | } 139 | 140 | void RGLSystemComponent::OnEntityContextDestroyEntity(const AZ::EntityId& id) 141 | { 142 | m_unprocessedEntities.erase(id); 143 | m_entityManagers.erase(id); 144 | } 145 | 146 | void RGLSystemComponent::OnEntityContextReset() 147 | { 148 | m_entityManagers.clear(); 149 | m_unprocessedEntities.clear(); 150 | m_modelLibrary.Clear(); 151 | m_rglLidarSystem.Clear(); 152 | } 153 | 154 | void RGLSystemComponent::OnLidarCreated() 155 | { 156 | ++m_activeLidarCount; 157 | 158 | if (m_activeLidarCount > 1U) 159 | { 160 | return; 161 | } 162 | 163 | RGLNotificationBus::Broadcast(&RGLNotifications::OnAnyLidarExists); 164 | for (auto entityIdIt = m_unprocessedEntities.begin(); entityIdIt != m_unprocessedEntities.end();) 165 | { 166 | AZ::Entity* entity = nullptr; 167 | AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, *entityIdIt); 168 | AZ_Assert(entity, "Failed to find entity with provided id!"); 169 | ProcessEntity(*entity); 170 | entityIdIt = m_unprocessedEntities.erase(entityIdIt); 171 | } 172 | } 173 | 174 | void RGLSystemComponent::OnLidarDestroyed() 175 | { 176 | --m_activeLidarCount; 177 | 178 | if (m_activeLidarCount > 0U) 179 | { 180 | return; 181 | } 182 | 183 | RGLNotificationBus::Broadcast(&RGLNotifications::OnNoLidarExists); 184 | for (auto& m_entityManager : m_entityManagers) 185 | { 186 | m_unprocessedEntities.emplace(m_entityManager.first); 187 | } 188 | m_entityManagers.clear(); 189 | m_modelLibrary.Clear(); 190 | } 191 | 192 | void RGLSystemComponent::ProcessEntity(const AZ::Entity& entity) 193 | { 194 | AZStd::unique_ptr entityManager; 195 | if (entity.FindComponent()) 196 | { 197 | entityManager = AZStd::make_unique(entity.GetId()); 198 | } 199 | else if (entity.FindComponent(AZ::Render::MeshComponentTypeId)) 200 | { 201 | entityManager = AZStd::make_unique(entity.GetId()); 202 | } 203 | else 204 | { 205 | return; 206 | } 207 | 208 | [[maybe_unused]] bool inserted = m_entityManagers.emplace(entity.GetId(), AZStd::move(entityManager)).second; 209 | AZ_Error(__func__, inserted, "Object with provided entityId already exists."); 210 | } 211 | 212 | void RGLSystemComponent::UpdateScene() 213 | { 214 | AZ::ScriptTimePoint currentTime; 215 | AZ::TickRequestBus::BroadcastResult(currentTime, &AZ::TickRequestBus::Events::GetTimeAtCurrentTick); 216 | // Skip if already updated 217 | if (m_sceneUpdateLastTime.Get() == currentTime.Get()) 218 | { 219 | return; 220 | } 221 | m_sceneUpdateLastTime = currentTime; 222 | 223 | for (auto&& [entityId, entityManager] : m_entityManagers) 224 | { 225 | entityManager->Update(); 226 | } 227 | } 228 | } // namespace RGL 229 | -------------------------------------------------------------------------------- /Code/Source/RGLSystemComponent.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace RGL 27 | { 28 | class EntityManager; 29 | 30 | class RGLSystemComponent 31 | : public AZ::Component 32 | , protected RGLRequestBus::Handler 33 | , protected AzFramework::EntityContextEventBus::Handler 34 | , protected LidarSystemNotificationBus::Handler 35 | { 36 | public: 37 | AZ_COMPONENT(RGL::RGLSystemComponent, "{dbd5b1c5-249f-4eca-a142-2533ebe7f680}"); 38 | 39 | static void Reflect(AZ::ReflectContext* context); 40 | 41 | static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); 42 | static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible); 43 | static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); 44 | static void GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent); 45 | 46 | RGLSystemComponent(); 47 | ~RGLSystemComponent() override; 48 | 49 | protected: 50 | // AZ::Component overrides 51 | void Activate() override; 52 | void Deactivate() override; 53 | 54 | // RGLRequestBus overrides 55 | void ExcludeEntity(const AZ::EntityId& excludedEntityId) override; 56 | void SetSceneConfiguration(const SceneConfiguration& config) override; 57 | [[nodiscard]] const SceneConfiguration& GetSceneConfiguration() const override; 58 | void UpdateScene() override; 59 | 60 | // AzFramework::EntityContextEventBus overrides 61 | void OnEntityContextCreateEntity(AZ::Entity& entity) override; 62 | void OnEntityContextDestroyEntity(const AZ::EntityId& id) override; 63 | void OnEntityContextReset() override; 64 | 65 | // LidarNotificationBus overides 66 | void OnLidarCreated() override; 67 | void OnLidarDestroyed() override; 68 | 69 | private: 70 | void ProcessEntity(const AZ::Entity& entity); 71 | 72 | LidarSystem m_rglLidarSystem; 73 | 74 | ModelLibrary m_modelLibrary; 75 | AZStd::set m_excludedEntities; 76 | AZStd::set m_unprocessedEntities; 77 | SceneConfiguration m_sceneConfig; 78 | AZStd::unordered_map> m_entityManagers; 79 | AZ::ScriptTimePoint m_sceneUpdateLastTime{}; 80 | 81 | size_t m_activeLidarCount{}; 82 | }; 83 | } // namespace RGL 84 | -------------------------------------------------------------------------------- /Code/Source/SceneConfiguration.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2024, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace RGL 21 | { 22 | void TerrainIntensityConfiguration::Reflect(AZ::ReflectContext* context) 23 | { 24 | if (auto* serializeContext = azrtti_cast(context)) 25 | { 26 | serializeContext->Class() 27 | ->Version(0) 28 | ->Field("DefaultIntensity", &TerrainIntensityConfiguration::m_defaultValue) 29 | ->Field("ColorTexture", &TerrainIntensityConfiguration::m_colorImageAsset) 30 | ->Field("Tiled", &TerrainIntensityConfiguration::m_isTiled); 31 | 32 | if (auto* editContext = serializeContext->GetEditContext()) 33 | { 34 | editContext->Class("RGL Intensity Configuration", "") 35 | ->DataElement( 36 | AZ::Edit::UIHandlers::Default, 37 | &TerrainIntensityConfiguration::m_defaultValue, 38 | "Default Intensity", 39 | "Defines a default intensity value to be used when no color texture is provided. Must be in range [0, 255]." 40 | "Set to 0 by default.") 41 | ->DataElement( 42 | AZ::Edit::UIHandlers::Default, 43 | &TerrainIntensityConfiguration::m_colorImageAsset, 44 | "Color Texture", 45 | "Color texture is used to calculate intensity. This texture is not required.") 46 | ->DataElement( 47 | AZ::Edit::UIHandlers::Default, 48 | &TerrainIntensityConfiguration::m_isTiled, 49 | "Tiled", 50 | "If enabled, the provided color texture is tiled over the terrain grid. Enabled by default"); 51 | } 52 | } 53 | } 54 | 55 | void SceneConfiguration::Reflect(AZ::ReflectContext* context) 56 | { 57 | TerrainIntensityConfiguration::Reflect(context); 58 | 59 | if (auto* serializeContext = azrtti_cast(context)) 60 | { 61 | serializeContext->Class() 62 | ->Version(0) 63 | ->Field("TerrainIntensityConfig", &SceneConfiguration::m_terrainIntensityConfig) 64 | ->Field("SkinnedMeshUpdate", &SceneConfiguration::m_isSkinnedMeshUpdateEnabled); 65 | 66 | if (auto* editContext = serializeContext->GetEditContext()) 67 | { 68 | editContext->Class("RGL Scene Configuration", "") 69 | ->DataElement( 70 | AZ::Edit::UIHandlers::Default, &SceneConfiguration::m_terrainIntensityConfig, "Terrain Intensity Configuration", "") 71 | ->DataElement( 72 | AZ::Edit::UIHandlers::Default, 73 | &SceneConfiguration::m_isSkinnedMeshUpdateEnabled, 74 | "Skinned Mesh Update", 75 | "Should the Skinned Meshes be updated?"); 76 | } 77 | } 78 | } 79 | 80 | } // namespace RGL -------------------------------------------------------------------------------- /Code/Source/SceneConfigurationComponent.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2024, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | namespace RGL 22 | { 23 | void SceneConfigurationComponent::Reflect(AZ::ReflectContext* context) 24 | { 25 | SceneConfiguration::Reflect(context); 26 | 27 | if (auto* serializeContext = azrtti_cast(context)) 28 | { 29 | serializeContext->Class()->Version(0)->Field( 30 | "Config", &SceneConfigurationComponent::m_config); 31 | 32 | if (auto* editContext = serializeContext->GetEditContext()) 33 | { 34 | // clang-format off 35 | editContext->Class("RGL Scene Configuration", "") 36 | ->ClassElement(AZ::Edit::ClassElements::EditorData, "") 37 | ->Attribute(AZ::Edit::Attributes::Category, "RGL") 38 | ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZStd::vector({ AZ_CRC_CE("Level") })) 39 | ->DataElement(AZ::Edit::UIHandlers::Default, &SceneConfigurationComponent::m_config, "Config", ""); 40 | // clang-format on 41 | } 42 | } 43 | } 44 | 45 | void SceneConfigurationComponent::Activate() 46 | { 47 | m_config.m_terrainIntensityConfig.m_colorImageAsset.QueueLoad(); 48 | RGLInterface::Get()->SetSceneConfiguration(m_config); 49 | } 50 | 51 | void SceneConfigurationComponent::Deactivate() 52 | { 53 | m_config.m_terrainIntensityConfig.m_colorImageAsset.Release(); 54 | } 55 | } // namespace RGL 56 | -------------------------------------------------------------------------------- /Code/Source/SceneConfigurationComponent.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2024, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | namespace RGL 21 | { 22 | class SceneConfigurationComponent : public AZ::Component 23 | { 24 | public: 25 | AZ_COMPONENT(SceneConfigurationComponent, "{18c28e89-27f0-4230-936e-d7858b39d53f}", AZ::Component); 26 | 27 | SceneConfigurationComponent() = default; 28 | ~SceneConfigurationComponent() override = default; 29 | 30 | static void Reflect(AZ::ReflectContext* context); 31 | 32 | // clang-format off 33 | static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType &required) {} 34 | // clang-format on 35 | 36 | // AZ::Component overrides 37 | void Activate() override; 38 | void Deactivate() override; 39 | 40 | private: 41 | SceneConfiguration m_config; 42 | }; 43 | } // namespace RGL 44 | -------------------------------------------------------------------------------- /Code/Source/Utilities/RGLUtils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) Contributors to the Open 3D Engine Project. 3 | * For complete copyright and license terms please see the LICENSE at the root of this distribution. 4 | * 5 | * SPDX-License-Identifier: Apache-2.0 OR MIT 6 | * 7 | */ 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace RGL::Utils 16 | { 17 | static constexpr AZ::u8 RglEntityIdBits = 28; 18 | static constexpr AZ::u8 ClassIdBits = 8; 19 | static constexpr AZ::u8 CompressedIdBitDepth = RglEntityIdBits - ClassIdBits; 20 | 21 | int32_t PackRglEntityId(ROS2::SegmentationIds segmentationIds) 22 | { 23 | AZ_Assert(segmentationIds.m_entityId < (1 << CompressedIdBitDepth), "Entity ID is too large to be packed into RGL entity ID"); 24 | const int32_t id = (static_cast(segmentationIds.m_classId) << CompressedIdBitDepth) | 25 | (segmentationIds.m_entityId & ((1 << CompressedIdBitDepth) - 1)); 26 | 27 | return id; 28 | } 29 | 30 | ROS2::SegmentationIds UnpackRglEntityId(int32_t rglPackedEntityId) 31 | { 32 | const uint8_t classId = rglPackedEntityId >> CompressedIdBitDepth; 33 | const int32_t entityId = rglPackedEntityId & ((1 << CompressedIdBitDepth) - 1); 34 | return { entityId, classId }; 35 | } 36 | 37 | int32_t GenerateSegmentationEntityId() 38 | { 39 | static_assert(CompressedIdBitDepth <= 31, "CompressedIdBitDepth must be less than or equal to 31"); 40 | static uint32_t compressedIdCounter = 1U; 41 | 42 | const auto generatedId = aznumeric_cast(++compressedIdCounter % (1 << RGL::Utils::CompressedIdBitDepth)); 43 | 44 | return generatedId; 45 | } 46 | 47 | void ErrorCheck(const rgl_status_t& status, const char* file, int line, bool* successDest) 48 | { 49 | static const AZStd::unordered_set UnrecoverableStatuses = { 50 | RGL_INVALID_STATE, 51 | RGL_LOGGING_ERROR, 52 | RGL_INITIALIZATION_ERROR, 53 | RGL_INTERNAL_EXCEPTION, 54 | }; 55 | 56 | if (status == RGL_SUCCESS) 57 | { 58 | if (successDest) 59 | { 60 | *successDest = true; 61 | } 62 | return; 63 | } 64 | 65 | AZStd::string errorLocationString{ AZStd::string{ "in file " }.append(file).append(" at line ").append(AZStd::to_string(line)) }; 66 | const char* errorString; 67 | rgl_get_last_error_string(&errorString); 68 | if (UnrecoverableStatuses.contains(status)) 69 | { 70 | AZ_Assert( 71 | false, 72 | std::string("RGL encountered an unrecoverable error ") 73 | .append(errorLocationString.c_str()) 74 | .append(" with message: ") 75 | .append(errorString) 76 | .c_str()); 77 | } 78 | 79 | AZ_Error( 80 | __func__, 81 | false, 82 | std::string("RGL encountered an error ") 83 | .append(errorLocationString.c_str()) 84 | .append(" with message: ") 85 | .append(errorString) 86 | .c_str()); 87 | 88 | std::cout << std::string("RGL encountered an error ") 89 | .append(errorLocationString.c_str()) 90 | .append(" with message: ") 91 | .append(errorString) 92 | .c_str() 93 | << std::endl; 94 | 95 | if (successDest) 96 | { 97 | *successDest = false; 98 | } 99 | } 100 | 101 | rgl_mat3x4f RglMat3x4FromAzMatrix3x4(const AZ::Matrix3x4& azMatrix) 102 | { 103 | return { 104 | { 105 | { azMatrix.GetRow(0).GetX(), azMatrix.GetRow(0).GetY(), azMatrix.GetRow(0).GetZ(), azMatrix.GetRow(0).GetW() }, 106 | { azMatrix.GetRow(1).GetX(), azMatrix.GetRow(1).GetY(), azMatrix.GetRow(1).GetZ(), azMatrix.GetRow(1).GetW() }, 107 | { azMatrix.GetRow(2).GetX(), azMatrix.GetRow(2).GetY(), azMatrix.GetRow(2).GetZ(), azMatrix.GetRow(2).GetW() }, 108 | }, 109 | }; 110 | } 111 | 112 | AZ::Matrix3x4 AzMatrix3x4FromRglMat3x4(const rgl_mat3x4f& rglMatrix) 113 | { 114 | const float m_rowMajorValues[]{ 115 | rglMatrix.value[0][0], rglMatrix.value[0][1], rglMatrix.value[0][2], rglMatrix.value[0][3], 116 | rglMatrix.value[1][0], rglMatrix.value[1][1], rglMatrix.value[1][2], rglMatrix.value[1][3], 117 | rglMatrix.value[2][0], rglMatrix.value[2][1], rglMatrix.value[2][2], rglMatrix.value[2][3], 118 | }; 119 | return AZ::Matrix3x4::CreateFromRowMajorFloat12(m_rowMajorValues); 120 | } 121 | 122 | AZ::Vector3 AzVector3FromRglVec3f(const rgl_vec3f& rglVector) 123 | { 124 | return { rglVector.value[0], rglVector.value[1], rglVector.value[2] }; 125 | } 126 | 127 | rgl_vec3f RglVector3FromAzVec3f(const AZ::Vector3& azVector) 128 | { 129 | return { azVector.GetX(), azVector.GetY(), azVector.GetZ() }; 130 | } 131 | 132 | rgl_vec2f RglVec2fFromAzVector2(const AZ::Vector2& azVector) 133 | { 134 | return { azVector.GetX(), azVector.GetY() }; 135 | } 136 | } // namespace RGL::Utils 137 | -------------------------------------------------------------------------------- /Code/Source/Utilities/RGLUtils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) Contributors to the Open 3D Engine Project. 3 | * For complete copyright and license terms please see the LICENSE at the root of this distribution. 4 | * 5 | * SPDX-License-Identifier: Apache-2.0 OR MIT 6 | * 7 | */ 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace RGL::Utils 15 | { 16 | //! Packs an entity ID and a segmentation class ID into an 32-bit integer. 17 | //! Entity IDs must be generated using the GenerateSegmentationEntityId function. 18 | //! @see GenerateSegmentationEntityId 19 | int32_t PackRglEntityId(ROS2::SegmentationIds); 20 | 21 | //! Unpacks a packed RGL entity ID into an entity ID and a segmentation class ID. 22 | //! @see PackRglEntityId 23 | ROS2::SegmentationIds UnpackRglEntityId(int32_t rglPackedEntityId); 24 | 25 | //! Generates a new unique ID for an entity. 26 | //! This ID can then be used to generate a packed RGL entity ID. 27 | int32_t GenerateSegmentationEntityId(); 28 | 29 | //! If the provided status signifies an error, prints the last RGL error message. 30 | //! @param status Status returned by an API call. 31 | //! @param file String representing the file path of the file in which the API call was made 32 | //! @param line Line at which the API call was made 33 | //! @param line Line at which the API call was made 34 | //! @param successDest Optional parameter for the destination of boolean value representing success. 35 | //! The value is not written if the pointer is set to nullptr. 36 | void ErrorCheck(const rgl_status_t& status, const char* file, int line, bool* successDest = nullptr); 37 | 38 | //! Macro used for calling the ErrorCheck function. 39 | //! Each status returned by RGL API should be passed to it. 40 | #define RGL_CHECK(x) RGL::Utils::ErrorCheck(x, __FILE__, __LINE__) 41 | 42 | rgl_mat3x4f RglMat3x4FromAzMatrix3x4(const AZ::Matrix3x4& azMatrix); 43 | AZ::Matrix3x4 AzMatrix3x4FromRglMat3x4(const rgl_mat3x4f& rglMatrix); 44 | AZ::Vector3 AzVector3FromRglVec3f(const rgl_vec3f& rglVector); 45 | rgl_vec3f RglVector3FromAzVec3f(const AZ::Vector3& azVector); 46 | rgl_vec2f RglVec2fFromAzVector2(const AZ::Vector2& azVector); 47 | 48 | constexpr rgl_mat3x4f IdentityTransform{ 49 | .value{ 50 | { 1, 0, 0, 0 }, 51 | { 0, 1, 0, 0 }, 52 | { 0, 0, 1, 0 }, 53 | }, 54 | }; 55 | } // namespace RGL::Utils -------------------------------------------------------------------------------- /Code/Source/Wrappers/RglEntity.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2024, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace RGL::Wrappers 21 | { 22 | RglEntity::RglEntity(const RglMesh& mesh) 23 | { 24 | bool success = false; 25 | Utils::ErrorCheck(rgl_entity_create(&m_nativePtr, nullptr, mesh.m_nativePtr), __FILE__, __LINE__, &success); 26 | if (!success && m_nativePtr) 27 | { 28 | RGL_CHECK(rgl_entity_destroy(m_nativePtr)); 29 | m_nativePtr = nullptr; 30 | } 31 | } 32 | 33 | RglEntity::RglEntity(RglEntity&& other) 34 | { 35 | if (IsValid()) 36 | { 37 | RGL_CHECK(rgl_entity_destroy(m_nativePtr)); 38 | } 39 | 40 | m_nativePtr = other.m_nativePtr; 41 | other.m_nativePtr = nullptr; 42 | } 43 | 44 | RglEntity::~RglEntity() 45 | { 46 | if (IsValid()) 47 | { 48 | RGL_CHECK(rgl_entity_destroy(m_nativePtr)); 49 | } 50 | } 51 | 52 | void RglEntity::SetTransform(const rgl_mat3x4f& pose) 53 | { 54 | AZ_Assert(IsValid(), "Tried to set pose of an invalid entity."); 55 | RGL_CHECK(rgl_entity_set_transform(m_nativePtr, &pose)); 56 | } 57 | 58 | void RglEntity::SetId(int32_t id) 59 | { 60 | AZ_Assert(IsValid(), "Tried to set id of an invalid entity."); 61 | RGL_CHECK(rgl_entity_set_id(m_nativePtr, id)); 62 | } 63 | 64 | void RglEntity::SetIntensityTexture(const RglTexture& texture) 65 | { 66 | AZ_Assert(IsValid(), "Tried to set intensity texture of an invalid entity."); 67 | RGL_CHECK(rgl_entity_set_intensity_texture(m_nativePtr, texture.m_nativePtr)); 68 | } 69 | 70 | void RglEntity::ApplyExternalAnimation(const rgl_vec3f* vertices, size_t vertexCount) 71 | { 72 | AZ_Assert(IsValid(), "Tried to set intensity texture of an invalid entity."); 73 | RGL_CHECK(rgl_entity_apply_external_animation(m_nativePtr, vertices, aznumeric_cast(vertexCount))); 74 | } 75 | 76 | RglEntity& RglEntity::operator=(RglEntity&& other) 77 | { 78 | if (this != &other) 79 | { 80 | if (IsValid()) 81 | { 82 | RGL_CHECK(rgl_entity_destroy(m_nativePtr)); 83 | } 84 | 85 | m_nativePtr = other.m_nativePtr; 86 | other.m_nativePtr = nullptr; 87 | } 88 | 89 | return *this; 90 | } 91 | } // namespace RGL::Wrappers 92 | -------------------------------------------------------------------------------- /Code/Source/Wrappers/RglEntity.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2024, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | 19 | namespace RGL::Wrappers 20 | { 21 | class RglMesh; 22 | class RglTexture; 23 | 24 | class RglEntity 25 | { 26 | public: 27 | static RglEntity CreateInvalid() 28 | { 29 | return {}; 30 | } 31 | 32 | explicit RglEntity(const RglMesh& mesh); 33 | RglEntity(const RglEntity& other) = delete; 34 | RglEntity(RglEntity&& other); 35 | ~RglEntity(); 36 | 37 | [[nodiscard]] bool IsValid() const 38 | { 39 | return m_nativePtr; 40 | } 41 | 42 | void SetTransform(const rgl_mat3x4f& pose); 43 | void SetId(int32_t id); 44 | void SetIntensityTexture(const RglTexture& texture); 45 | void ApplyExternalAnimation(const rgl_vec3f* vertices, size_t vertexCount); 46 | 47 | RglEntity& operator=(const RglEntity& other) = delete; 48 | RglEntity& operator=(RglEntity&& other); 49 | 50 | private: 51 | //! Creates an invalid entity. 52 | //! To avoid creating an invalid entity by accident, it is private. 53 | //! See CreateInvalid. 54 | RglEntity() = default; 55 | 56 | rgl_entity_t m_nativePtr{ nullptr }; 57 | }; 58 | } // namespace RGL::Wrappers 59 | -------------------------------------------------------------------------------- /Code/Source/Wrappers/RglMesh.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2024, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace RGL::Wrappers 21 | { 22 | RglMesh::RglMesh(const rgl_vec3f* vertices, size_t vertexCount, const rgl_vec3i* indices, size_t indexCount) 23 | { 24 | bool success = false; 25 | Utils::ErrorCheck( 26 | rgl_mesh_create(&m_nativePtr, vertices, aznumeric_cast(vertexCount), indices, aznumeric_cast(indexCount)), 27 | __FILE__, 28 | __LINE__, 29 | &success); 30 | 31 | if (!success && m_nativePtr) 32 | { 33 | RGL_CHECK(rgl_mesh_destroy(m_nativePtr)); 34 | m_nativePtr = nullptr; 35 | } 36 | } 37 | 38 | RglMesh::RglMesh(RglMesh&& other) 39 | { 40 | if (IsValid()) 41 | { 42 | RGL_CHECK(rgl_mesh_destroy(m_nativePtr)); 43 | } 44 | 45 | m_nativePtr = other.m_nativePtr; 46 | other.m_nativePtr = nullptr; 47 | } 48 | 49 | RglMesh::~RglMesh() 50 | { 51 | if (IsValid()) 52 | { 53 | RGL_CHECK(rgl_mesh_destroy(m_nativePtr)); 54 | m_nativePtr = nullptr; 55 | } 56 | } 57 | 58 | void RglMesh::SetTextureCoordinates(const rgl_vec2f* uvs, size_t uvCount) 59 | { 60 | AZ_Assert(IsValid(), "Tried to set texture coordinates of an invalid mesh."); 61 | RGL_CHECK(rgl_mesh_set_texture_coords(m_nativePtr, uvs, aznumeric_cast(uvCount))); 62 | } 63 | 64 | RglMesh& RglMesh::operator=(RglMesh&& other) 65 | { 66 | if (this != &other) 67 | { 68 | if (IsValid()) 69 | { 70 | RGL_CHECK(rgl_mesh_destroy(m_nativePtr)); 71 | } 72 | 73 | m_nativePtr = other.m_nativePtr; 74 | other.m_nativePtr = nullptr; 75 | } 76 | 77 | return *this; 78 | } 79 | } // namespace RGL::Wrappers 80 | -------------------------------------------------------------------------------- /Code/Source/Wrappers/RglMesh.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2024, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | namespace RGL::Wrappers 21 | { 22 | class RglEntity; 23 | 24 | class RglMesh 25 | { 26 | friend class RglEntity; 27 | 28 | public: 29 | static RglMesh CreateInvalid() 30 | { 31 | return {}; 32 | } 33 | 34 | RglMesh(const rgl_vec3f* vertices, size_t vertexCount, const rgl_vec3i* indices, size_t indexCount); 35 | RglMesh(const RglMesh& other) = delete; 36 | RglMesh(RglMesh&& other); 37 | ~RglMesh(); 38 | 39 | [[nodiscard]] bool IsValid() const 40 | { 41 | return m_nativePtr; 42 | } 43 | 44 | void SetTextureCoordinates(const rgl_vec2f* uvs, size_t uvCount); 45 | 46 | RglMesh& operator=(const RglMesh& other) = delete; 47 | RglMesh& operator=(RglMesh&& other); 48 | 49 | private: 50 | //! Creates an invalid mesh. 51 | //! To avoid creating an invalid mesh by accident, it is private. 52 | //! See CreateInvalid. 53 | RglMesh() = default; 54 | 55 | rgl_mesh_t m_nativePtr{ nullptr }; 56 | }; 57 | } // namespace RGL::Wrappers 58 | -------------------------------------------------------------------------------- /Code/Source/Wrappers/RglTexture.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2024, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace RGL::Wrappers 21 | { 22 | RglTexture RglTexture::CreateFromMaterialAsset(const AZ::Data::Asset& materialAsset) 23 | { 24 | static const AZStd::string TraceWindowName = ConstructTraceWindowName(__func__); 25 | static const AZ::Name albedoTexName = AZ::Name::FromStringLiteral("baseColor.textureMap", AZ::Interface::Get()); 26 | static const AZ::Name albedoColorName = AZ::Name::FromStringLiteral("baseColor.color", AZ::Interface::Get()); 27 | 28 | RglTexture imageRglTexture = CreateInvalid(); 29 | 30 | [[maybe_unused]] const AZ::Data::AssetId& id = materialAsset.GetId(); 31 | if (!materialAsset.IsReady()) 32 | { 33 | AZ_Warning( 34 | TraceWindowName.c_str(), false, "The material asset with ID: %s was not ready.", id.ToString().c_str()); 35 | return imageRglTexture; 36 | } 37 | 38 | const auto* propLayout = materialAsset->GetMaterialPropertiesLayout(); 39 | 40 | if (!propLayout) 41 | { 42 | AZ_Warning( 43 | TraceWindowName.c_str(), 44 | false, 45 | "Unable to access material properties layout of material asset with ID: %s.", 46 | id.ToString().c_str()); 47 | return imageRglTexture; 48 | } 49 | 50 | auto& propertyValues = materialAsset->GetPropertyValues(); 51 | 52 | if (const auto albedoTexPropIdx = propLayout->FindPropertyIndex(albedoTexName); !albedoTexPropIdx.IsNull()) 53 | { 54 | const auto albedoTexImagePropVal = propertyValues.at(albedoTexPropIdx.GetIndex()); 55 | const auto& imageAsset = albedoTexImagePropVal.GetValue>(); 56 | imageRglTexture = CreateFromImageAsset(imageAsset.GetId()); 57 | } 58 | 59 | if (imageRglTexture.IsValid()) 60 | { 61 | return imageRglTexture; 62 | } 63 | 64 | if (const auto albedoColorPropIdx = propLayout->FindPropertyIndex(albedoColorName); !albedoColorPropIdx.IsNull()) 65 | { 66 | imageRglTexture = 67 | AZStd::move(CreateFromFactor(CreateGrayFromColor(propertyValues.at(albedoColorPropIdx.GetIndex()).GetValue()))); 68 | } 69 | else 70 | { 71 | AZ_Error( 72 | TraceWindowName.c_str(), 73 | false, 74 | "Unable to find specular color and texture properties of material asset with ID: %s.", 75 | id.ToString().c_str()); 76 | } 77 | 78 | return imageRglTexture; 79 | } 80 | 81 | RglTexture RglTexture::CreateFromFactor(float factor) 82 | { 83 | auto factor8 = aznumeric_cast(factor * 255.0f); 84 | return RglTexture{ &factor8, 1, 1 }; 85 | } 86 | 87 | float RglTexture::CreateGrayFromColor(const AZ::Color& color) 88 | { 89 | return RedGrayMultiplier * color.GetR() + GreenGrayMultiplier * color.GetG() + BlueGrayMultiplier * color.GetB(); 90 | } 91 | 92 | uint8_t RglTexture::CreateGray8FromColor(const AZ::Color& color) 93 | { 94 | return static_cast(CreateGrayFromColor(color) * 255.0f); 95 | } 96 | 97 | RglTexture RglTexture::CreateFromImageAsset(const AZ::Data::AssetId& imageAssetId) 98 | { 99 | // Made a static member to minimize reallocations, since multiple assets will be processed. 100 | static AZStd::vector tempRglTextureData; 101 | 102 | RglTexture imageRglTexture = CreateInvalid(); 103 | 104 | if (!imageAssetId.IsValid()) 105 | { 106 | return imageRglTexture; 107 | } 108 | 109 | AZ::Data::Asset imageAsset = 110 | AZ::Data::AssetManager::Instance().GetAsset( 111 | imageAssetId, 112 | AZ::Data::AssetLoadBehavior::PreLoad, 113 | AZ::Data::AssetLoadParameters(nullptr, AZ::Data::AssetDependencyLoadRules::LoadAll)); 114 | imageAsset.QueueLoad(); 115 | imageAsset.BlockUntilLoadComplete(); 116 | 117 | const AZ::RHI::ImageDescriptor imageDescriptor = imageAsset->GetImageDescriptor(); 118 | 119 | using Format = AZ::RHI::Format; 120 | const auto format = imageDescriptor.m_format; 121 | const auto& size = imageDescriptor.m_size; 122 | if (format == Format::BC1_UNORM || format == Format::BC1_UNORM_SRGB || format == Format::BC4_UNORM) 123 | { 124 | tempRglTextureData.resize(size.m_width * size.m_height); 125 | 126 | // Only highest detail mip. 127 | AZStd::span imageData = imageAsset->GetSubImageData(0, 0); 128 | size_t srcIdx = 0, sliceIdx = 0; 129 | for (size_t y = 0; y < size.m_height; y += 4) 130 | { 131 | for (size_t x = 0; x < size.m_width; x += 4) 132 | { 133 | if (format == Format::BC4_UNORM) 134 | { 135 | LoadBlockToGrays( 136 | reinterpret_cast(imageData.data() + srcIdx), tempRglTextureData, x, y, size.m_width); 137 | } 138 | else 139 | { 140 | LoadBlockToGrays( 141 | reinterpret_cast(imageData.data() + srcIdx), tempRglTextureData, x, y, size.m_width); 142 | } 143 | 144 | srcIdx += 8; 145 | if (srcIdx == imageData.size() && ++sliceIdx < imageDescriptor.m_arraySize) 146 | { 147 | imageData = imageAsset->GetSubImageData(0, sliceIdx); 148 | srcIdx = 0; 149 | } 150 | } 151 | } 152 | 153 | imageRglTexture = RglTexture{ tempRglTextureData.data(), size.m_width, size.m_height }; 154 | } 155 | else 156 | { 157 | AZ_Warning( 158 | ConstructTraceWindowName(__func__).c_str(), 159 | false, 160 | "Image is of unsupported type: %s. Only BC1 and BC4 formats are currently supported. Skipping...", 161 | ToString(imageDescriptor.m_format)); 162 | } 163 | 164 | return imageRglTexture; 165 | } 166 | 167 | RglTexture::RglTexture(const uint8_t* texels, size_t width, size_t height) 168 | { 169 | bool success = false; 170 | Utils::ErrorCheck( 171 | rgl_texture_create(&m_nativePtr, texels, aznumeric_cast(width), aznumeric_cast(height)), 172 | __FILE__, 173 | __LINE__, 174 | &success); 175 | 176 | if (!success && m_nativePtr) 177 | { 178 | RGL_CHECK(rgl_texture_destroy(m_nativePtr)); 179 | m_nativePtr = nullptr; 180 | } 181 | } 182 | 183 | RglTexture::RglTexture(RglTexture&& other) 184 | { 185 | if (IsValid()) 186 | { 187 | RGL_CHECK(rgl_texture_destroy(m_nativePtr)); 188 | } 189 | 190 | m_nativePtr = other.m_nativePtr; 191 | other.m_nativePtr = nullptr; 192 | } 193 | 194 | RglTexture::~RglTexture() 195 | { 196 | if (IsValid()) 197 | { 198 | RGL_CHECK(rgl_texture_destroy(m_nativePtr)); 199 | } 200 | } 201 | 202 | RglTexture& RglTexture::operator=(RglTexture&& other) 203 | { 204 | if (this != &other) 205 | { 206 | if (IsValid()) 207 | { 208 | RGL_CHECK(rgl_texture_destroy(m_nativePtr)); 209 | } 210 | 211 | m_nativePtr = other.m_nativePtr; 212 | other.m_nativePtr = nullptr; 213 | } 214 | 215 | return *this; 216 | } 217 | } // namespace RGL::Wrappers 218 | -------------------------------------------------------------------------------- /Code/Source/Wrappers/RglTexture.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2024, Robotec.ai sp. z o.o. 2 | * 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace RGL::Wrappers 23 | { 24 | class RglEntity; 25 | 26 | class RglTexture 27 | { 28 | friend class RglEntity; 29 | 30 | public: 31 | static RglTexture CreateInvalid() 32 | { 33 | return {}; 34 | } 35 | 36 | //! This function is not thread safe. 37 | static RglTexture CreateFromMaterialAsset(const AZ::Data::Asset& materialAsset); 38 | static RglTexture CreateFromFactor(float factor); 39 | //! This function is not thread safe. Currently, only images in the BC1 format are supported. 40 | static RglTexture CreateFromImageAsset(const AZ::Data::AssetId& imageAssetId); 41 | 42 | RglTexture(const uint8_t* texels, size_t width, size_t height); 43 | RglTexture(const RglTexture& other) = delete; 44 | RglTexture(RglTexture&& other); 45 | ~RglTexture(); 46 | 47 | [[nodiscard]] bool IsValid() const 48 | { 49 | return m_nativePtr; 50 | } 51 | 52 | RglTexture& operator=(const RglTexture& other) = delete; 53 | RglTexture& operator=(RglTexture&& other); 54 | 55 | private: 56 | //! Creates an invalid texture. 57 | //! To avoid creating an invalid texture by accident, it is private. 58 | //! See CreateInvalid. 59 | RglTexture() = default; 60 | 61 | static AZStd::string ConstructTraceWindowName(const char* functionName) 62 | { 63 | return AZStd::string("RGL::RglTexture::") + functionName; 64 | } 65 | 66 | static float CreateGrayFromColor(const AZ::Color& color); 67 | static uint8_t CreateGray8FromColor(const AZ::Color& color); 68 | 69 | template 70 | static void LoadBlockToGrays(const BlockT* block, AZStd::vector& grayValues, size_t blockX, size_t blockY, size_t width) 71 | { 72 | size_t i = 0; 73 | for (size_t y = blockY; y < blockY + 4; ++y) 74 | { 75 | for (size_t x = blockX; x < blockX + 4; ++x) 76 | { 77 | grayValues[x + y * width] = CreateGray8FromColor(block->GetBlockColor(i)); 78 | ++i; 79 | } 80 | } 81 | } 82 | 83 | // Weights used to convert RGB to Grayscale using the luminosity 84 | // method as opposed to taking an average. 85 | static constexpr float RedGrayMultiplier = 0.299f; 86 | static constexpr float GreenGrayMultiplier = 0.587f; 87 | static constexpr float BlueGrayMultiplier = 0.114f; 88 | 89 | rgl_texture_t m_nativePtr{ nullptr }; 90 | }; 91 | } // namespace RGL::Wrappers 92 | -------------------------------------------------------------------------------- /Code/rgl_editor_files.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | set(FILES 15 | Source/Entity/Terrain/TerrainEntityManagerEditorSystemComponent.cpp 16 | Source/Entity/Terrain/TerrainEntityManagerEditorSystemComponent.h 17 | Source/RGLEditorSystemComponent.cpp 18 | Source/RGLEditorSystemComponent.h 19 | ) 20 | -------------------------------------------------------------------------------- /Code/rgl_editor_shared_files.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | set(FILES 15 | Source/RGLEditorModule.cpp 16 | ) 17 | -------------------------------------------------------------------------------- /Code/rgl_files.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | set(FILES 15 | Source/Entity/ActorEntityManager.cpp 16 | Source/Entity/ActorEntityManager.h 17 | Source/Entity/MeshEntityManager.cpp 18 | Source/Entity/MeshEntityManager.h 19 | Source/Entity/EntityManager.cpp 20 | Source/Entity/EntityManager.h 21 | Source/Entity/MaterialEntityManager.cpp 22 | Source/Entity/MaterialEntityManager.h 23 | Source/Entity/Terrain/TerrainData.cpp 24 | Source/Entity/Terrain/TerrainData.h 25 | Source/Entity/Terrain/TerrainEntityManagerSystemComponent.cpp 26 | Source/Entity/Terrain/TerrainEntityManagerSystemComponent.h 27 | Source/Lidar/LidarRaycaster.cpp 28 | Source/Lidar/LidarRaycaster.h 29 | Source/Lidar/LidarSystem.cpp 30 | Source/Lidar/LidarSystem.h 31 | Source/Lidar/LidarSystemNotificationBus.h 32 | Source/Lidar/PipelineGraph.cpp 33 | Source/Lidar/PipelineGraph.h 34 | Source/Model/ModelLibraryBus.h 35 | Source/Model/ModelLibrary.cpp 36 | Source/Model/ModelLibrary.h 37 | Source/RGLSystemComponent.cpp 38 | Source/RGLSystemComponent.h 39 | Source/Utilities/RGLUtils.cpp 40 | Source/Utilities/RGLUtils.h 41 | Source/Wrappers/RglEntity.cpp 42 | Source/Wrappers/RglEntity.h 43 | Source/Wrappers/RglMesh.cpp 44 | Source/Wrappers/RglMesh.h 45 | Source/Wrappers/RglTexture.cpp 46 | Source/Wrappers/RglTexture.h 47 | Source/SceneConfiguration.cpp 48 | Source/SceneConfigurationComponent.cpp 49 | Source/SceneConfigurationComponent.h 50 | ) 51 | -------------------------------------------------------------------------------- /Code/rgl_header_files.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | set(FILES 15 | Include/RGL/RGLBus.h 16 | Include/RGL/SceneConfiguration.h 17 | ) 18 | -------------------------------------------------------------------------------- /Code/rgl_shared_files.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2020-2021, Robotec.ai sp. z o.o. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | set(FILES 15 | Source/RGLModule.cpp 16 | ) 17 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Robotec GPU Lidar (RGL) Gem for Open 3D Engine (O3DE) 2 | 3 | This project enables GPU-accelerated LIDAR simulation for robotics. 4 | 5 | ## A bit of context 6 | 7 | * [Open 3D Engine](https:://o3de.org) - an open source game & simulation engine. O3DE is extendable through modules 8 | called Gems. This is one of such Gems. 9 | * [LIDAR](https://en.wikipedia.org/wiki/Lidar) - a type of active sensor used in robotics, emitting and receiving back 10 | laser beams. 11 | * [Robot Operating System (ROS)](https://docs.ros.org/en/rolling/index.html) - an open source middleware and de facto 12 | standard for robotics. 13 | * [ROS2 Gem](https://github.com/o3de/o3de-extras/tree/development/Gems/ROS2) - an open source module for O3DE which 14 | enables simulation for robotics using modern ROS. 15 | * [Robotec GPU Lidar](https://github.com/RobotecAI/RobotecGPULidar) - an open source library for simulating LIDAR 16 | through raycasting based on GPU with CUDA and OptiX. 17 | 18 | ## Description 19 | 20 | The O3DE Robotec GPU Lidar Gem is a module working 21 | with [O3DE ROS2 Gem](https://github.com/o3de/o3de-extras/tree/development/Gems/ROS2) and extending it with a fast and 22 | scalable LiDAR implementation by making use of the [Robotec GPU Lidar](https://github.com/RobotecAI/RobotecGPULidar) 23 | library. 24 | 25 | ## Features 26 | 27 | Combined with the O3DE ROS2 Gem's `Lidar Sensor Component` the O3DE RGL Gem allows for creation of a configurable, 28 | high - performance LiDAR. The Gem provides a faithful representation of the simulated environment by supporting the 29 | following visuals: 30 | 31 | - Mesh Component 32 | - Terrain created using the O3DE Terrain Gem 33 | - Actor Component 34 | 35 | and providing the following features: 36 | 37 | - Gaussian Noise 38 | - Point cloud intensity data 39 | 40 | drawing 41 | 42 | You can fully customize the LiDAR's settings using the O3DE Level Editor. Those include properties like: 43 | 44 | - configurable raycasting pattern 45 | - lidar range 46 | - entities excluded from raycasting 47 | - lidar noise 48 | 49 | You can also choose one of the presets provided by the ROS2 Gem to create a LiDAR model that fits your needs. 50 | 51 | drawing 52 | 53 | ## Requirements 54 | 55 | - [ 56 | **Runtime requirements** of the Robotec GPU Lidar. 57 | ](https://github.com/RobotecAI/RobotecGPULidar#runtime-requirements) 58 | - Any O3DE project with the [O3DE ROS2 Gem](https://github.com/o3de/o3de-extras/tree/development/Gems/ROS2) enabled. 59 | - The following ROS2 packages installed on your system: 60 | - `cyclonedds`, 61 | - `fastrtps`, 62 | - `radar-msgs`. 63 | 64 | You can install those packages with the following commands: 65 | ```bash 66 | sudo apt install -y ros-${ROS_DISTRO}-cyclonedds ros-${ROS_DISTRO}-rmw-cyclonedds-cpp 67 | sudo apt install -y ros-${ROS_DISTRO}-fastrtps ros-${ROS_DISTRO}-rmw-fastrtps-cpp 68 | sudo apt install -y ros-${ROS_DISTRO}-radar-msgs 69 | ``` 70 | 71 | ***IMPORTANT:*** *You do not need to download or set up the RobotecGPULidar library itself and only have to meet 72 | the **RUNTIME** requirements.* 73 | 74 | ## Setup 75 | 76 | 1. **Clone the Gem's repository.** 77 | ```bash 78 | git clone https://github.com/RobotecAI/o3de-rgl-gem.git 79 | ``` 80 | 2. **Switch to the correct branch.** \ 81 | Depending on which version of O3DE and ROS2 gem you use this step may differ. 82 | Below is a table of supported build combinations: 83 | 84 | | o3de-rgl-gem | [o3de/o3de-extras](https://github.com/o3de/o3de-extras) | [Robotecai/o3de-extras](https://github.com/robotecai/o3de-extras) | [o3de](https://github.com/o3de/o3de) | 85 | | -------------------------- | ------------------------------------------------------- | ----------------------------------------------------------------- | ------------------------------------ | 86 | | development branch | development branch | N/A | development branch | 87 | | development branch | N/A | rgl/stable-dev branch | Release 2310.3 | 88 | | main branch, O3DE_2409 tag | Release 2409.x | N/A | Release 2409.x | 89 | | main branch, O3DE_2310 tag | Release 2310.x | N/A | Release 2310.x | 90 | 91 | **_Note:_** _This table describes build combinations that are guaranteed to work. There may exist other working build 92 | combinations._ 93 | 94 | 3. **Register the Gem.** \ 95 | You can either register the gem through the Command Line Interface or the O3DE Project Manager: 96 | - **CLI** \ 97 | Head to your local O3DE engine directory (*o3de-dir*) and register the gem using its path (*gem-path*). 98 | ```bash 99 | cd 100 | ./scripts/o3de.sh register --gem-path 101 | ``` 102 | - **Project Manager** \ 103 | Open the Project Manager. Select **Gems -> Add Existing Gem**. Locate the gem's directory and select **Choose**. 104 | 105 | 4. **Enable the Gem in your project.** \ 106 | Once again you can either enable it through the Command Line Interface or the O3DE Project Manager: 107 | 108 | ***Note:*** *Please, make sure to enable the ROS2 Gem first.* 109 | 110 | - **CLI** \ 111 | In your local o3de engine directory you can enable the gem for your project (*project-path*). 112 | ```bash 113 | ./scripts/o3de.sh enable-gem -gn RGL -pp 114 | ``` 115 | - **Project Manager** \ 116 | Open your project. Select **File -> Edit Project Settings -> Configure Gems**. Now, search for the Robotec GPU 117 | Lidar Gem and enable it. 118 | 119 | ## Usage 120 | 121 | ### Enabling and configuring the RobotecGPULidar implementation 122 | 123 | 1. **Create an entity with a `ROS2 Lidar Sensor` component.** 124 | 125 | Within your O3DE project add a new entity by right - clicking on the viewport and selecting **Create entity**. 126 | 127 | drawing 128 | 129 | Select the newly created entity within the Entity Outliner. Next, within the Entity Inspector select 130 | **Add Component**. 131 | Then, search for `ROS2 Lidar Sensor` and add it to your entity using the left mouse button. 132 | 133 | drawing 134 | 135 | ***Note:** You need to add the required `ROS2 Frame` component as well.* 136 | 137 | 2. **Select `RobotecGPULidar` as your LiDAR implementation.** 138 | 139 | In the Entity Inspector find the `ROS2 Lidar Sensor` component and change the **Lidar Implementation** 140 | to `RobotecGPULidar`. 141 | 142 | drawing 143 | 144 | ***Note:** If you do not see the `RobotecGPULidar` implementation, please make sure you followed the **Setup** 145 | instructions correctly.* 146 | 3. **Customize your LiDAR.** 147 | 148 | After following through all previous instructions, you can customize the `ROS2 Lidar Sensor` component in the Entity 149 | Inspector to fit all your needs. 150 | 151 | ### Scene configuration 152 | 153 | The RGL gem allows for global scene configuration. To achieve this: 154 | 155 | 1. **Add the ``RGL Scene Configuration`` component to the ``Level`` entity.** 156 | 157 | The Level entity is the root entity, and it can be found in the uppermost section of the Entity Outliner. 158 | In the Entity Inspector select **Add Component** . Then search for the ``RGL Scene Configuration`` component and add 159 | it to the entity. 160 | 2. **Customize the scene configurations.** 161 | 162 | In the Entity Outliner, under the ``RGL Scene configuration`` component parameters, 163 | you can customize the global scene configuration to fit your needs. 164 | 165 | ## Troubleshooting 166 | 167 | ### Issues related to the `libRobotecGPULidar.so` file 168 | 169 | If you encounter any issues relating the `libRobotecGPULidar.so` file please follow these steps: 170 | 171 | 1. Make sure no files named `RGL_DOWNLOAD_IN_PROGRESS` exist inside the build directory of your project. This file prevents the Gem from downloading new binaries and may not be deleted if the O3DE project configuration was unexpectedly stopped. 172 | 2. If this didn't help, try removing the `RGL_VERSION_METADATA` and `ROS_DISTRO_METADATA` metadata files (also located under the build path). This will force the RGL gem to download required native RGL files. 173 | 174 | For more details on how the RGL gem handles downloads of the native RGL library binaries and API source code, please refer to the [FindRGL.cmake](Code/FindRGL.cmake) file. 175 | 176 | ### Issues related to the in-game lidar behaviour 177 | 178 | One common issue is when the lidar detects unwanted geometry as shown below. 179 | 180 | drawing 181 | 182 | You can fix this with the following steps: 183 | 184 | 1. Locate the `Excluded Entities` section in the `ROS2 Lidar Sensor` component properties. 185 | 186 | drawing 187 | 188 | 2. Add a new excluded entity. You can select it in the Viewport or in the Entity Outliner. 189 | 190 | drawing 191 | 192 | The entity's name should appear in the Excluded Entities list (as seen on the image below). 193 | 194 | drawing 195 | 196 | ### Other issues 197 | 198 | If this section does not seem to help, feel free to post an issue on the gem's github 199 | repository in which you describe the problem you are facing. 200 | -------------------------------------------------------------------------------- /Registry/assetprocessor_settings.setreg: -------------------------------------------------------------------------------- 1 | { 2 | "Amazon": { 3 | "AssetProcessor": { 4 | "Settings": { 5 | "ScanFolder RGL/Assets": { 6 | "watch": "@GEMROOT:RGL@/Assets", 7 | "recursive": 1, 8 | "order": 101 9 | }, 10 | "ScanFolder RGL/Registry": { 11 | "watch": "@GEMROOT:RGL@/Registry", 12 | "recursive": 1, 13 | "order": 102 14 | } 15 | } 16 | } 17 | } 18 | } 19 | -------------------------------------------------------------------------------- /gem.json: -------------------------------------------------------------------------------- 1 | { 2 | "gem_name": "RGL", 3 | "version": "1.2.0", 4 | "platforms": [ 5 | "Linux" 6 | ], 7 | "display_name": "RobotecGPULidar", 8 | "license": "Apache-2.0", 9 | "license_url": "https://opensource.org/licenses/Apache-2.0", 10 | "origin": "RobotecAI", 11 | "origin_url": "", 12 | "type": "Code", 13 | "summary": "Extension to the ROS2 Gem that provides it with the RobotecGPULidar lidar implementation.", 14 | "canonical_tags": [ 15 | "Gem" 16 | ], 17 | "user_tags": [ 18 | "RobotecGPULidar", 19 | "ROS2" 20 | ], 21 | "icon_path": "preview.png", 22 | "requirements": "Requires a CUDA-capable GPU with the nvidia drivers specified in the README.md file of the RGL gem github repository.", 23 | "documentation_url": "", 24 | "dependencies": [ 25 | "ROS2>=3.2.0", 26 | "LmbrCentral" 27 | ], 28 | "restricted": "RGL" 29 | } 30 | -------------------------------------------------------------------------------- /preview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobotecAI/o3de-rgl-gem/07442270bb707926f859eab298084021016a39dc/preview.png -------------------------------------------------------------------------------- /static/PipelineGraph.mmd: -------------------------------------------------------------------------------- 1 | flowchart TD 2 | RP[Ray Poses] --> RR[Ray Ranges] 3 | RR --> LT[Lidar Transform] 4 | LT -->|Noise enabled| AN[Angular Noise] 5 | LT -->|Noise disabled| RT[Ray Trace] 6 | AN --> RT 7 | RT -->|Noise enabled| DN[Distance Noise] 8 | RT -->|Noise disabled| DNY[Yield Node] 9 | DN --> DNY 10 | DNY -->|Compact enabled| PC[Points Compact] 11 | DNY -->|Compact disabled| PCY[Yield Node] 12 | PC --> PCY 13 | PCY --> PY[Points Yield] 14 | PCY -->|Publishing enabled| PT[Points Transform] 15 | PT --> PF2[Points Format] 16 | PF2 --> PCP[Point Cloud Publish] 17 | -------------------------------------------------------------------------------- /static/gif/rgl_gem_preview1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobotecAI/o3de-rgl-gem/07442270bb707926f859eab298084021016a39dc/static/gif/rgl_gem_preview1.gif -------------------------------------------------------------------------------- /static/gif/rgl_gem_preview2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobotecAI/o3de-rgl-gem/07442270bb707926f859eab298084021016a39dc/static/gif/rgl_gem_preview2.gif -------------------------------------------------------------------------------- /static/png/excluded_entities1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobotecAI/o3de-rgl-gem/07442270bb707926f859eab298084021016a39dc/static/png/excluded_entities1.png -------------------------------------------------------------------------------- /static/png/excluded_entities2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobotecAI/o3de-rgl-gem/07442270bb707926f859eab298084021016a39dc/static/png/excluded_entities2.png -------------------------------------------------------------------------------- /static/png/excluded_entities3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobotecAI/o3de-rgl-gem/07442270bb707926f859eab298084021016a39dc/static/png/excluded_entities3.png -------------------------------------------------------------------------------- /static/png/lidar_sphere.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobotecAI/o3de-rgl-gem/07442270bb707926f859eab298084021016a39dc/static/png/lidar_sphere.png -------------------------------------------------------------------------------- /static/png/usage_instruction1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobotecAI/o3de-rgl-gem/07442270bb707926f859eab298084021016a39dc/static/png/usage_instruction1.png -------------------------------------------------------------------------------- /static/png/usage_instruction2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobotecAI/o3de-rgl-gem/07442270bb707926f859eab298084021016a39dc/static/png/usage_instruction2.png -------------------------------------------------------------------------------- /static/png/usage_instruction3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobotecAI/o3de-rgl-gem/07442270bb707926f859eab298084021016a39dc/static/png/usage_instruction3.png --------------------------------------------------------------------------------